Diff: main.cpp

Revision:

150:08c13bfc7417

Parent:

143:a7a0c9d70e8a

Child:

152:6877dceec871

--- a/main.cpp	Thu May 04 12:54:00 2017 +0000
+++ b/main.cpp	Thu May 04 14:23:13 2017 +0000
@@ -9,6 +9,7 @@
 #include "DQMapper.h"
 #include "ThrottleMapper.h"
 #include "Calibration.h"
+#include "Filter.h"
 
 #include "BREMSStructs.h"
 #include "BREMSConfig.h"
@@ -29,9 +30,11 @@
 
 DQMapper *dq;
 ThrottleMapper *th;
+MedianFilter *throttle_filter, *velocity_filter;
 
 int loop_counter = 0;
 bool control_enabled = false;
+float throttle_median, w_median;
 
 void update_velocity() {
     read.last_p_mech = read.p_mech;
@@ -45,6 +48,7 @@
     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);
 }
 
@@ -134,6 +138,7 @@
 
 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);
     
     dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
@@ -160,8 +165,9 @@
 }
 
 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,%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);
 }
@@ -187,6 +193,9 @@
 int main() {
     dq = new LutMapper();
     th = new NullThrottleMapper();
+    throttle_filter = new MedianFilter(3);
+    velocity_filter = new MedianFilter(3);
+    
     BREMSInit(&io, &read, &foc, &control, false);
     
     for (;;) {