Diff: main.cpp

Revision:

16:f283d6032fe5

Parent:

15:b583cd30b063

Child:

17:2b852039bb05

--- a/main.cpp	Sun Oct 30 22:16:30 2016 +0000
+++ b/main.cpp	Sun Oct 30 22:41:00 2016 +0000
@@ -2,7 +2,6 @@
 #include "math.h"
 #include "PositionSensor.h"
 #include "FastPWM.h"
-#include "Transforms.h"
 #include "config_motor.h"
 #include "config_loop.h"
 #include "config_inverter.h"
@@ -12,14 +11,11 @@
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
-DigitalOut toggle(PC_10);
-PWM_IN p_in(TH_PIN, 1100, 1900);
-bool control_enabled = false;
+PwmIn throttle(TH_PIN, 1100, 1900);
 PositionSensorEncoder pos(CPR, 0);
 
 Serial pc(USBTX, USBRX);
 
-int state = 0;
 int adval1, adval2;
 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
 float p_mech, last_p_mech, w;
@@ -30,45 +26,23 @@
 float last_d = 0.0f, last_q = 0.0f;
 float d_ref = 0.0f, q_ref = 0.0f;
 
+bool control_enabled = false;
 
 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;
-}
+void go_enabled();
+void go_disabled();
+float fminf(float, float);
+float fmaxf(float, float);
 
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF ) {
-        toggle = 1;
         ADC1->CR2  |= 0x40000000; 
         volatile int delay;
         for (delay = 0; delay < 35; delay++);
-        toggle = 0;
         adval1 = ADC1->DR;
         adval2 = ADC2->DR;
         commutate();
@@ -177,9 +151,8 @@
 }    
 
 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;
+    if(control_enabled && !throttle.get_enabled()) go_disabled();
+    if(!control_enabled && throttle.get_enabled()) go_enabled();
     
     p = pos.GetElecPosition() - POS_OFFSET;
     if (p < 0) p += 2 * PI;
@@ -193,6 +166,9 @@
     float w_raw = dp_mech * (float) 1e6 / loop_period; //rad/s
     w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw;
     
+    q_ref = throttle.get_throttle() * Q_MAX;
+    d_ref = 0.0f;
+    
     float sin_p = sinf(p);
     float cos_p = cosf(p);
     
@@ -268,3 +244,25 @@
         //wait(0.1);
     }
 }
+
+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;
+}