Jared DiCarlo / Mbed 2 deprecated george

motor controller

Dependencies:   mbed plotter

io.cpp@2:7312ac02785d, 2016-10-30 (annotated)

Committer:
dicarloj
Date:
Sun Oct 30 19:37:46 2016 +0000
Revision:
2:7312ac02785d
Parent:
1:2acd7dfc4b1b
Child:
3:08746709f023
working, no safety

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