Diff: main.cpp

Revision:

15:8eb1dfbf0d41

Parent:

12:0811d08424e1

Child:

16:46703e957b30

--- a/main.cpp	Mon Apr 18 21:06:26 2016 +0000
+++ b/main.cpp	Tue Apr 19 03:26:15 2016 +0000
@@ -26,37 +26,29 @@
 
 #define K_LOOP 0.02
 #define KI_BASE 0.008
-#define BUS_VOLTAGE 20.0
+#define BUS_VOLTAGE 200.0
 
 #define KP (K_LOOP / BUS_VOLTAGE)
 #define KI (KI_BASE * K_LOOP / BUS_VOLTAGE)
 
 #define INTEGRAL_MAX 1.0f
 
-#define THROTTLE_MAX 1900.0f
-#define THROTTLE_MIN 1600.0f 
-
-#define THROTTLE_HIGH 5000.0f
-#define THROTTLE_LOW 1550.0f
-
-#define Q_REF_MAX (-200.0)
+#define Q_REF_MAX (-100.0)
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
 
-InterruptIn throttle_interrupt(PB_8);
+DigitalIn throttle_in(PB_8);
 PositionSensorEncoder pos(CPR, 0);
 
 int adval1, adval2;
 float ia, ib, ic, alpha, beta, q;
 double vq = 0.0, q_integral = 0.0, last_q = 0.0;
-volatile float throttle = 0.0f, throttle_scaler = 0.0f;
+float throttle = 0.0f;
 
-volatile int edge_seen = 0, watchdog_divider = 0;
-
-volatile int err_throttle_too_low = 0, err_throttle_too_high = 0;
+int edge_seen = 0, watchdog_divider = 0;
  
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF) {
@@ -87,10 +79,18 @@
         
         q = -alpha * sin_p - beta * cos_p;
         
-        DAC->DHR12R2 = throttle;
+        float throttle_scaler;
+                
+        if (throttle_in.read() == 0) {
+            throttle_scaler = 0.0f;
+            en = 0;
+        } else {
+            throttle_scaler = 1.0f;
+            en = 1;
+        }
         
         double q_err = Q_REF_MAX * (double) throttle_scaler - q;
-                
+        
         //DAC->DHR12R2 = (unsigned int) (q_err * 20 + 2048);
         
         q_integral += q_err * KI;
@@ -100,60 +100,16 @@
         vq = KP * q_err + q_integral;
         if (vq < -1.0f) vq = -1.0f;
         if (vq > 1.0f) vq = 1.0f;
-                        
+        
         //DAC->DHR12R2 = (unsigned int) (vq * 2000 + 2048);
-        //DAC->DHR12R2 = (unsigned int) (err_throttle_too_low * 2000 + 2048);
-                                            
+                    
         *a = 0.5f + 0.5f * vq * sinf(p);
         *b = 0.5f + 0.5f * vq * sinf(p + 2 * PI / 3);
         *c = 0.5f + 0.5f * vq * sinf(p - 2 * PI / 3);
-        
-        watchdog_divider++;
-        if (watchdog_divider == 15000) {
-            watchdog_divider = 0;
-            edge_seen = 0;
-        }
     }
     TIM1->SR = 0x00;
 }
 
-void throttle_rise() {
-    TIM5->CNT = 0;
-    edge_seen = 1;
-}
-
-void throttle_fall() {
-    throttle = TIM5->CNT;
-    
-    /*
-    if (throttle < THROTTLE_LOW) {
-        err_throttle_too_low = 1;
-    } else {
-        err_throttle_too_low = 0;
-    }
-    
-    if (throttle > THROTTLE_HIGH) {
-        err_throttle_too_high = 1;
-    } else {
-        err_throttle_too_high = 0;
-    }
-    */
-    
-    if (throttle < THROTTLE_MIN) throttle = THROTTLE_MIN;
-    if (throttle > THROTTLE_MAX) throttle = THROTTLE_MAX;
-    throttle_scaler = (throttle - THROTTLE_MIN) / (THROTTLE_MAX - THROTTLE_MIN);
-    
-    /*
-    if (err_throttle_too_low || err_throttle_too_high || !edge_seen) {
-        en = 0;
-    } else {
-        en = 1;
-    }
-    */
-    
-    edge_seen = 1;
-}
-
 int main() {
     //Enable clocks for GPIOs
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
@@ -226,8 +182,7 @@
     
     en = 1;
     
-    throttle_interrupt.rise(throttle_rise);
-    throttle_interrupt.fall(throttle_fall);
+    throttle_in.mode(PullUp);
     
     for (;;) {
     }