Diff: main.cpp

Revision:

42:030e0ec4eac5

Parent:

40:22aede3d096f

Child:

44:3fd6a43b91f0

--- a/main.cpp	Thu Jan 05 05:54:20 2017 +0000
+++ b/main.cpp	Fri Jan 06 07:54:22 2017 +0000
@@ -5,8 +5,12 @@
 #include "FastPWM.h"
 #include "PwmIn.h"
 #include "MathHelpers.h"
-#include "Driving.h"
-#include "LutOptimize.h"
+#include "Transforms.h"
+#include "DQMapper.h"
+#include "ThrottleMapper.h"
+
+#include "BREMSStructs.h"
+#include "BREMSConfig.h"
 
 #include "config_motor.h"
 #include "config_loop.h"
@@ -14,258 +18,131 @@
 #include "config_inverter.h"
 #include "config_driving.h"
 
-FastPWM *a;
-FastPWM *b;
-FastPWM *c;
-DigitalOut en(EN);
-PwmIn throttle_in(TH_PIN, 1100, 1900);
-PositionSensorEncoder pos(CPR, 0);
-
-Serial pc(USBTX, USBRX);
+#include "main.h"
 
-int adval1, adval2, adval3;
-float vbus = BUS_VOLTAGE;
-float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
-float p_mech, last_p_mech, w = 0.0f;
-float d_filtered = 0.0f, q_filtered = 0.0f;
+IOStruct io;
+ReadDataStruct read;
+FOCStruct foc;
+ControlStruct control;
 
-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 = 0.0f;
+DQMapper *dq;
+ThrottleMapper *th;
 
 bool control_enabled = false;
 
-void commutate();
-void zero_current();
-void config_globals();
-void startup_msg();
+void update_velocity() {
+    read.last_p_mech = read.p_mech;
+    read.p_mech = io.pos->GetMechPosition();
+    float dp_mech = read.p_mech - read.last_p_mech;
+    if (dp_mech < -PI) dp_mech += 2 * PI;
+    if (dp_mech >  PI) dp_mech -= 2 * PI;
+    float w_raw = dp_mech * F_SW; //rad/s
+    if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0
+    if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
+    read.w = update_pid(read.w, w_raw, W_FILTER_STRENGTH);
+}
 
