Diff: main.cpp

Revision:

14:10743610ecb5

Parent:

11:825203ff4371

--- a/main.cpp	Mon Apr 18 20:08:05 2016 +0000
+++ b/main.cpp	Mon Apr 18 22:53:35 2016 +0000
@@ -26,17 +26,13 @@
 
 #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 2000.0f
-#define THROTTLE_ERROR 5000.0f
-#define THROTTLE_MIN 1500.0f 
-
 #define Q_REF_MAX (-200.0)
 
 FastPWM *a;
@@ -44,7 +40,7 @@
 FastPWM *c;
 DigitalOut en(EN);
 
-InterruptIn throttle_interrupt(PB_8);
+DigitalIn throttle_in(PB_8);
 PositionSensorEncoder pos(CPR, 0);
 
 int adval1, adval2;
@@ -53,8 +49,6 @@
 float throttle = 0.0f;
 
 int edge_seen = 0, watchdog_divider = 0;
-
-int err_throttle_too_low = 0, err_throttle_too_high = 0;
  
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF) {
@@ -85,31 +79,18 @@
         
         q = -alpha * sin_p - beta * cos_p;
         
-        //if (throttle < THROTTLE_MIN || throttle > THROTTLE_ERROR) throttle = THROTTLE_MIN;
-        if (throttle < THROTTLE_MIN) {
-            throttle = THROTTLE_MIN;
-            err_throttle_too_low = 1;
-        }
-        
-        /*
-        if(edge_seen == 0) {
-            throttle = THROTTLE_MIN;        
+        float throttle_scaler;
+                
+        if (throttle_in.read() == 0) {
+            throttle_scaler = 0.0f;
             en = 0;
         } else {
+            throttle_scaler = 1.0f;
             en = 1;
         }
-        */
-        
-        if (throttle > THROTTLE_ERROR) {
-            throttle = THROTTLE_MIN;
-            err_throttle_too_high = 1;
-        }
-        
-        if (throttle > THROTTLE_MAX) throttle = THROTTLE_MAX;
-        float throttle_scaler = (throttle - THROTTLE_MIN) / (THROTTLE_MAX - THROTTLE_MIN);
         
         double q_err = Q_REF_MAX * (double) throttle_scaler - q;
-                
+        
         //DAC->DHR12R2 = (unsigned int) (q_err * 20 + 2048);
         
         q_integral += q_err * KI;
@@ -119,39 +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) (throttle_scaler * 2000 + 2048);
-        
-        if (err_throttle_too_low || err_throttle_too_high || !edge_seen) {
-            en = 0;
-        } else {
-            en = 1;
-        }
-                                            
+                    
         *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;
-    edge_seen = 1;
-}
-
 int main() {
     //Enable clocks for GPIOs
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
@@ -224,8 +182,7 @@
     
     en = 1;
     
-    throttle_interrupt.rise(throttle_rise);
-    throttle_interrupt.fall(throttle_fall);
+    throttle_in.mode(PullUp);
     
     for (;;) {
     }