File content as of revision 11:825203ff4371:

#include "mbed.h"
#include "math.h"
#include "PositionSensor.h"
#include "FastPWM.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

#define THROTTLE_MAX 2000.0f
#define THROTTLE_ERROR 5000.0f
#define THROTTLE_MIN 1500.0f 

#define Q_REF_MAX (-200.0)

FastPWM *a;
FastPWM *b;
FastPWM *c;
DigitalOut en(EN);

InterruptIn throttle_interrupt(PB_8);
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;
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) {
        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++);
        adval1 = ADC1->DR;
        adval2 = ADC2->DR;
        
        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;
        
        //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;
        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;
                        
        //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);
        
        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;
    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 = 0x2EE0;
    //TIM1->ARR = 0x1770; //15 Khz
    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
    
    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);
    
    *a = 0.0f;
    *b = 0.0f;
    *c = 0.0f;
    
    en = 1;
    
    throttle_interrupt.rise(throttle_rise);
    throttle_interrupt.fall(throttle_fall);
    
    for (;;) {
    }
}