Diff: main.cpp

Revision:

153:8a1f9888f003

Parent:

152:6877dceec871

--- a/main.cpp	Thu May 04 15:16:56 2017 +0000
+++ b/main.cpp	Thu May 04 16:14:29 2017 +0000
@@ -31,6 +31,7 @@
 DQMapper *dq;
 ThrottleMapper *th;
 MedianFilter *throttle_filter, *velocity_filter;
+MovingAverageFilter *d_filter, *q_filter;
 
 int loop_counter = 0;
 bool control_enabled = false;
@@ -76,8 +77,8 @@
     park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
     
     /*PI controller*/
-    control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
-    control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
+    control.d_filtered = d_filter->update(foc.d);
+    control.q_filtered = q_filter->update(foc.q);
         
     float d_err = control.d_ref - control.d_filtered;
     float q_err = control.q_ref - control.q_filtered;
@@ -160,8 +161,8 @@
 }
 
 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,%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,8 +188,11 @@
 int main() {
     dq = new LutMapper();
     th = new NullThrottleMapper();
+    
     throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW);
     velocity_filter = new MedianFilter(W_FILTER_WINDOW);
+    d_filter = new MovingAverageFilter(DQ_FILTER_WINDOW);
+    q_filter = new MovingAverageFilter(DQ_FILTER_WINDOW);
     
     BREMSInit(&io, &read, &foc, &control, false);