Diff: main.cpp

Revision:

8:314074b56470

Parent:

7:caebf421f288

Child:

9:4812c9e932ea

--- a/main.cpp	Wed Mar 30 06:50:11 2016 +0000
+++ b/main.cpp	Mon Apr 04 09:02:59 2016 +0000
@@ -2,64 +2,99 @@
 #include "math.h"
 #include "PositionSensor.h"
 #include "FastPWM.h"
-#include "Transforms.h"
-#include "config.h"
+
+#define PWMA PA_8
+#define PWMB PA_9
+#define PWMC PA_10
+#define EN PB_15
+
+#define IA PA_4
+#define IB PB_0
+
+#define PI 3.141593f
+#define CPR 4096
+#define POS_OFFSET 4.5f
+
+#define I_SCALE_RAW 25.0f //mv/A
+#define R_UP 12000.0f //ohms
+#define R_DOWN 3600.0f //ohms
+#define R_BIAS 3600.0f //ohms
+#define AVDD 3300.0f //mV
+
+#define I_OFFSET (AVDD * R_DOWN * R_UP / (R_DOWN * R_UP + R_BIAS * (R_DOWN + R_UP)))
+#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.008
+#define BUS_VOLTAGE 20.0
+
+#define KP (K_LOOP / BUS_VOLTAGE)
+#define KI (KI_BASE * K_LOOP / BUS_VOLTAGE)
+
+#define INTEGRAL_MAX 1.0f
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
-//DigitalOut toggle(PC_10);
+DigitalOut toggle(PC_10);
 
 PositionSensorEncoder pos(CPR, 0);
 
-Serial pc(USBTX, USBRX);
-
-int state = 0;
 int adval1, adval2;
-float ia, ib, ic, alpha, beta, d, q, vd, vq, p = 0.0f;
-
-float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
-
-float d_integral = 0.0f, q_integral = 0.0f;
-float last_d = 0.0f, last_q = 0.0f;
-float d_ref = -0.0f, q_ref = 30.0f;
-
-void commutate();
-void zero_current();
-void config_globals();
-void startup_msg();
-
+float ia, ib, ic, alpha, beta, q;
+double vq = 0.0, q_integral = 0.0, last_q = 0.0, q_ref = 250.0, q_filtered = 0.0;
+ 
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
-    if (TIM1->SR & TIM_SR_UIF ) {
-        //toggle = 1;
+    if (TIM1->SR & TIM_SR_UIF) {
+        float p = pos.GetElecPosition() - POS_OFFSET;
+        if (p < 0) p += 2 * PI;
+        
+        //float pos_dac = 0.85f * p / (2 * PI) + 0.05f;
+        //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096);
+        
+        float sin_p = sinf(p);
+        float cos_p = cosf(p);
+        
         ADC1->CR2  |= 0x40000000; 
         volatile int delay;
         for (delay = 0; delay < 35; delay++);
-        //toggle = 0;
         adval1 = ADC1->DR;
         adval2 = ADC2->DR;
-        commutate();
+        
+        ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET) / I_SCALE;
+        ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET) / I_SCALE;
+        ic = -ia - ib;
+        
+        float u = ib;
+        float v = ic;
+    
+        alpha = u;
+        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_filtered * 20 + 2048);
+        
+        double q_err = q_ref - q_filtered;
+        
+        q_integral += q_err * KI;
+        if (q_integral > INTEGRAL_MAX) q_integral = INTEGRAL_MAX;
+        if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX;
+        
+        vq = KP * q_err + q_integral;
+        if (vq < -1.0f) vq = -1.0f;
+        if (vq > 1.0f) vq = 1.0f;
+            
+        *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);
     }
     TIM1->SR = 0x00;
 }
