Jared DiCarlo
/
george
motor controller
io.cpp@4:4e00b310811d, 2016-10-30 (annotated)
- Committer:
- dicarloj
- Date:
- Sun Oct 30 22:30:39 2016 +0000
- Revision:
- 4:4e00b310811d
- Parent:
- 3:08746709f023
- Child:
- 7:ed19a937daa0
dsafdsafdsa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dicarloj | 0:f899a5183b5e | 1 | #include "mbed.h" |
dicarloj | 0:f899a5183b5e | 2 | #include "config.h" |
dicarloj | 0:f899a5183b5e | 3 | #include "io.h" |
dicarloj | 0:f899a5183b5e | 4 | #include "foc.h" |
dicarloj | 2:7312ac02785d | 5 | #include "pwm_in.h" |
dicarloj | 1:2acd7dfc4b1b | 6 | |
dicarloj | 2:7312ac02785d | 7 | Serial pc_ser(USBTX, USBRX); |
dicarloj | 2:7312ac02785d | 8 | DigitalOut dout(PC_9); |
dicarloj | 2:7312ac02785d | 9 | PwmOut a(PA_9); |
dicarloj | 2:7312ac02785d | 10 | PwmOut b(PA_8); |
dicarloj | 2:7312ac02785d | 11 | PwmOut c(PA_10); |
dicarloj | 2:7312ac02785d | 12 | PWM_IN p_in(PB_8, 1100, 1900); |
dicarloj | 2:7312ac02785d | 13 | InterruptIn *index_interrupt; |
dicarloj | 2:7312ac02785d | 14 | DigitalIn *index_in; |
dicarloj | 0:f899a5183b5e | 15 | |
dicarloj | 2:7312ac02785d | 16 | void handle_index(); |
dicarloj | 2:7312ac02785d | 17 | |
dicarloj | 2:7312ac02785d | 18 | float get_throttle() |
dicarloj | 0:f899a5183b5e | 19 | { |
dicarloj | 2:7312ac02785d | 20 | return p_in.get_throttle(); |
dicarloj | 0:f899a5183b5e | 21 | } |
dicarloj | 0:f899a5183b5e | 22 | |
dicarloj | 0:f899a5183b5e | 23 | void setup() |
dicarloj | 0:f899a5183b5e | 24 | { |
dicarloj | 2:7312ac02785d | 25 | //set serial baud |
dicarloj | 2:7312ac02785d | 26 | get_serial(0)->baud(K_SERIAL_BAUD); |
dicarloj | 0:f899a5183b5e | 27 | |
dicarloj | 4:4e00b310811d | 28 | //get_serial(0)->printf("GEORGE\n\r"); |
dicarloj | 4:4e00b310811d | 29 | //get_serial(0)->printf("*******INITIALIZING**********\n\r"); |
dicarloj | 0:f899a5183b5e | 30 | |
dicarloj | 4:4e00b310811d | 31 | //get_serial(0)->printf("Setting up clocks...\n\r"); |
dicarloj | 0:f899a5183b5e | 32 | /************************************* |
dicarloj | 0:f899a5183b5e | 33 | CLOCKS |
dicarloj | 0:f899a5183b5e | 34 | **************************************/ |
dicarloj | 0:f899a5183b5e | 35 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; //gpio a |
dicarloj | 0:f899a5183b5e | 36 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; //gpio b |
dicarloj | 0:f899a5183b5e | 37 | RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; //gpio c |
dicarloj | 0:f899a5183b5e | 38 | RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //tim1 |
dicarloj | 0:f899a5183b5e | 39 | RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; //adc1 |
dicarloj | 0:f899a5183b5e | 40 | RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; //adc2 |
dicarloj | 0:f899a5183b5e | 41 | RCC->APB1ENR |= RCC_APB1ENR_DACEN; //dac |
dicarloj | 0:f899a5183b5e | 42 | |
dicarloj | 4:4e00b310811d | 43 | //get_serial(0)->printf("Setting up interrupts...\n\r"); |
dicarloj | 0:f899a5183b5e | 44 | /************************************* |
dicarloj | 0:f899a5183b5e | 45 | INTERRUPTS |
dicarloj | 0:f899a5183b5e | 46 | **************************************/ |
dicarloj | 0:f899a5183b5e | 47 | NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //setup interrupt controller |
dicarloj | 0:f899a5183b5e | 48 | TIM1->DIER |= TIM_DIER_UIE; //enable timer interrupts |
dicarloj | 0:f899a5183b5e | 49 | |
dicarloj | 4:4e00b310811d | 50 | // |
dicarloj | 4:4e00b310811d | 51 | |
dicarloj | 4:4e00b310811d | 52 | |
dicarloj | 4:4e00b310811d | 53 | //get_serial(0)->printf("Setting up PWM...\n\r"); |
dicarloj | 0:f899a5183b5e | 54 | /************************************ |
dicarloj | 0:f899a5183b5e | 55 | PWM CONFIGURATION |
dicarloj | 0:f899a5183b5e | 56 | *************************************/ |
dicarloj | 0:f899a5183b5e | 57 | //set center aligned mode bits to 10 (might be 01 though). |
dicarloj | 2:7312ac02785d | 58 | TIM1->CR1 |= TIM_CR1_CMS_1; //counts up and down, but only interrupts when counting up. |
dicarloj | 2:7312ac02785d | 59 | TIM1->CR1 = 0x40; |
dicarloj | 2:7312ac02785d | 60 | TIM1->CR1 |= TIM_CR1_ARPE; |
dicarloj | 2:7312ac02785d | 61 | TIM1->RCR |= 0x01; //repetition counter set for 2 repetitions |
dicarloj | 2:7312ac02785d | 62 | TIM1->EGR |= TIM_EGR_UG; //reference manual says I need to go this. |
dicarloj | 0:f899a5183b5e | 63 | //enable and set polarity of output compares (using 1, 2, 3) |
dicarloj | 0:f899a5183b5e | 64 | TIM1->CCER |= TIM_CCER_CC3E; |
dicarloj | 0:f899a5183b5e | 65 | TIM1->CCER |= TIM_CCER_CC3P; |
dicarloj | 0:f899a5183b5e | 66 | TIM1->CCER |= TIM_CCER_CC2E; |
dicarloj | 0:f899a5183b5e | 67 | TIM1->CCER |= TIM_CCER_CC2P; |
dicarloj | 0:f899a5183b5e | 68 | TIM1->CCER |= TIM_CCER_CC1E; |
dicarloj | 0:f899a5183b5e | 69 | TIM1->CCER |= TIM_CCER_CC1P; |
dicarloj | 0:f899a5183b5e | 70 | |
dicarloj | 4:4e00b310811d | 71 | //get_serial(0)->printf("Setting up Timers...\n\r"); |
dicarloj | 0:f899a5183b5e | 72 | /************************************ |
dicarloj | 0:f899a5183b5e | 73 | TIMER CONFIGURATION |
dicarloj | 0:f899a5183b5e | 74 | *************************************/ |
dicarloj | 0:f899a5183b5e | 75 | //buffer the auto reload register (ARR) to update only on update event |
dicarloj | 2:7312ac02785d | 76 | |
dicarloj | 0:f899a5183b5e | 77 | TIM1->PSC = 0x0; //no prescaler. |
dicarloj | 2:7312ac02785d | 78 | //set counter maximum to value for 5 kHz PWM |
dicarloj | 2:7312ac02785d | 79 | TIM1->ARR = 0x4650; //5khz for prius inverter |
dicarloj | 0:f899a5183b5e | 80 | //counter needs to do two cycles for 1 full pwm cycle I think. |
dicarloj | 0:f899a5183b5e | 81 | //this is due to the center aligned mode. |
dicarloj | 0:f899a5183b5e | 82 | |
dicarloj | 4:4e00b310811d | 83 | //get_serial(0)->printf("Setting up ADC...\n"); |
dicarloj | 0:f899a5183b5e | 84 | /************************************ |
dicarloj | 0:f899a5183b5e | 85 | ADC CONFIG |
dicarloj | 0:f899a5183b5e | 86 | *************************************/ |
dicarloj | 0:f899a5183b5e | 87 | ADC->CCR |= 0b110; //set adc 1, 2 to sample together. |
dicarloj | 0:f899a5183b5e | 88 | ADC1->SQR3 |= 0x4; //on ADC1, sample ADC12_IN channel 4 (PA4) |
dicarloj | 0:f899a5183b5e | 89 | ADC2->SQR3 |= 0x8; //on ADC2, sample ADC12_IN channel 8 (PB0) |
dicarloj | 0:f899a5183b5e | 90 | |
dicarloj | 4:4e00b310811d | 91 | //get_serial(0)->printf("Setting up GPIO...\n"); |
dicarloj | 0:f899a5183b5e | 92 | /************************************ |
dicarloj | 0:f899a5183b5e | 93 | GPIO CONFIG |
dicarloj | 0:f899a5183b5e | 94 | *************************************/ |
dicarloj | 0:f899a5183b5e | 95 | //pwm outs |
dicarloj | 0:f899a5183b5e | 96 | //0x2A0000 = 0b 10 10 10 00 00 00 00 00 00 00 00 |
dicarloj | 2:7312ac02785d | 97 | GPIOA->MODER |= 0x2AA000; |
dicarloj | 0:f899a5183b5e | 98 | // 109876543210 |
dicarloj | 0:f899a5183b5e | 99 | GPIOA->MODER |= 0b001100000000; //PA4 as analog. |
dicarloj | 0:f899a5183b5e | 100 | GPIOB->MODER |= 0b000000000011; //PB0 as analog. |
dicarloj | 0:f899a5183b5e | 101 | GPIOA->MODER |= 0b110000000000; //PA5 as analog. |
dicarloj | 0:f899a5183b5e | 102 | |
dicarloj | 4:4e00b310811d | 103 | //get_serial(0)->printf("Turning on ADC/DAC...\n"); |
dicarloj | 0:f899a5183b5e | 104 | /************************************ |
dicarloj | 0:f899a5183b5e | 105 | ADC/DAC START |
dicarloj | 0:f899a5183b5e | 106 | *************************************/ |
dicarloj | 0:f899a5183b5e | 107 | ADC1->CR2 |= ADC_CR2_ADON; //adc 1 on |
dicarloj | 0:f899a5183b5e | 108 | ADC2->CR2 |= ADC_CR2_ADON; //adc 2 on |
dicarloj | 0:f899a5183b5e | 109 | DAC->CR |= DAC_CR_EN2; //dac 2 (PA5) on |
dicarloj | 0:f899a5183b5e | 110 | |
dicarloj | 4:4e00b310811d | 111 | //get_serial(0)->printf("Turning on TIMER\n"); |
dicarloj | 0:f899a5183b5e | 112 | /************************************ |
dicarloj | 0:f899a5183b5e | 113 | TIMER START |
dicarloj | 0:f899a5183b5e | 114 | *************************************/ |
dicarloj | 0:f899a5183b5e | 115 | TIM1->CR1 |= TIM_CR1_CEN; //enable! |
dicarloj | 0:f899a5183b5e | 116 | wait_us(100000); |
dicarloj | 0:f899a5183b5e | 117 | |
dicarloj | 0:f899a5183b5e | 118 | /************************************ |
dicarloj | 2:7312ac02785d | 119 | INIT CONTROL |
dicarloj | 0:f899a5183b5e | 120 | *************************************/ |
dicarloj | 2:7312ac02785d | 121 | init_control(); |
dicarloj | 0:f899a5183b5e | 122 | /************************************ |
dicarloj | 0:f899a5183b5e | 123 | ENCODER |
dicarloj | 0:f899a5183b5e | 124 | *************************************/ |
dicarloj | 2:7312ac02785d | 125 | //from ben's encoder code |
dicarloj | 2:7312ac02785d | 126 | GPIOB->MODER |= GPIO_MODER_MODER3_1; |
dicarloj | 2:7312ac02785d | 127 | GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ; |
dicarloj | 2:7312ac02785d | 128 | GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ; |
dicarloj | 2:7312ac02785d | 129 | GPIOB->AFR[0] |= 0x00001000 ; |
dicarloj | 0:f899a5183b5e | 130 | |
dicarloj | 2:7312ac02785d | 131 | GPIOA->MODER |= GPIO_MODER_MODER15_1; |
dicarloj | 2:7312ac02785d | 132 | GPIOA->OTYPER |= GPIO_OTYPER_OT_15; |
dicarloj | 2:7312ac02785d | 133 | GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15; |
dicarloj | 2:7312ac02785d | 134 | GPIOA->AFR[1] |= 0x10000000 ; |
dicarloj | 2:7312ac02785d | 135 | |
dicarloj | 2:7312ac02785d | 136 | __TIM2_CLK_ENABLE(); |
dicarloj | 2:7312ac02785d | 137 | |
dicarloj | 2:7312ac02785d | 138 | TIM2->CR1 = 0x0001; |
dicarloj | 2:7312ac02785d | 139 | TIM2->SMCR = TIM_ENCODERMODE_TI12; |
dicarloj | 2:7312ac02785d | 140 | TIM2->CCMR1 = 0xf1f1; |
dicarloj | 2:7312ac02785d | 141 | TIM2->CCMR2 = 0x0000; |
dicarloj | 2:7312ac02785d | 142 | TIM2->CCER = 0x0011; |
dicarloj | 2:7312ac02785d | 143 | TIM2->PSC = 0x0000; |
dicarloj | 2:7312ac02785d | 144 | TIM2->ARR = 0xffffffff; |
dicarloj | 2:7312ac02785d | 145 | |
dicarloj | 2:7312ac02785d | 146 | TIM2->CNT = 0; |
dicarloj | 2:7312ac02785d | 147 | index_interrupt = new InterruptIn(PB_12); |
dicarloj | 2:7312ac02785d | 148 | index_in = new DigitalIn(PB_12); |
dicarloj | 2:7312ac02785d | 149 | index_interrupt-> enable_irq(); |
dicarloj | 2:7312ac02785d | 150 | index_interrupt-> rise(&handle_index); |
dicarloj | 2:7312ac02785d | 151 | index_interrupt-> mode(PullDown); |
dicarloj | 2:7312ac02785d | 152 | |
dicarloj | 0:f899a5183b5e | 153 | } |
dicarloj | 0:f899a5183b5e | 154 | |
dicarloj | 2:7312ac02785d | 155 | Serial *get_serial(int port) |
dicarloj | 0:f899a5183b5e | 156 | { |
dicarloj | 0:f899a5183b5e | 157 | switch(port) |
dicarloj | 0:f899a5183b5e | 158 | { |
dicarloj | 0:f899a5183b5e | 159 | case 0: |
dicarloj | 0:f899a5183b5e | 160 | return &pc_ser; |
dicarloj | 0:f899a5183b5e | 161 | case 1: |
dicarloj | 2:7312ac02785d | 162 | //return &debug_ser; |
dicarloj | 0:f899a5183b5e | 163 | case 2: |
dicarloj | 2:7312ac02785d | 164 | // return &panel_ser; |
dicarloj | 2:7312ac02785d | 165 | break; |
dicarloj | 0:f899a5183b5e | 166 | } |
dicarloj | 0:f899a5183b5e | 167 | return &pc_ser; |
dicarloj | 0:f899a5183b5e | 168 | } |
dicarloj | 0:f899a5183b5e | 169 | |
dicarloj | 2:7312ac02785d | 170 | //timer interrupt handler |
dicarloj | 0:f899a5183b5e | 171 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) |
dicarloj | 0:f899a5183b5e | 172 | { |
dicarloj | 2:7312ac02785d | 173 | //check to see if timer1 caused the interrupt |
dicarloj | 0:f899a5183b5e | 174 | if(TIM1->SR & TIM_SR_UIF) { |
dicarloj | 2:7312ac02785d | 175 | //togle debugging pin |
dicarloj | 2:7312ac02785d | 176 | dout = !dout; |
dicarloj | 2:7312ac02785d | 177 | //run control loop |
dicarloj | 3:08746709f023 | 178 | motor_control(&p_in); |
dicarloj | 0:f899a5183b5e | 179 | } |
dicarloj | 2:7312ac02785d | 180 | //reset timer 1 interrupt flag |
dicarloj | 0:f899a5183b5e | 181 | TIM1->SR = 0; |
dicarloj | 0:f899a5183b5e | 182 | } |
dicarloj | 0:f899a5183b5e | 183 | |
dicarloj | 2:7312ac02785d | 184 | //convert phase voltage to timer ticks |
dicarloj | 1:2acd7dfc4b1b | 185 | uint32_t volts_to_ticks(float volts) |
dicarloj | 1:2acd7dfc4b1b | 186 | { |
dicarloj | 1:2acd7dfc4b1b | 187 | float duty_cycle = volts/K_V_BUS + .5f; |
dicarloj | 2:7312ac02785d | 188 | //for prius brick, we don't need to limit duty cycle! |
dicarloj | 2:7312ac02785d | 189 | //duty_cycle = fmaxf(.04f, duty_cycle); |
dicarloj | 2:7312ac02785d | 190 | //duty_cycle = fminf(.96f, duty_cycle); |
dicarloj | 2:7312ac02785d | 191 | return (uint32_t)(duty_cycle * 0x4650); |
dicarloj | 1:2acd7dfc4b1b | 192 | } |
dicarloj | 1:2acd7dfc4b1b | 193 | |
dicarloj | 2:7312ac02785d | 194 | void set_inverter(float v_a, float v_b, float v_c) |
dicarloj | 0:f899a5183b5e | 195 | { |
dicarloj | 2:7312ac02785d | 196 | TIM1->CCR1 = volts_to_ticks(v_a); |
dicarloj | 2:7312ac02785d | 197 | TIM1->CCR2 = volts_to_ticks(v_b); |
dicarloj | 2:7312ac02785d | 198 | TIM1->CCR3 = volts_to_ticks(v_c); |
dicarloj | 1:2acd7dfc4b1b | 199 | } |
dicarloj | 1:2acd7dfc4b1b | 200 | |
dicarloj | 1:2acd7dfc4b1b | 201 | void ser_send(Serial *ser, const void* start, size_t size) |
dicarloj | 1:2acd7dfc4b1b | 202 | { |
dicarloj | 1:2acd7dfc4b1b | 203 | const char* data_ptr = (const char*)start; //ptr to beginning of buffer |
dicarloj | 1:2acd7dfc4b1b | 204 | const char* end = data_ptr + size; //ptr to end of buffer |
dicarloj | 1:2acd7dfc4b1b | 205 | while (data_ptr != end) //if we haven't reached the end |
dicarloj | 1:2acd7dfc4b1b | 206 | { |
dicarloj | 2:7312ac02785d | 207 | //if(ser->writable()) |
dicarloj | 2:7312ac02785d | 208 | if(true) |
dicarloj | 2:7312ac02785d | 209 | ser->putc(*data_ptr++);//go write to the serial port and move the buffer ptr forward |
dicarloj | 2:7312ac02785d | 210 | else |
dicarloj | 2:7312ac02785d | 211 | return; //need to go implement return codes. |
dicarloj | 1:2acd7dfc4b1b | 212 | } |
dicarloj | 1:2acd7dfc4b1b | 213 | } |
dicarloj | 1:2acd7dfc4b1b | 214 | |
dicarloj | 1:2acd7dfc4b1b | 215 | int ser_read(Serial *ser, void* start, size_t size) |
dicarloj | 1:2acd7dfc4b1b | 216 | { |
dicarloj | 1:2acd7dfc4b1b | 217 | unsigned char* b_ptr = (unsigned char*)start; //start of buffer ptr |
dicarloj | 1:2acd7dfc4b1b | 218 | for(int i = 0; i < size; i++) //loop through the buffer |
dicarloj | 1:2acd7dfc4b1b | 219 | { |
dicarloj | 1:2acd7dfc4b1b | 220 | if(ser->readable()) (*b_ptr++) = ser->getc(); //if there's something to read, read and advance buffer ptr |
dicarloj | 1:2acd7dfc4b1b | 221 | else return i; //otherwise, break from loop and return bytes read. |
dicarloj | 1:2acd7dfc4b1b | 222 | } |
dicarloj | 1:2acd7dfc4b1b | 223 | return size; //we finished the loop so buffer is full, all bytes read. |
dicarloj | 2:7312ac02785d | 224 | } |
dicarloj | 2:7312ac02785d | 225 | |
dicarloj | 2:7312ac02785d | 226 | //interrupt handler for encoder index |
dicarloj | 2:7312ac02785d | 227 | void handle_index() |
dicarloj | 2:7312ac02785d | 228 | { |
dicarloj | 2:7312ac02785d | 229 | if (index_in->read()) |
dicarloj | 2:7312ac02785d | 230 | { |
dicarloj | 2:7312ac02785d | 231 | if(index_in->read()) |
dicarloj | 2:7312ac02785d | 232 | TIM2->CNT=0; |
dicarloj | 2:7312ac02785d | 233 | } |
dicarloj | 2:7312ac02785d | 234 | } |
dicarloj | 2:7312ac02785d | 235 | |
dicarloj | 2:7312ac02785d | 236 | float get_position() |
dicarloj | 2:7312ac02785d | 237 | { |
dicarloj | 2:7312ac02785d | 238 | int raw = TIM2->CNT; |
dicarloj | 2:7312ac02785d | 239 | if (raw < 0) raw += CPR; |
dicarloj | 2:7312ac02785d | 240 | if (raw >= CPR) raw -= CPR; |
dicarloj | 2:7312ac02785d | 241 | float signed_elec = fmod((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)CPR)), 6.28318530718f); |
dicarloj | 2:7312ac02785d | 242 | if (signed_elec < 0) |
dicarloj | 2:7312ac02785d | 243 | { |
dicarloj | 2:7312ac02785d | 244 | return signed_elec + 6.28318530718f; |
dicarloj | 2:7312ac02785d | 245 | } |
dicarloj | 2:7312ac02785d | 246 | else |
dicarloj | 2:7312ac02785d | 247 | { |
dicarloj | 2:7312ac02785d | 248 | return signed_elec; |
dicarloj | 2:7312ac02785d | 249 | } |
dicarloj | 2:7312ac02785d | 250 | } |
dicarloj | 2:7312ac02785d | 251 | |
dicarloj | 2:7312ac02785d | 252 | |
dicarloj | 2:7312ac02785d | 253 | float position_last; |
dicarloj | 2:7312ac02785d | 254 | Timer vel_timer; |
dicarloj | 2:7312ac02785d | 255 | |
dicarloj | 2:7312ac02785d | 256 | float get_velocity() |
dicarloj | 2:7312ac02785d | 257 | { |
dicarloj | 2:7312ac02785d | 258 | int usecs = vel_timer.read_us(); |
dicarloj | 2:7312ac02785d | 259 | float dx = get_position() - position_last; |
dicarloj | 2:7312ac02785d | 260 | if(dx > M_PI) dx -= 2 * M_PI; |
dicarloj | 2:7312ac02785d | 261 | if(dx < -M_PI) dx += 2 * M_PI; |
dicarloj | 2:7312ac02785d | 262 | vel_timer.stop(); |
dicarloj | 2:7312ac02785d | 263 | vel_timer.reset(); |
dicarloj | 2:7312ac02785d | 264 | vel_timer.start(); |
dicarloj | 2:7312ac02785d | 265 | position_last = get_position(); |
dicarloj | 2:7312ac02785d | 266 | return 1000000*dx/usecs; |
dicarloj | 0:f899a5183b5e | 267 | } |