Diff: main.cpp

Revision:

11:825203ff4371

Parent:

10:7624146c5945

Child:

12:0811d08424e1

Child:

14:10743610ecb5

--- a/main.cpp	Tue Apr 12 13:09:36 2016 +0000
+++ b/main.cpp	Mon Apr 18 20:08:05 2016 +0000
@@ -33,18 +33,18 @@
 
 #define INTEGRAL_MAX 1.0f
 
-#define THROTTLE_MAX 2400.0f
-#define THROTTLE_ERROR 4800.0f
-#define THROTTLE_MIN 500.0f 
+#define THROTTLE_MAX 2000.0f
+#define THROTTLE_ERROR 5000.0f
+#define THROTTLE_MIN 1500.0f 
 
-#define Q_REF_MAX 20.0
+#define Q_REF_MAX (-200.0)
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
 
-InterruptIn throttle_interrupt(PC_10);
+InterruptIn throttle_interrupt(PB_8);
 PositionSensorEncoder pos(CPR, 0);
 
 int adval1, adval2;
@@ -53,6 +53,8 @@
 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) {
@@ -83,13 +85,31 @@
         
         q = -alpha * sin_p - beta * cos_p;
         
-        if(edge_seen == 0) throttle = THROTTLE_MIN;        
-        if (throttle < THROTTLE_MIN || throttle > THROTTLE_ERROR) throttle = THROTTLE_MIN;
+        //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;        
+            en = 0;
+        } else {
+            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;
@@ -99,9 +119,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);