Diff: main.cpp

Revision:

240:2aaffa217627

Parent:

234:ad68d7f8eff1

Child:

242:ac30f04fd6f7

--- a/main.cpp	Sat Nov 10 11:59:50 2018 +0000
+++ b/main.cpp	Sun Nov 11 12:34:19 2018 +0000
@@ -38,7 +38,7 @@
         go_enabled();
     }
     
-    /*update velocity, references*/
+    /*update velocity, user command*/
     update_velocity();
     if (loop_counter % SLOW_LOOP_COUNTER == 0) {
         loop_counter = 0;
@@ -46,6 +46,36 @@
     }
     loop_counter++;
     io.blink->update();
+    
+    /*generate torque command from user command*/
+    float w_ref, w_err;
+    switch (BREMS_op) {
+    case OP_TORQUE:
+        control.w_integral = 0.0f;
+        control.torque_percent = control.user_cmd;
+        break;
+    case OP_DRIVING:
+        control.w_integral = 0.0f;
+        control.torque_percent = th->map(control.user_cmd, read.w);
+        break;
+    case OP_SPEED:
+        w_ref = control.user_cmd * _W_SETPOINT_MAX;
+        w_err = w_ref - read.w;
+        control.w_integral += w_err * KI_W;
+        control.w_integral = constrain(control.w_integral, -1.0f, 1.0f);
+        control.torque_percent = KP_W * w_err + control.w_integral;
+        control.torque_percent = constrain(control.torque_percent, -1.0f, 1.0f);
+        break;
+    case OP_POSITION:
+        break;
+    default:
+        //this should never happen!
+        control.torque_percent = 0.0f;
+        break;
+    }
+    
+    /*update references*/    
+    dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
         
     /*update position, sin, cos*/
     foc.p = io.pos->GetElecPosition() - _POS_OFFSET;
@@ -142,9 +172,6 @@
         control.user_cmd = 0.0f;
         break;
     }
-
-    control.torque_percent = th->map(control.user_cmd, read.w);
-    dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
 }
 
 void log() {