Diff: main.cpp

Revision:

10:7624146c5945

Parent:

9:4812c9e932ea

Child:

11:825203ff4371

--- a/main.cpp	Tue Apr 05 04:05:21 2016 +0000
+++ b/main.cpp	Tue Apr 12 13:09:36 2016 +0000
@@ -25,7 +25,7 @@
 #define I_SCALE (R_BIAS * R_DOWN * I_SCALE_RAW / (R_DOWN * R_UP + R_BIAS * (R_DOWN + R_UP)))
 
 #define K_LOOP 0.02
-#define KI_BASE 0.08//0.008
+#define KI_BASE 0.008
 #define BUS_VOLTAGE 20.0
 
 #define KP (K_LOOP / BUS_VOLTAGE)
@@ -33,17 +33,26 @@
 
 #define INTEGRAL_MAX 1.0f
 
+#define THROTTLE_MAX 2400.0f
+#define THROTTLE_ERROR 4800.0f
+#define THROTTLE_MIN 500.0f 
+
+#define Q_REF_MAX 20.0
+
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
-DigitalOut toggle(PC_10);
 
+InterruptIn throttle_interrupt(PC_10);
 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, q_ref = 0.0, q_filtered = 0.0;
+double vq = 0.0, q_integral = 0.0, last_q = 0.0;
+float throttle = 0.0f;
+
+int edge_seen = 0, watchdog_divider = 0;
  
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF) {
@@ -73,11 +82,15 @@
         beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v;
         
         q = -alpha * sin_p - beta * cos_p;
-        q_filtered = 0.1 * (double) q + 0.9 * q_filtered;
         
-        DAC->DHR12R2 = (unsigned int) (q * 20 + 2048);
+        if(edge_seen == 0) throttle = THROTTLE_MIN;        
+        if (throttle < THROTTLE_MIN || throttle > THROTTLE_ERROR) throttle = THROTTLE_MIN;
+        if (throttle > THROTTLE_MAX) throttle = THROTTLE_MAX;
+        float throttle_scaler = (throttle - THROTTLE_MIN) / (THROTTLE_MAX - THROTTLE_MIN);
         
-        double q_err = q_ref - q;//_filtered;
+        double q_err = Q_REF_MAX * (double) throttle_scaler - q;
+        
+        //DAC->DHR12R2 = (unsigned int) (q_err * 20 + 2048);
         
         q_integral += q_err * KI;
         if (q_integral > INTEGRAL_MAX) q_integral = INTEGRAL_MAX;
@@ -92,10 +105,26 @@
         *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;
@@ -122,6 +151,12 @@
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;
     
+    TIM5->CR1 |= TIM_CR1_ARPE;
+    TIM5->EGR |= TIM_EGR_UG;
+    TIM5->PSC = 0x00;
+    TIM5->ARR = 0xFFFFFFFF;
+    TIM5->CR1 |= TIM_CR1_CEN;
+    
     //ADC Setup
     RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
     RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
@@ -162,10 +197,9 @@
     
     en = 1;
     
+    throttle_interrupt.rise(throttle_rise);
+    throttle_interrupt.fall(throttle_fall);
+    
     for (;;) {
-        q_ref = 0.0f;
-        wait_ms(2000);
-        q_ref = 20.0f;
-        wait_ms(2000);
     }
 }