-void go_enabled();
-void go_disabled();
+void commutate() {
+    /*safety checks*/
+    if (control_enabled && !io.throttle_in->get_enabled()) go_disabled();
+    if (control_enabled && !io.pos->IsValid()) go_disabled();
+    if (!control_enabled && io.throttle_in->get_enabled()) go_enabled();
+    
+    /*update velocity, references*/
+    update_velocity();
+    float torque_percent = th->map(io.throttle_in->get_throttle(), read.w);
+    dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref);
+    
+    /*update position, sin, cos*/
+    foc.p = io.pos->GetElecPosition() - POS_OFFSET;
+    float sin_p = sinf(foc.p);
+    float cos_p = cosf(foc.p);
+    
+    /*scale and offset currents (adval1, 2 are updated in ISR*/    
+    foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
+    foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
+    
+    /*compute d, q*/
+    clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
+    park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
+    
+    /*PI controller*/
+    control.d_filtered = update_pid(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
+    control.q_filtered = update_pid(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
+        
+    float d_err = control.d_ref - control.d_filtered;
+    float q_err = control.q_ref - control.q_filtered;
+    
+    control.d_integral += d_err * KI;
+    control.q_integral += q_err * KI;
+    
+    control.q_integral = constrain(control.q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
+    control.d_integral = constrain(control.d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
+    
+    foc.vd = KP * d_err + control.d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE;
+    foc.vq = KP * q_err + control.q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE;
+    
+    foc.vd = constrain(foc.vd, -1.0f, 1.0f);
+    foc.vq = constrain(foc.vq, -1.0f, 1.0f);
+    
+    if (!control_enabled) {
+        foc.vd = 0.0f;
+        foc.vq = 0.0f;
+    }
+    
+    /*inverse transforms*/
+    invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta);
+    
+    float va, vb, vc, voff;
+    
+    invclarke(foc.valpha, foc.vbeta, &va, &vb);
+    vc = -va - vb;
+    
+    /*SVPWM*/
+    voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
+    va = va - voff;
+    vb = vb - voff;
+    vc = vc - voff;
+    
+    /*output to timers*/
+    set_dtc(io.a, 0.5f + 0.5f * va);
+    set_dtc(io.b, 0.5f + 0.5f * vb);
+    set_dtc(io.c, 0.5f + 0.5f * vc);
+}
+
+void go_enabled() {
+    control.d_integral = 0.0f;
+    control.q_integral = 0.0f;
+    control_enabled = true;
+    io.en->write(1);
+}
+
+void go_disabled() {
+    control_enabled = false;
+    io.en->write(0);
+}
+
+float update_pid(float old, float x, float str) {
+    return str * old + (1.0f - str) * x;
+}
 
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF ) {
         ADC1->CR2  |= 0x40000000; 
         volatile int delay;
         for (delay = 0; delay < 35; delay++);
-        adval1 = ADC1->DR;
-        adval2 = ADC2->DR;
+        read.adval1 = ADC1->DR;
+        read.adval2 = ADC2->DR;
         commutate();
     }
     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 update_velocity() {
-    last_p_mech = p_mech;
-    p_mech = pos.GetMechPosition();
-    float dp_mech = p_mech - last_p_mech;
-    if (dp_mech < -PI) dp_mech += 2 * PI;
-    if (dp_mech >  PI) dp_mech -= 2 * PI;
-    float w_raw = dp_mech * F_SW; //rad/s
-    if (w_raw > W_CRAZY) w_raw = w; //with this limiting scheme noise < 0
-    if (w_raw < -W_CRAZY) w_raw = w; //so we need to throw out the large deltas first
-    w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw;
-}
-
-/*map throttle to percent max torque*/
-float get_tqpct_cmd(float throttle, float w) {
-    float tq;
-    if (TORQUE_MODE) {
-        tq = throttle;
-    } else {
-        tq = get_driving_tqpct_cmd(throttle, w);
-    }
-    return tq;
-}
-
-/*get d, q based on % max torque command and velocity*/
-void get_dq(float torque_percent, float w, float *d, float *q) {
-    get_dq_lut(torque_percent, w, d, q); 
-}
-
-void commutate() {
-    if (control_enabled && !throttle_in.get_enabled()) go_disabled();
-    if (control_enabled && !pos.IsValid()) go_disabled();
-    if (!control_enabled && throttle_in.get_enabled()) go_enabled();
-    
-    update_velocity();
-    
-    p = pos.GetElecPosition() - POS_OFFSET;
-    float sin_p = sinf(p);
-    float cos_p = cosf(p);
-    
-    float torque_percent = get_tqpct_cmd(throttle_in.get_throttle(), w);
-    
-    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;
-    
-    alpha = ia;
-    beta = 1 / sqrtf(3.0f) * ia + 2 / sqrtf(3.0f) * ib;
-    
-    d = alpha * cos_p + beta * sin_p;
-    q = -alpha * sin_p + beta * cos_p;
-    
-    d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d;
-    q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q;
-    
-    get_dq(torque_percent, w, &d_ref, &q_ref);
-    
-    float d_err = d_ref - d_filtered;
-    float q_err = q_ref - q_filtered;
-    
-    d_integral += d_err * KI;
-    q_integral += q_err * KI;
-    
-    q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
-    d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
-    
-    vd = KP * d_err + d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE;
-    vq = KP * q_err + q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE;
-    
-    vd = constrain(vd, -1.0f, 1.0f);
-    vq = constrain(vq, -1.0f, 1.0f);
-    
-    if (!control_enabled) {
-        vd = 0.0f;
-        vq = 0.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;
-    
-    float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
-    va = va - voff;
-    vb = vb - voff;
-    vc = vc - voff;
-    
-    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();
+    BREMSInit(&io, &read, &foc, &control, false);
+    dq = new LutMapper();
+    th = new DrivingThrottleMapper();
     
     for (;;) {
     }
-}
-
-void go_enabled() {
-    d_integral = 0.0f;
-    q_integral = 0.0f;
-    control_enabled = true;
-    en = 1;
-}
-
-void go_disabled() {
-    control_enabled = false;
-    en = 0;
-}
-
-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("Switching Frequency: %f KHz \n\r", F_SW / 1000.0f);
-    pc.printf("Pole pairs: %d\n\r", (int) POLE_PAIRS);
-    pc.printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES);
-    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 config_globals() {
-    pc.baud(115200);
-    
-    //Enable clocks for GPIOs
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
-    
-    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock
-    
-    a = new FastPWM(PWMA);
-    b = new FastPWM(PWMB);
-    c = new FastPWM(PWMC);
-    
-    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 = (int) ((float) 9e7 / F_SW);
-    TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
-    TIM1->CR1 |= TIM_CR1_CEN;
-    
-    //ADC Setup
-    RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
-    RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
-    
-    ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels
-    
-    ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on
-    ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0
-    
-    ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON
-    ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1
-    
-    GPIOA->MODER |= (1 << 8);
-    GPIOA->MODER |= (1 << 9);
-    
-    GPIOA->MODER |= (1 << 2);
-    GPIOA->MODER |= (1 << 3);
-    
-    GPIOA->MODER |= (1 << 0);
-    GPIOA->MODER |= (1 << 1);
-    
-    GPIOB->MODER |= (1 << 0);
-    GPIOB->MODER |= (1 << 1);
-    
-    GPIOC->MODER |= (1 << 2);
-    GPIOC->MODER |= (1 << 3);
-    
-    //DAC setup
-    RCC->APB1ENR |= 0x20000000;
-    DAC->CR |= DAC_CR_EN2;
-    
-    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();
-    p_mech = pos.GetMechPosition();
-    en = 1;
-}
+}
\ No newline at end of file