File content as of revision 0:2b1edabdd26b:

#include "mbed.h"
#include "globals.h"
#include "CommandProcessor.h"
#include "PreferenceWriter.h"
#include "Filter.h"
#include "FastPWM.h"

#define AVDD 3.3f
#define I_PN 300.0f
#define G_TH (.625f / I_PN)
#define R_UP 12.f
#define R_DN 12.f
#define V_REF 2.5f

Serial pc(USBTX, USBRX);
FastPWM *out1, *out2, *out3;
int adval1, adval2;
DigitalOut test(PC_6);
PreferenceWriter *pref;
float user_cmd;

char linebuf[128];
int index = 0;

float current, throttle, vout;
float integral, err;

void rxCallback() {
    while (pc.readable()) {
        char c = pc.getc();
        if (c != 127 && c != 8 && c != '\r' && c != '\t') {
            linebuf[index] = c;
            if (index < 127) index++;
            pc.putc(c);
        } else if (c == 127 || c == 8) {
            if (index > 0) {
                index--;
                //BS (8) should delete previous char
                pc.putc(127);
            }
        } else if (c == '\r') {
            linebuf[index] = 0;
            pc.putc(c);
            processCmd(&pc, linebuf);
            index = 0;
            pc.putc('>');
        }
    }
}

inline float to_amps(float adval) {
    float adv = adval * AVDD;
    float adv_unscaled = adv * (R_UP + R_DN) / R_DN;
    float adv_unoffs = adv_unscaled - V_REF;
    return adv_unoffs / G_TH;
}

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;
        
        current = to_amps((float) adval1 / 4095.0f);

        switch (BREMS_src) {
        case CMD_SRC_TERMINAL:
            throttle = user_cmd;
            break;
        case CMD_SRC_ANALOG:
            throttle = adval2 / 4095.0f;
            break;
        default:
            break;
        }
        
        if (throttle < THROTTLE_DEADBAND) throttle = 0.0f;
        
        err = throttle * I_LIMIT - current;
        integral += err * KI;
        if (integral < 0.0f) integral = 0.0f;
        if (integral > 1.0f) integral = 1.0f;
        
        vout = KP * err + integral;
        if (vout < 0.0f) vout = 0.0f;
        if (vout > 1.0f) vout = 1.0f;
                
        switch (BREMS_op) {
        case OP_TORQUE:
            set_dtc(out1, vout);
            break;
        case OP_SPEED:
            set_dtc(out1, user_cmd);
            break;
        default:
            break;
        }     
    }
    TIM1->SR = 0x00;
}

void BREMSConfigRegisters() {
    //Load preferences
    cmd_clear(&pc);
    pref = new PreferenceWriter(6);
    cmd_reload(&pc, pref);
    if (PREFS_VALID != 1) {
        pc.printf("Stored preferences invalid, loading defaults\n");
        KP = 1.0f;
        KI = 1.0f;
        I_LIMIT = 400.0f;
        BREMS_src = CMD_SRC_ANALOG;
    }
    
    //safety first!
    user_cmd = 0.0f;

    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
    
    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock
    
    out1 = new FastPWM(PA_8);
    out2 = new FastPWM(PA_9);
    out3 = new FastPWM(PA_10);
    
    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ

    TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
    TIM1->CR1 = 0x40; //0x40 for CMS = 10, center aligned mode 1, 0x00 for edge aligned
    TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, 
    TIM1->RCR |= 0x01; //0x01 for center-aligned, 0x00 for edge aligned
    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); //*2 for edge aligned
    TIM1->CCER |= TIM_CCER_CC1P; //rising edge aligned, non-inverting
    TIM1->CCER |= TIM_CCER_CC2P;
    TIM1->CCER |= TIM_CCER_CC3P;
    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
    
    //PA_4 as analog
    GPIOA->MODER |= (1 << 8);
    GPIOA->MODER |= (1 << 9);
    
    //PB_0 as analog
    GPIOB->MODER |= (1 << 0);
    GPIOB->MODER |= (1 << 1);
    
    //Outputs off
    set_dtc(out1, 0.0f);
    set_dtc(out2, 0.0f);
    set_dtc(out3, 0.0f);
    current = 0.0f;
    throttle = 0.0f;
    integral = 0.0f;
    
    //Wait for sensors to settle
    wait(1.0);    
}

int main() {    
    pc.baud(115200);
    pc.attach(rxCallback);
        
    BREMSConfigRegisters();
    pc.printf(">");
        
    for (;;) {
    }
}