Diff: main.cpp

Revision:

44:3fd6a43b91f0

Parent:

42:030e0ec4eac5

Child:

45:cf8ad81fb0f0

--- a/main.cpp	Fri Jan 06 09:10:45 2017 +0000
+++ b/main.cpp	Sun Jan 08 09:03:15 2017 +0000
@@ -28,18 +28,22 @@
 DQMapper *dq;
 ThrottleMapper *th;
 
+int loop_counter = 0;
 bool control_enabled = false;
 
 void update_velocity() {
     read.last_p_mech = read.p_mech;
     read.p_mech = io.pos->GetMechPosition();
+    
     float dp_mech = read.p_mech - read.last_p_mech;
     if (dp_mech < -PI) dp_mech += 2 * PI;
     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; //with this limiting scheme noise < 0
     if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
-    read.w = update_pid(read.w, w_raw, W_FILTER_STRENGTH);
+    
+    read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
 }
 
 void commutate() {
@@ -50,8 +54,11 @@
     
     /*update velocity, references*/
     update_velocity();
-    float torque_percent = th->map(io.throttle_in->get_throttle(), read.w);
-    dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref);
+    if (loop_counter % SLOW_LOOP_COUNTER == 0) {
+        loop_counter = 0;
+        slow_loop();
+    }
+    loop_counter++;
     
     /*update position, sin, cos*/
     foc.p = io.pos->GetElecPosition() - POS_OFFSET;
@@ -67,8 +74,8 @@
     park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
     
     /*PI controller*/
-    control.d_filtered = update_pid(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
-    control.q_filtered = update_pid(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
+    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);
         
     float d_err = control.d_ref - control.d_filtered;
     float q_err = control.q_ref - control.q_filtered;
@@ -99,7 +106,7 @@
     vc = -va - vb;
     
     /*SVPWM*/
-    voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
+    voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
     va = va - voff;
     vb = vb - voff;
     vc = vc - voff;
@@ -110,6 +117,11 @@
     set_dtc(io.c, 0.5f + 0.5f * vc);
 }
 
+void slow_loop() {
+    float torque_percent = th->map(io.throttle_in->get_throttle(), read.w);
+    dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref);
+}
+
 void go_enabled() {
     control.d_integral = 0.0f;
     control.q_integral = 0.0f;
@@ -122,7 +134,7 @@
     io.en->write(0);
 }
 
-float update_pid(float old, float x, float str) {
+float update_filter(float old, float x, float str) {
     return str * old + (1.0f - str) * x;
 }