Bayley Wang
/
dc_motor_control
bad dc motor controller with current mode
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?
User | Revision | Line number | New 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 | } |