Diff: main.cpp

Revision:

17:77e5b417e6ef

Parent:

16:46703e957b30

--- a/main.cpp	Tue Apr 19 05:46:29 2016 +0000
+++ b/main.cpp	Tue Apr 19 06:35:06 2016 +0000
@@ -33,7 +33,12 @@
 
 #define INTEGRAL_MAX 1.0f
 
-#define Q_REF_MAX (-100.0)
+#define MAX_AMPS_DRIVE (-100.0)
+#define MAX_AMPS_BRAKE (20.0)
+
+#define STATE_ON 1
+#define STATE_BRAKE 2
+#define STATE_COAST 3
 
 FastPWM *a;
 FastPWM *b;
@@ -44,13 +49,15 @@
 PositionSensorEncoder pos(CPR, 0);
 Serial pc(USBTX, USBRX);
 
-int adval1, adval2;
-float ia, ib, ic, alpha, beta, q;
-double vq = 0.0, q_integral = 0.0, last_q = 0.0;
+volatile int adval1, adval2;
+volatile float ia, ib, ic, alpha, beta, q;
+volatile double vq = 0.0, q_integral = 0.0, last_q = 0.0, q_ref = 0.0;
 
-float throttle_scaler = 0.0f;
+volatile int throttle = 0, last_throttle = 0;
 
-int state = 0;
+volatile int state = STATE_COAST;
+
+volatile int brake_divider = 0, brake_ticker = 0;
  
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF) {
@@ -80,18 +87,36 @@
         beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v;
         
         q = -alpha * sin_p - beta * cos_p;
-                
-        if (throttle_in.read() == 0) {
-            state = 0;
-            throttle_scaler = 0.0f;
-            en = 0;
+        
+        last_throttle = throttle;
+        throttle = throttle_in.read();
+        
+        if (throttle == 0) {
+            if (last_throttle == 1) {
+                state = STATE_BRAKE;
+                brake_ticker = 0;
+            } else if (brake_ticker > 10) {
+                state = STATE_COAST;
+            }
         } else {
-            state = 1;
-            throttle_scaler = 1.0f;
-            en = 1;
+            state = STATE_ON;
         }
         
-        double q_err = Q_REF_MAX * (double) throttle_scaler - q;
+        if (state == STATE_ON || state == STATE_BRAKE) {
+            en = 1;
+        } else if (state == STATE_COAST) {
+            en = 0;
+        }
+        
+        if (state == STATE_ON) {
+            q_ref = MAX_AMPS_DRIVE;
+        } else if (state == STATE_OFF) {
+            q_ref = MAX_AMPS_BRAKE;
+        } else if (state == STATE_COAST) {
+            q_ref = 0;
+        }
+        
+        double q_err = q_ref - q;
         
         //DAC->DHR12R2 = (unsigned int) (q_err * 20 + 2048);
         
@@ -108,6 +133,12 @@
         *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);
+        
+        brake_divider++;
+        if (brake_divider == 15000) {
+            brake_divider = 0;
+            brake_ticker++;
+        }
     }
     TIM1->SR = 0x00;
 }
@@ -127,14 +158,12 @@
     NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
 
     TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
-    //TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up
     TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, 
     TIM1->RCR |= 0x01; //update event once per up/down count of tim1 
     TIM1->EGR |= TIM_EGR_UG;
     
     TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
     TIM1->ARR = 0x2EE0;
-    //TIM1->ARR = 0x1770; //15 Khz
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;
     
@@ -191,7 +220,7 @@
     pc.printf("%s\n\r", "THE DENTIST controller Rev. A");
     
     for (;;) {
-        pc.printf("%d %f\n\r", state, throttle_scaler);
-        wait_ms(100);
+        //pc.printf("%d %f\n\r", state, throttle);
+        //wait_ms(100);
     }
 }