Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Diff: main.cpp

Revision:
21:b7fb355c8c2d
Parent:
20:91ae97a811e3
Child:
22:72840d3db788
--- a/main.cpp	Sat Nov 05 07:52:36 2016 +0000
+++ b/main.cpp	Sat Nov 05 09:12:10 2016 +0000
@@ -123,6 +123,14 @@
     q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     
+    if(control_enabled) {
+        vd = KP * d_err + d_integral;
+        vq = KP * q_err + q_integral;
+    } else {
+        vd = 0;
+        vq = 0;
+    }
+    
     vd = constrain(vd, -1.0f, 1.0f);
     vq = constrain(vq, -1.0f, 1.0f);