![](/media/cache/profiles/916954106a13204fa4909ae2d48e0602.jpg.50x50_q85.jpg)
motor controller
io.cpp
- Committer:
- dicarloj
- Date:
- 2016-11-06
- Revision:
- 7:ed19a937daa0
- Parent:
- 4:4e00b310811d
File content as of revision 7:ed19a937daa0:
#include "mbed.h" #include "config.h" #include "io.h" #include "foc.h" #include "pwm_in.h" Serial pc_ser(USBTX, USBRX); DigitalOut dout(PC_9); PwmOut a(PA_9); PwmOut b(PA_8); PwmOut c(PA_10); PWM_IN p_in(PB_8, 1100, 1900); InterruptIn *index_interrupt; DigitalIn *index_in; void handle_index(); float get_throttle() { return p_in.get_throttle(); } void setup() { //set serial baud get_serial(0)->baud(K_SERIAL_BAUD); //get_serial(0)->printf("GEORGE\n\r"); //get_serial(0)->printf("*******INITIALIZING**********\n\r"); //get_serial(0)->printf("Setting up clocks...\n\r"); /************************************* CLOCKS **************************************/ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; //gpio a RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; //gpio b RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; //gpio c RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //tim1 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; //adc1 RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; //adc2 RCC->APB1ENR |= RCC_APB1ENR_DACEN; //dac //get_serial(0)->printf("Setting up interrupts...\n\r"); /************************************* INTERRUPTS **************************************/ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //setup interrupt controller TIM1->DIER |= TIM_DIER_UIE; //enable timer interrupts // //get_serial(0)->printf("Setting up PWM...\n\r"); /************************************ PWM CONFIGURATION *************************************/ //set center aligned mode bits to 10 (might be 01 though). TIM1->CR1 |= TIM_CR1_CMS_1; //counts up and down, but only interrupts when counting up. TIM1->CR1 = 0x40; TIM1->CR1 |= TIM_CR1_ARPE; TIM1->RCR |= 0x01; //repetition counter set for 2 repetitions TIM1->EGR |= TIM_EGR_UG; //reference manual says I need to go this. //enable and set polarity of output compares (using 1, 2, 3) TIM1->CCER |= TIM_CCER_CC3E; TIM1->CCER |= TIM_CCER_CC3P; TIM1->CCER |= TIM_CCER_CC2E; TIM1->CCER |= TIM_CCER_CC2P; TIM1->CCER |= TIM_CCER_CC1E; TIM1->CCER |= TIM_CCER_CC1P; //get_serial(0)->printf("Setting up Timers...\n\r"); /************************************ TIMER CONFIGURATION *************************************/ //buffer the auto reload register (ARR) to update only on update event TIM1->PSC = 0x0; //no prescaler. //set counter maximum to value for 5 kHz PWM TIM1->ARR = 0x4650; //5khz for prius inverter //counter needs to do two cycles for 1 full pwm cycle I think. //this is due to the center aligned mode. //get_serial(0)->printf("Setting up ADC...\n"); /************************************ ADC CONFIG *************************************/ ADC->CCR |= 0b110; //set adc 1, 2 to sample together. ADC1->SQR3 |= 0x4; //on ADC1, sample ADC12_IN channel 4 (PA4) ADC2->SQR3 |= 0x8; //on ADC2, sample ADC12_IN channel 8 (PB0) //get_serial(0)->printf("Setting up GPIO...\n"); /************************************ GPIO CONFIG *************************************/ //pwm outs //0x2A0000 = 0b 10 10 10 00 00 00 00 00 00 00 00 GPIOA->MODER |= 0x2AA000; // 109876543210 GPIOA->MODER |= 0b001100000000; //PA4 as analog. GPIOB->MODER |= 0b000000000011; //PB0 as analog. GPIOA->MODER |= 0b110000000000; //PA5 as analog. //get_serial(0)->printf("Turning on ADC/DAC...\n"); /************************************ ADC/DAC START *************************************/ // RCC->APB1ENR |= 0x20000000; // Enable clock for DAC // DAC->CR |= 0x00000001; // DAC control reg, both channels ON GPIOA->MODER |= 0x00000300; // PA04 as analog output ADC1->CR2 |= ADC_CR2_ADON; //adc 1 on ADC2->CR2 |= ADC_CR2_ADON; //adc 2 on //DAC->CR |= DAC_CR_EN2; //dac 2 (PA5) on //get_serial(0)->printf("Turning on TIMER\n"); /************************************ TIMER START *************************************/ TIM1->CR1 |= TIM_CR1_CEN; //enable! wait_us(100000); /************************************ INIT CONTROL *************************************/ init_control(); /************************************ ENCODER *************************************/ //from ben's encoder code GPIOB->MODER |= GPIO_MODER_MODER3_1; GPIOB->OTYPER |= GPIO_OTYPER_OT_3 | GPIO_OTYPER_OT_10 ; GPIOB->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR3 | GPIO_OSPEEDER_OSPEEDR10 ; GPIOB->AFR[0] |= 0x00001000 ; GPIOA->MODER |= GPIO_MODER_MODER15_1; GPIOA->OTYPER |= GPIO_OTYPER_OT_15; GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR15; GPIOA->AFR[1] |= 0x10000000 ; __TIM2_CLK_ENABLE(); TIM2->CR1 = 0x0001; TIM2->SMCR = TIM_ENCODERMODE_TI12; TIM2->CCMR1 = 0x0101; TIM2->CCMR2 = 0x0000; TIM2->CCER = 0x0011; TIM2->PSC = 0x0000; TIM2->ARR = 0xffffffff; TIM2->CNT = 0; index_interrupt = new InterruptIn(PB_12); index_in = new DigitalIn(PB_12); index_interrupt-> enable_irq(); index_interrupt-> rise(&handle_index); index_interrupt-> mode(PullDown); } Serial *get_serial(int port) { switch(port) { case 0: return &pc_ser; case 1: //return &debug_ser; case 2: // return &panel_ser; break; } return &pc_ser; } //timer interrupt handler extern "C" void TIM1_UP_TIM10_IRQHandler(void) { //check to see if timer1 caused the interrupt if(TIM1->SR & TIM_SR_UIF) { //togle debugging pin dout = !dout; //run control loop motor_control(&p_in); } //reset timer 1 interrupt flag TIM1->SR = 0; } //convert phase voltage to timer ticks uint32_t volts_to_ticks(float volts) { float duty_cycle = volts/K_V_BUS + .5f; //for prius brick, we don't need to limit duty cycle! //duty_cycle = fmaxf(.04f, duty_cycle); //duty_cycle = fminf(.96f, duty_cycle); return (uint32_t)(duty_cycle * 0x4650); } void set_inverter(float v_a, float v_b, float v_c) { TIM1->CCR1 = volts_to_ticks(v_a); TIM1->CCR2 = volts_to_ticks(v_b); TIM1->CCR3 = volts_to_ticks(v_c); } void ser_send(Serial *ser, const void* start, size_t size) { const char* data_ptr = (const char*)start; //ptr to beginning of buffer const char* end = data_ptr + size; //ptr to end of buffer while (data_ptr != end) //if we haven't reached the end { //if(ser->writable()) if(true) ser->putc(*data_ptr++);//go write to the serial port and move the buffer ptr forward else return; //need to go implement return codes. } } int ser_read(Serial *ser, void* start, size_t size) { unsigned char* b_ptr = (unsigned char*)start; //start of buffer ptr for(int i = 0; i < size; i++) //loop through the buffer { if(ser->readable()) (*b_ptr++) = ser->getc(); //if there's something to read, read and advance buffer ptr else return i; //otherwise, break from loop and return bytes read. } return size; //we finished the loop so buffer is full, all bytes read. } //interrupt handler for encoder index void handle_index() { if (index_in->read()) { if(index_in->read()) { TIM2->CNT=0; } } } float get_position() { int raw = TIM2->CNT; if (raw < 0) raw += CPR; if (raw >= CPR) raw -= CPR; float signed_elec = fmod((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)CPR)), 6.28318530718f); if (signed_elec < 0) { return signed_elec + 6.28318530718f; } else { return signed_elec; } } float position_last; Timer vel_timer; float get_velocity() { int usecs = vel_timer.read_us(); float dx = get_position() - position_last; if(dx > M_PI) dx -= 2 * M_PI; if(dx < -M_PI) dx += 2 * M_PI; vel_timer.stop(); vel_timer.reset(); vel_timer.start(); position_last = get_position(); return 1000000*dx/usecs; }