Bayley Wang / Mbed 2 deprecated dc_motor_control

bad dc motor controller with current mode

Dependencies:   mbed FastPWM3

main.cpp@0:2b1edabdd26b, 2019-02-03 (annotated)

Committer:
bwang
Date:
Sun Feb 03 03:38:05 2019 +0000
Revision:
0:2b1edabdd26b
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 0:2b1edabdd26b 1 #include "mbed.h"
bwang 0:2b1edabdd26b 2 #include "globals.h"
bwang 0:2b1edabdd26b 3 #include "CommandProcessor.h"
bwang 0:2b1edabdd26b 4 #include "PreferenceWriter.h"
bwang 0:2b1edabdd26b 5 #include "Filter.h"
bwang 0:2b1edabdd26b 6 #include "FastPWM.h"
bwang 0:2b1edabdd26b 7
bwang 0:2b1edabdd26b 8 #define AVDD 3.3f
bwang 0:2b1edabdd26b 9 #define I_PN 300.0f
bwang 0:2b1edabdd26b 10 #define G_TH (.625f / I_PN)
bwang 0:2b1edabdd26b 11 #define R_UP 12.f
bwang 0:2b1edabdd26b 12 #define R_DN 12.f
bwang 0:2b1edabdd26b 13 #define V_REF 2.5f
bwang 0:2b1edabdd26b 14
bwang 0:2b1edabdd26b 15 Serial pc(USBTX, USBRX);
bwang 0:2b1edabdd26b 16 FastPWM *out1, *out2, *out3;
bwang 0:2b1edabdd26b 17 int adval1, adval2;
bwang 0:2b1edabdd26b 18 DigitalOut test(PC_6);
bwang 0:2b1edabdd26b 19 PreferenceWriter *pref;
bwang 0:2b1edabdd26b 20 float user_cmd;
bwang 0:2b1edabdd26b 21
bwang 0:2b1edabdd26b 22 char linebuf[128];
bwang 0:2b1edabdd26b 23 int index = 0;
bwang 0:2b1edabdd26b 24
bwang 0:2b1edabdd26b 25 float current, throttle, vout;
bwang 0:2b1edabdd26b 26 float integral, err;
bwang 0:2b1edabdd26b 27
bwang 0:2b1edabdd26b 28 void rxCallback() {
bwang 0:2b1edabdd26b 29 while (pc.readable()) {
bwang 0:2b1edabdd26b 30 char c = pc.getc();
bwang 0:2b1edabdd26b 31 if (c != 127 && c != 8 && c != '\r' && c != '\t') {
bwang 0:2b1edabdd26b 32 linebuf[index] = c;
bwang 0:2b1edabdd26b 33 if (index < 127) index++;
bwang 0:2b1edabdd26b 34 pc.putc(c);
bwang 0:2b1edabdd26b 35 } else if (c == 127 || c == 8) {
bwang 0:2b1edabdd26b 36 if (index > 0) {
bwang 0:2b1edabdd26b 37 index--;
bwang 0:2b1edabdd26b 38 //BS (8) should delete previous char
bwang 0:2b1edabdd26b 39 pc.putc(127);
bwang 0:2b1edabdd26b 40 }
bwang 0:2b1edabdd26b 41 } else if (c == '\r') {
bwang 0:2b1edabdd26b 42 linebuf[index] = 0;
bwang 0:2b1edabdd26b 43 pc.putc(c);
bwang 0:2b1edabdd26b 44 processCmd(&pc, linebuf);
bwang 0:2b1edabdd26b 45 index = 0;
bwang 0:2b1edabdd26b 46 pc.putc('>');
bwang 0:2b1edabdd26b 47 }
bwang 0:2b1edabdd26b 48 }
bwang 0:2b1edabdd26b 49 }
bwang 0:2b1edabdd26b 50
bwang 0:2b1edabdd26b 51 inline float to_amps(float adval) {
bwang 0:2b1edabdd26b 52 float adv = adval * AVDD;
bwang 0:2b1edabdd26b 53 float adv_unscaled = adv * (R_UP + R_DN) / R_DN;
bwang 0:2b1edabdd26b 54 float adv_unoffs = adv_unscaled - V_REF;
bwang 0:2b1edabdd26b 55 return adv_unoffs / G_TH;
bwang 0:2b1edabdd26b 56 }
bwang 0:2b1edabdd26b 57
bwang 0:2b1edabdd26b 58 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 0:2b1edabdd26b 59 if (TIM1->SR & TIM_SR_UIF ) {
bwang 0:2b1edabdd26b 60 ADC1->CR2 |= 0x40000000;
bwang 0:2b1edabdd26b 61 volatile int delay;
bwang 0:2b1edabdd26b 62 for (delay = 0; delay < 35; delay++);
bwang 0:2b1edabdd26b 63
bwang 0:2b1edabdd26b 64 adval1 = ADC1->DR;
bwang 0:2b1edabdd26b 65 adval2 = ADC2->DR;
bwang 0:2b1edabdd26b 66
bwang 0:2b1edabdd26b 67 current = to_amps((float) adval1 / 4095.0f);
bwang 0:2b1edabdd26b 68
bwang 0:2b1edabdd26b 69 switch (BREMS_src) {
bwang 0:2b1edabdd26b 70 case CMD_SRC_TERMINAL:
bwang 0:2b1edabdd26b 71 throttle = user_cmd;
bwang 0:2b1edabdd26b 72 break;
bwang 0:2b1edabdd26b 73 case CMD_SRC_ANALOG:
bwang 0:2b1edabdd26b 74 throttle = adval2 / 4095.0f;
bwang 0:2b1edabdd26b 75 break;
bwang 0:2b1edabdd26b 76 default:
bwang 0:2b1edabdd26b 77 break;
bwang 0:2b1edabdd26b 78 }
bwang 0:2b1edabdd26b 79
bwang 0:2b1edabdd26b 80 if (throttle < THROTTLE_DEADBAND) throttle = 0.0f;
bwang 0:2b1edabdd26b 81
bwang 0:2b1edabdd26b 82 err = throttle * I_LIMIT - current;
bwang 0:2b1edabdd26b 83 integral += err * KI;
bwang 0:2b1edabdd26b 84 if (integral < 0.0f) integral = 0.0f;
bwang 0:2b1edabdd26b 85 if (integral > 1.0f) integral = 1.0f;
bwang 0:2b1edabdd26b 86
bwang 0:2b1edabdd26b 87 vout = KP * err + integral;
bwang 0:2b1edabdd26b 88 if (vout < 0.0f) vout = 0.0f;
bwang 0:2b1edabdd26b 89 if (vout > 1.0f) vout = 1.0f;
bwang 0:2b1edabdd26b 90
bwang 0:2b1edabdd26b 91 switch (BREMS_op) {
bwang 0:2b1edabdd26b 92 case OP_TORQUE:
bwang 0:2b1edabdd26b 93 set_dtc(out1, vout);
bwang 0:2b1edabdd26b 94 break;
bwang 0:2b1edabdd26b 95 case OP_SPEED:
bwang 0:2b1edabdd26b 96 set_dtc(out1, user_cmd);
bwang 0:2b1edabdd26b 97 break;
bwang 0:2b1edabdd26b 98 default:
bwang 0:2b1edabdd26b 99 break;
bwang 0:2b1edabdd26b 100 }
bwang 0:2b1edabdd26b 101 }
bwang 0:2b1edabdd26b 102 TIM1->SR = 0x00;
bwang 0:2b1edabdd26b 103 }
bwang 0:2b1edabdd26b 104
bwang 0:2b1edabdd26b 105 void BREMSConfigRegisters() {
bwang 0:2b1edabdd26b 106 //Load preferences
bwang 0:2b1edabdd26b 107 cmd_clear(&pc);
bwang 0:2b1edabdd26b 108 pref = new PreferenceWriter(6);
bwang 0:2b1edabdd26b 109 cmd_reload(&pc, pref);
bwang 0:2b1edabdd26b 110 if (PREFS_VALID != 1) {
bwang 0:2b1edabdd26b 111 pc.printf("Stored preferences invalid, loading defaults\n");
bwang 0:2b1edabdd26b 112 KP = 1.0f;
bwang 0:2b1edabdd26b 113 KI = 1.0f;
bwang 0:2b1edabdd26b 114 I_LIMIT = 400.0f;
bwang 0:2b1edabdd26b 115 BREMS_src = CMD_SRC_ANALOG;
bwang 0:2b1edabdd26b 116 }
bwang 0:2b1edabdd26b 117
bwang 0:2b1edabdd26b 118 //safety first!
bwang 0:2b1edabdd26b 119 user_cmd = 0.0f;
bwang 0:2b1edabdd26b 120
bwang 0:2b1edabdd26b 121 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
bwang 0:2b1edabdd26b 122 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
bwang 0:2b1edabdd26b 123 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
bwang 0:2b1edabdd26b 124
bwang 0:2b1edabdd26b 125 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock
bwang 0:2b1edabdd26b 126
bwang 0:2b1edabdd26b 127 out1 = new FastPWM(PA_8);
bwang 0:2b1edabdd26b 128 out2 = new FastPWM(PA_9);
bwang 0:2b1edabdd26b 129 out3 = new FastPWM(PA_10);
bwang 0:2b1edabdd26b 130
bwang 0:2b1edabdd26b 131 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
bwang 0:2b1edabdd26b 132
bwang 0:2b1edabdd26b 133 TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
bwang 0:2b1edabdd26b 134 TIM1->CR1 = 0x40; //0x40 for CMS = 10, center aligned mode 1, 0x00 for edge aligned
bwang 0:2b1edabdd26b 135 TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on,
bwang 0:2b1edabdd26b 136 TIM1->RCR |= 0x01; //0x01 for center-aligned, 0x00 for edge aligned
bwang 0:2b1edabdd26b 137 TIM1->EGR |= TIM_EGR_UG;
bwang 0:2b1edabdd26b 138
bwang 0:2b1edabdd26b 139 TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
bwang 0:2b1edabdd26b 140 TIM1->ARR = (int) ((float) 9e7 / F_SW); //*2 for edge aligned
bwang 0:2b1edabdd26b 141 TIM1->CCER |= TIM_CCER_CC1P; //rising edge aligned, non-inverting
bwang 0:2b1edabdd26b 142 TIM1->CCER |= TIM_CCER_CC2P;
bwang 0:2b1edabdd26b 143 TIM1->CCER |= TIM_CCER_CC3P;
bwang 0:2b1edabdd26b 144 TIM1->CR1 |= TIM_CR1_CEN;
bwang 0:2b1edabdd26b 145
bwang 0:2b1edabdd26b 146 //ADC Setup
bwang 0:2b1edabdd26b 147 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
bwang 0:2b1edabdd26b 148 RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
bwang 0:2b1edabdd26b 149
bwang 0:2b1edabdd26b 150 ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels
bwang 0:2b1edabdd26b 151
bwang 0:2b1edabdd26b 152 ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on
bwang 0:2b1edabdd26b 153 ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0
bwang 0:2b1edabdd26b 154
bwang 0:2b1edabdd26b 155 ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON
bwang 0:2b1edabdd26b 156 ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1
bwang 0:2b1edabdd26b 157
bwang 0:2b1edabdd26b 158 //PA_4 as analog
bwang 0:2b1edabdd26b 159 GPIOA->MODER |= (1 << 8);
bwang 0:2b1edabdd26b 160 GPIOA->MODER |= (1 << 9);
bwang 0:2b1edabdd26b 161
bwang 0:2b1edabdd26b 162 //PB_0 as analog
bwang 0:2b1edabdd26b 163 GPIOB->MODER |= (1 << 0);
bwang 0:2b1edabdd26b 164 GPIOB->MODER |= (1 << 1);
bwang 0:2b1edabdd26b 165
bwang 0:2b1edabdd26b 166 //Outputs off
bwang 0:2b1edabdd26b 167 set_dtc(out1, 0.0f);
bwang 0:2b1edabdd26b 168 set_dtc(out2, 0.0f);
bwang 0:2b1edabdd26b 169 set_dtc(out3, 0.0f);
bwang 0:2b1edabdd26b 170 current = 0.0f;
bwang 0:2b1edabdd26b 171 throttle = 0.0f;
bwang 0:2b1edabdd26b 172 integral = 0.0f;
bwang 0:2b1edabdd26b 173
bwang 0:2b1edabdd26b 174 //Wait for sensors to settle
bwang 0:2b1edabdd26b 175 wait(1.0);
bwang 0:2b1edabdd26b 176 }
bwang 0:2b1edabdd26b 177
bwang 0:2b1edabdd26b 178 int main() {
bwang 0:2b1edabdd26b 179 pc.baud(115200);
bwang 0:2b1edabdd26b 180 pc.attach(rxCallback);
bwang 0:2b1edabdd26b 181
bwang 0:2b1edabdd26b 182 BREMSConfigRegisters();
bwang 0:2b1edabdd26b 183 pc.printf(">");
bwang 0:2b1edabdd26b 184
bwang 0:2b1edabdd26b 185 for (;;) {
bwang 0:2b1edabdd26b 186 }
bwang 0:2b1edabdd26b 187 }