Jared DiCarlo / Mbed 2 deprecated george

motor controller

Dependencies:   mbed plotter

io.cpp@7:ed19a937daa0, 2016-11-06 (annotated)

Committer:
dicarloj
Date:
Sun Nov 06 03:09:28 2016 +0000
Revision:
7:ed19a937daa0
Parent:
4:4e00b310811d 
works at 200V

Who changed what in which revision?

UserRevisionLine numberNew 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 7:ed19a937daa0 107 // RCC->APB1ENR |= 0x20000000; // Enable clock for DAC
dicarloj 7:ed19a937daa0 108 // DAC->CR |= 0x00000001; // DAC control reg, both channels ON
dicarloj 7:ed19a937daa0 109 GPIOA->MODER |= 0x00000300; // PA04 as analog output
dicarloj 0:f899a5183b5e 110 ADC1->CR2 |= ADC_CR2_ADON; //adc 1 on
dicarloj 0:f899a5183b5e 111 ADC2->CR2 |= ADC_CR2_ADON; //adc 2 on
dicarloj 7:ed19a937daa0 112 //DAC->CR |= DAC_CR_EN2; //dac 2 (PA5) on
dicarloj 0:f899a5183b5e 113
dicarloj 4:4e00b310811d 114 //get_serial(0)->printf("Turning on TIMER\n");
dicarloj 0:f899a5183b5e 115 /************************************
dicarloj 0:f899a5183b5e 116 TIMER START
dicarloj 0:f899a5183b5e 117 *************************************/
dicarloj 0:f899a5183b5e 118 TIM1->CR1 |= TIM_CR1_CEN; //enable!
dicarloj 0:f899a5183b5e 119 wait_us(100000);
dicarloj 0:f899a5183b5e 120
dicarloj 0:f899a5183b5e 121 /************************************
dicarloj 2:7312ac02785d 122 INIT CONTROL
dicarloj 0:f899a5183b5e 123 *************************************/
dicarloj 2:7312ac02785d 124 init_control();
dicarloj 0:f899a5183b5e 125 /************************************
dicarloj 0:f899a5183b5e 126 ENCODER
dicarloj 0:f899a5183b5e 127 *************************************/
dicarloj 2:7312ac02785d 128 //from ben's encoder code
dicarloj 2:7312ac02785d 129 GPIOB->MODER |= GPIO_MODER_MODER3_1;
dicarloj 2:7312ac02785d 130 GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ;
dicarloj 2:7312ac02785d 131 GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ;
dicarloj 2:7312ac02785d 132 GPIOB->AFR[0] |= 0x00001000 ;
dicarloj 0:f899a5183b5e 133
dicarloj 2:7312ac02785d 134 GPIOA->MODER |= GPIO_MODER_MODER15_1;
dicarloj 2:7312ac02785d 135 GPIOA->OTYPER |= GPIO_OTYPER_OT_15;
dicarloj 2:7312ac02785d 136 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15;
dicarloj 2:7312ac02785d 137 GPIOA->AFR[1] |= 0x10000000 ;
dicarloj 2:7312ac02785d 138
dicarloj 2:7312ac02785d 139 __TIM2_CLK_ENABLE();
dicarloj 2:7312ac02785d 140
dicarloj 2:7312ac02785d 141 TIM2->CR1 = 0x0001;
dicarloj 2:7312ac02785d 142 TIM2->SMCR = TIM_ENCODERMODE_TI12;
dicarloj 7:ed19a937daa0 143 TIM2->CCMR1 = 0x0101;
dicarloj 2:7312ac02785d 144 TIM2->CCMR2 = 0x0000;
dicarloj 2:7312ac02785d 145 TIM2->CCER = 0x0011;
dicarloj 2:7312ac02785d 146 TIM2->PSC = 0x0000;
dicarloj 2:7312ac02785d 147 TIM2->ARR = 0xffffffff;
dicarloj 2:7312ac02785d 148
dicarloj 2:7312ac02785d 149 TIM2->CNT = 0;
dicarloj 2:7312ac02785d 150 index_interrupt = new InterruptIn(PB_12);
dicarloj 2:7312ac02785d 151 index_in = new DigitalIn(PB_12);
dicarloj 2:7312ac02785d 152 index_interrupt-> enable_irq();
dicarloj 2:7312ac02785d 153 index_interrupt-> rise(&handle_index);
dicarloj 2:7312ac02785d 154 index_interrupt-> mode(PullDown);
dicarloj 2:7312ac02785d 155
dicarloj 0:f899a5183b5e 156 }
dicarloj 0:f899a5183b5e 157
dicarloj 2:7312ac02785d 158 Serial *get_serial(int port)
dicarloj 0:f899a5183b5e 159 {
dicarloj 0:f899a5183b5e 160 switch(port)
dicarloj 0:f899a5183b5e 161 {
dicarloj 0:f899a5183b5e 162 case 0:
dicarloj 0:f899a5183b5e 163 return &pc_ser;
dicarloj 0:f899a5183b5e 164 case 1:
dicarloj 2:7312ac02785d 165 //return &debug_ser;
dicarloj 0:f899a5183b5e 166 case 2:
dicarloj 2:7312ac02785d 167 // return &panel_ser;
dicarloj 2:7312ac02785d 168 break;
dicarloj 0:f899a5183b5e 169 }
dicarloj 0:f899a5183b5e 170 return &pc_ser;
dicarloj 0:f899a5183b5e 171 }
dicarloj 0:f899a5183b5e 172
dicarloj 2:7312ac02785d 173 //timer interrupt handler
dicarloj 0:f899a5183b5e 174 extern "C" void TIM1_UP_TIM10_IRQHandler(void)
dicarloj 0:f899a5183b5e 175 {
dicarloj 2:7312ac02785d 176 //check to see if timer1 caused the interrupt
dicarloj 0:f899a5183b5e 177 if(TIM1->SR & TIM_SR_UIF) {
dicarloj 2:7312ac02785d 178 //togle debugging pin
dicarloj 2:7312ac02785d 179 dout = !dout;
dicarloj 2:7312ac02785d 180 //run control loop
dicarloj 3:08746709f023 181 motor_control(&p_in);
dicarloj 0:f899a5183b5e 182 }
dicarloj 2:7312ac02785d 183 //reset timer 1 interrupt flag
dicarloj 0:f899a5183b5e 184 TIM1->SR = 0;
dicarloj 0:f899a5183b5e 185 }
dicarloj 0:f899a5183b5e 186
dicarloj 2:7312ac02785d 187 //convert phase voltage to timer ticks
dicarloj 1:2acd7dfc4b1b 188 uint32_t volts_to_ticks(float volts)
dicarloj 1:2acd7dfc4b1b 189 {
dicarloj 1:2acd7dfc4b1b 190 float duty_cycle = volts/K_V_BUS + .5f;
dicarloj 2:7312ac02785d 191 //for prius brick, we don't need to limit duty cycle!
dicarloj 2:7312ac02785d 192 //duty_cycle = fmaxf(.04f, duty_cycle);
dicarloj 2:7312ac02785d 193 //duty_cycle = fminf(.96f, duty_cycle);
dicarloj 2:7312ac02785d 194 return (uint32_t)(duty_cycle * 0x4650);
dicarloj 1:2acd7dfc4b1b 195 }
dicarloj 1:2acd7dfc4b1b 196
dicarloj 2:7312ac02785d 197 void set_inverter(float v_a, float v_b, float v_c)
dicarloj 0:f899a5183b5e 198 {
dicarloj 2:7312ac02785d 199 TIM1->CCR1 = volts_to_ticks(v_a);
dicarloj 2:7312ac02785d 200 TIM1->CCR2 = volts_to_ticks(v_b);
dicarloj 2:7312ac02785d 201 TIM1->CCR3 = volts_to_ticks(v_c);
dicarloj 1:2acd7dfc4b1b 202 }
dicarloj 1:2acd7dfc4b1b 203
dicarloj 1:2acd7dfc4b1b 204 void ser_send(Serial *ser, const void* start, size_t size)
dicarloj 1:2acd7dfc4b1b 205 {
dicarloj 1:2acd7dfc4b1b 206 const char* data_ptr = (const char*)start; //ptr to beginning of buffer
dicarloj 1:2acd7dfc4b1b 207 const char* end = data_ptr + size; //ptr to end of buffer
dicarloj 1:2acd7dfc4b1b 208 while (data_ptr != end) //if we haven't reached the end
dicarloj 1:2acd7dfc4b1b 209 {
dicarloj 2:7312ac02785d 210 //if(ser->writable())
dicarloj 2:7312ac02785d 211 if(true)
dicarloj 2:7312ac02785d 212 ser->putc(*data_ptr++);//go write to the serial port and move the buffer ptr forward
dicarloj 2:7312ac02785d 213 else
dicarloj 2:7312ac02785d 214 return; //need to go implement return codes.
dicarloj 1:2acd7dfc4b1b 215 }
dicarloj 1:2acd7dfc4b1b 216 }
dicarloj 1:2acd7dfc4b1b 217
dicarloj 1:2acd7dfc4b1b 218 int ser_read(Serial *ser, void* start, size_t size)
dicarloj 1:2acd7dfc4b1b 219 {
dicarloj 1:2acd7dfc4b1b 220 unsigned char* b_ptr = (unsigned char*)start; //start of buffer ptr
dicarloj 1:2acd7dfc4b1b 221 for(int i = 0; i < size; i++) //loop through the buffer
dicarloj 1:2acd7dfc4b1b 222 {
dicarloj 1:2acd7dfc4b1b 223 if(ser->readable()) (*b_ptr++) = ser->getc(); //if there's something to read, read and advance buffer ptr
dicarloj 1:2acd7dfc4b1b 224 else return i; //otherwise, break from loop and return bytes read.
dicarloj 1:2acd7dfc4b1b 225 }
dicarloj 1:2acd7dfc4b1b 226 return size; //we finished the loop so buffer is full, all bytes read.
dicarloj 2:7312ac02785d 227 }
dicarloj 2:7312ac02785d 228
dicarloj 7:ed19a937daa0 229
dicarloj 2:7312ac02785d 230 //interrupt handler for encoder index
dicarloj 2:7312ac02785d 231 void handle_index()
dicarloj 2:7312ac02785d 232 {
dicarloj 2:7312ac02785d 233 if (index_in->read())
dicarloj 2:7312ac02785d 234 {
dicarloj 2:7312ac02785d 235 if(index_in->read())
dicarloj 7:ed19a937daa0 236 {
dicarloj 7:ed19a937daa0 237 TIM2->CNT=0;
dicarloj 7:ed19a937daa0 238 }
dicarloj 2:7312ac02785d 239 }
dicarloj 2:7312ac02785d 240 }
dicarloj 2:7312ac02785d 241
dicarloj 2:7312ac02785d 242 float get_position()
dicarloj 2:7312ac02785d 243 {
dicarloj 2:7312ac02785d 244 int raw = TIM2->CNT;
dicarloj 2:7312ac02785d 245 if (raw < 0) raw += CPR;
dicarloj 2:7312ac02785d 246 if (raw >= CPR) raw -= CPR;
dicarloj 2:7312ac02785d 247 float signed_elec = fmod((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)CPR)), 6.28318530718f);
dicarloj 2:7312ac02785d 248 if (signed_elec < 0)
dicarloj 2:7312ac02785d 249 {
dicarloj 2:7312ac02785d 250 return signed_elec + 6.28318530718f;
dicarloj 2:7312ac02785d 251 }
dicarloj 2:7312ac02785d 252 else
dicarloj 2:7312ac02785d 253 {
dicarloj 2:7312ac02785d 254 return signed_elec;
dicarloj 2:7312ac02785d 255 }
dicarloj 2:7312ac02785d 256 }
dicarloj 2:7312ac02785d 257
dicarloj 2:7312ac02785d 258
dicarloj 2:7312ac02785d 259 float position_last;
dicarloj 2:7312ac02785d 260 Timer vel_timer;
dicarloj 2:7312ac02785d 261
dicarloj 2:7312ac02785d 262 float get_velocity()
dicarloj 2:7312ac02785d 263 {
dicarloj 2:7312ac02785d 264 int usecs = vel_timer.read_us();
dicarloj 2:7312ac02785d 265 float dx = get_position() - position_last;
dicarloj 2:7312ac02785d 266 if(dx > M_PI) dx -= 2 * M_PI;
dicarloj 2:7312ac02785d 267 if(dx < -M_PI) dx += 2 * M_PI;
dicarloj 2:7312ac02785d 268 vel_timer.stop();
dicarloj 2:7312ac02785d 269 vel_timer.reset();
dicarloj 2:7312ac02785d 270 vel_timer.start();
dicarloj 2:7312ac02785d 271 position_last = get_position();
dicarloj 2:7312ac02785d 272 return 1000000*dx/usecs;
dicarloj 0:f899a5183b5e 273 }