Diff: main.cpp

Revision:

0:2b1edabdd26b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Feb 03 03:38:05 2019 +0000
@@ -0,0 +1,187 @@
+#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 (;;) {
+    }
+}
\ No newline at end of file