Bayley Wang
/
dc_motor_control
bad dc motor controller with current mode
Diff: main.cpp
- Revision:
- 0:2b1edabdd26b
diff -r 000000000000 -r 2b1edabdd26b main.cpp --- /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