Diff: main.cpp

Revision:

13:41d102a53caf

Parent:

12:5723a4fa5864

Child:

14:59c4fcc1a4f7

--- a/main.cpp	Sun Oct 30 01:37:30 2016 +0000
+++ b/main.cpp	Sun Oct 30 02:06:03 2016 +0000
@@ -4,13 +4,15 @@
 #include "FastPWM.h"
 #include "Transforms.h"
 #include "config.h"
+#include "pwm_in.h"
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
 DigitalOut toggle(PC_10);
-
+PWM_IN p_in(PB_8, 1100, 1900);
+bool control_enabled = false;
 PositionSensorEncoder pos(CPR, 0);
 
 Serial pc(USBTX, USBRX);
@@ -25,11 +27,38 @@
 float last_d = 0.0f, last_q = 0.0f;
 float d_ref = 0.0f, q_ref = 50.0f;
 
+
 void commutate();
 void zero_current();
 void config_globals();
 void startup_msg();
 
+void go_enabled()
+{
+    d_integral = 0.0f;
+    q_integral = 0.0f;
+    control_enabled = true;
+    en = 1;
+}
+
+void go_disabled()
+{
+    control_enabled = false;
+    en = 0;
+}
+
+float fminf(float a, float b)
+{
+    if(a < b) return a;
+    return b;
+}
+
+float fmaxf(float a, float b)
+{
+    if(a > b) return a;
+    return b;
+}
+
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF ) {
         toggle = 1;
@@ -144,6 +173,9 @@
 }    
 
 void commutate() {
+    if(control_enabled && !p_in.get_enabled()) go_disabled();
+    if(!control_enabled && p_in.get_enabled()) go_enabled();
+    q_ref = p_in.get_throttle() * Q_MAX;
     p = pos.GetElecPosition() - POS_OFFSET;
     if (p < 0) p += 2 * PI;
     
@@ -177,8 +209,16 @@
     if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX;
     if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX;
     
-    vd = KP * d_err + d_integral;
-    vq = KP * q_err + q_integral;
+    if(control_enabled)
+    {
+        vd = KP * d_err + d_integral;
+        vq = KP * q_err + q_integral;
+    }
+    else
+    {
+        vd = 0;
+        vq = 0;
+    }
     
     if (vd < -1.0f) vd = -1.0f;
     if (vd > 1.0f) vd = 1.0f;
@@ -198,6 +238,11 @@
     float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
     float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
     
+    float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
+    va = va - voff;
+    vb = vb - voff;
+    vc = vc - voff;
+    
     set_dtc(a, 0.5f + 0.5f * va);
     set_dtc(b, 0.5f + 0.5f * vb);
     set_dtc(c, 0.5f + 0.5f * vc);