-
-void zero_current(){
-    for (int i = 0; i < 1000; i++){
-        ia_supp_offset += (float) (ADC1->DR);
-        ib_supp_offset += (float) (ADC2->DR);
-        ADC1->CR2  |= 0x40000000;
-        wait_us(100); 
-    }
-    ia_supp_offset /= 1000.0f;
-    ib_supp_offset /= 1000.0f;
-    ia_supp_offset = ia_supp_offset / 4096.0f * AVDD - I_OFFSET;
-    ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET;
-}
-
-void config_globals() {
-    pc.baud(115200);
-    
+   
+int main() {
     //Enable clocks for GPIOs
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
@@ -74,13 +109,14 @@
     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 = 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 = 0x1770; //15 Khz
+    TIM1->ARR = 0x2EE0;
+    //TIM1->ARR = 0x1770; //15 Khz
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;
     
@@ -118,109 +154,12 @@
     GPIOA->MODER |= (1 << 10);
     GPIOA->MODER |= (1 << 11);
     
-    //Zero duty cycles
-    set_dtc(a, 0.0f);
-    set_dtc(b, 0.0f);
-    set_dtc(c, 0.0f);
-    
-    wait_ms(250);
-    zero_current();
-    en = 1;
-}
-
-void startup_msg() {
-    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
-    pc.printf("%s\n\r", "====Config Data====");
-    pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
-    pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
-    pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
-    pc.printf("Loop KP: %f\n\r", KP);
-    pc.printf("Loop KI: %f\n\r", KI);
-    pc.printf("Ia offset: %f mV\n\r", ia_supp_offset);
-    pc.printf("Ib offset: %f mV\n\r", ib_supp_offset);
-    pc.printf("\n\r");
-}    
-
-void commutate() {
-    p = pos.GetElecPosition() - POS_OFFSET;
-         
-    if (p < 0) p += 2 * PI;
-    if (p > 2 * PI) p -= 2 * PI;
-    
-    float sin_p = sinf(p);
-    float cos_p = cosf(p);
-    
-    //float pos_dac = 0.8f * p / (2 * PI) + 0.1f;
-    //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096);
-    
-    ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
-    ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE;
-    ic = -ia - ib;
-    
-    float u = ib;
-    float v = ic;
-    
-    alpha = u;
-    beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v;
+    *a = 0.0f;
+    *b = 0.0f;
+    *c = 0.0f;
     
-    d = alpha * cos_p - beta * sin_p;
-    q = -alpha * sin_p - beta * cos_p;
-    
-    float d_err = d_ref - d;
-    float q_err = q_ref - q;
-    
-    d_integral += d_err * KI;
-    q_integral += q_err * KI;
-    
-    if (q_integral > INTEGRAL_MAX) q_integral = INTEGRAL_MAX;
-    if (d_integral > INTEGRAL_MAX) d_integral = INTEGRAL_MAX;
-    if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX;
-    if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX;
-    
-    vd = KP * d_err + d_integral;
-    vq = KP * q_err + q_integral;
-    
-    if (vd < -1.0f) vd = -1.0f;
-    if (vd > 1.0f) vd = 1.0f;
-    if (vq < -1.0f) vq = -1.0f;
-    if (vq > 1.0f) vq = 1.0f;
-    
-    //DAC->DHR12R2 = (unsigned int) (q * 20 + 2048);
-    //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048);
-    
-    //vd = 0.0f;
-    //vq = 1.0f;
-    
-    float valpha = vd * cos_p - vq * sin_p;
-    float vbeta = vd * sin_p + vq * cos_p;
-    
-    float va = valpha;
-    float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
-    float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
-    
-    //DAC->DHR12R2 = (unsigned int) (-va * 1500 + 2048);
-    
-    set_dtc(a, 0.5f + 0.5f * va);
-    set_dtc(b, 0.5f + 0.5f * vb);
-    set_dtc(c, 0.5f + 0.5f * vc);
-}
-    
-int main() {
-    config_globals();
-    startup_msg();
+    en = 1;
     
     for (;;) {
-        //pc.printf("%f\n\r", p);
-        //wait_ms(100);
-        /*
-        q_ref = 0.0f;
-        wait(3);
-        toggle = state;
-        state = !state;
-        q_ref = -50.0f;
-        wait(3);
-        toggle = state;
-        state = !state;
-        */
     }
 }