Diff: main.cpp

Revision:

152:6877dceec871

Parent:

150:08c13bfc7417

Child:

153:8a1f9888f003

Child:

154:0a22dcf91577

--- a/main.cpp	Thu May 04 14:34:34 2017 +0000
+++ b/main.cpp	Thu May 04 15:16:56 2017 +0000
@@ -34,7 +34,6 @@
 
 int loop_counter = 0;
 bool control_enabled = false;
-float throttle_median, w_median;
 
 void update_velocity() {
     read.last_p_mech = read.p_mech;
@@ -45,11 +44,8 @@
     if (dp_mech >  PI) dp_mech -= 2 * PI;
     
     float w_raw = dp_mech * F_SW; //rad/s
-    if (w_raw > W_CRAZY) w_raw = read.w; //throw out inplausible results
-    if (w_raw < -W_CRAZY) w_raw = read.w; 
     
-    w_median = velocity_filter->update(w_raw);
-    read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
+    read.w = velocity_filter->update(w_raw);
 }
 
 void commutate() {  
@@ -138,9 +134,8 @@
 
 void slow_loop() {
     float x = io.throttle_in->get_throttle();
-    throttle_median = throttle_filter->update(x);
-    control.torque_percent = update_filter(control.torque_percent, th->map(io.throttle_in->get_throttle(), read.w), THROTTLE_FILTER_STRENGTH);
-    
+    x = throttle_filter->update(x);
+    control.torque_percent = th->map(x, read.w);
     dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
 }
 
@@ -167,7 +162,6 @@
 void log() {
     //io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), 
     //                    (int) (255 * foc.vd), (int) (255 * foc.vq));
-    io.pc->printf("%d,%d\n", (int)(255 * control.torque_percent), (int)(255 * throttle_median));
     //io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd), (int) (255 * foc.vq));
     wait(1.0f / LOG_FREQUENCY);
 }
@@ -193,8 +187,8 @@
 int main() {
     dq = new LutMapper();
     th = new NullThrottleMapper();
-    throttle_filter = new MedianFilter(3);
-    velocity_filter = new MedianFilter(3);
+    throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW);
+    velocity_filter = new MedianFilter(W_FILTER_WINDOW);
     
     BREMSInit(&io, &read, &foc, &control, false);