Jared DiCarlo
motor controller
Diff: io.cpp
- Revision:
- 2:7312ac02785d
- Parent:
- 1:2acd7dfc4b1b
- Child:
- 3:08746709f023
--- a/io.cpp Mon Oct 03 05:57:20 2016 +0000
+++ b/io.cpp Sun Oct 30 19:37:46 2016 +0000
@@ -2,46 +2,33 @@
#include "config.h"
#include "io.h"
#include "foc.h"
-
-Serial pc_ser(USBTX, USBRX);
-Serial debug_ser(PC_10, PC_11);
-Serial panel_ser(PC_6, PC_7);
-
-float tick_const = SystemCoreClock/1000000.0f;
+#include "pwm_in.h"
-volatile bool enabled = false;
-volatile bool error_ = false;
-bool arduino_connected = false;
+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;
-bool getError()
+void handle_index();
+
+float get_throttle()
{
- return error_;
+ return p_in.get_throttle();
}
-void setError()
-{
- error_ = true;
-}
-
-bool getEnabled()
-{
- return enabled;
-}
-
-void setEnabled(bool _enabled)
-{
- enabled = _enabled;
-}
void setup()
{
- getSerial(0)->baud(SERIAL_BAUD); //set serial baud
- getSerial(1)->baud(SERIAL_BAUD); //set serial baud
- getSerial(2)->baud(SERIAL_BAUD); //set serial baud
+ //set serial baud
+ get_serial(0)->baud(K_SERIAL_BAUD);
- getSerial(0)->printf("GEORGE\n");
- getSerial(0)->printf("*******INITIALIZING**********\n");
+ get_serial(0)->printf("GEORGE\n\r");
+ get_serial(0)->printf("*******INITIALIZING**********\n\r");
- getSerial(0)->printf("Setting up clocks...\n");
+ get_serial(0)->printf("Setting up clocks...\n\r");
/*************************************
CLOCKS
**************************************/
@@ -53,20 +40,23 @@
RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; //adc2
RCC->APB1ENR |= RCC_APB1ENR_DACEN; //dac
- getSerial(0)->printf("Setting up interrupts...\n");
+ 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
- getSerial(0)->printf("Setting up PWM...\n");
+ 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->EGR |= TIM_EGR_UG; //reference manual says I need to go this.
+ 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;
@@ -75,20 +65,19 @@
TIM1->CCER |= TIM_CCER_CC1E;
TIM1->CCER |= TIM_CCER_CC1P;
- getSerial(0)->printf("Setting up Timers...\n");
+ get_serial(0)->printf("Setting up Timers...\n\r");
/************************************
TIMER CONFIGURATION
*************************************/
//buffer the auto reload register (ARR) to update only on update event
- TIM1->CR1 |= TIM_CR1_ARPE;
- TIM1->RCR |= 0x01; //repetition counter set for 2 repetitions
+
TIM1->PSC = 0x0; //no prescaler.
- //set counter maximum to value for 10 kHz PWM
- TIM1->ARR = 0x2328; //this is for 20 kHz, which is think is a needed correction.
+ //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.
- getSerial(0)->printf("Setting up ADC...\n");
+ get_serial(0)->printf("Setting up ADC...\n");
/************************************
ADC CONFIG
*************************************/
@@ -96,21 +85,19 @@
ADC1->SQR3 |= 0x4; //on ADC1, sample ADC12_IN channel 4 (PA4)
ADC2->SQR3 |= 0x8; //on ADC2, sample ADC12_IN channel 8 (PB0)
- getSerial(0)->printf("Setting up GPIO...\n");
- getSerial(0)->printf("PWM pins are set up as 'alternate function'...\n");
- getSerial(0)->printf("May eventually need to set speed, pullup...\n");
+ 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 |= 0x2A0000;
+ GPIOA->MODER |= 0x2AA000;
// 109876543210
GPIOA->MODER |= 0b001100000000; //PA4 as analog.
GPIOB->MODER |= 0b000000000011; //PB0 as analog.
GPIOA->MODER |= 0b110000000000; //PA5 as analog.
- getSerial(0)->printf("Turning on ADC/DAC...\n");
+ get_serial(0)->printf("Turning on ADC/DAC...\n");
/************************************
ADC/DAC START
*************************************/
@@ -118,7 +105,7 @@
ADC2->CR2 |= ADC_CR2_ADON; //adc 2 on
DAC->CR |= DAC_CR_EN2; //dac 2 (PA5) on
- getSerial(0)->printf("Turning on TIMER\n");
+ get_serial(0)->printf("Turning on TIMER\n");
/************************************
TIMER START
*************************************/
@@ -126,84 +113,99 @@
wait_us(100000);
/************************************
- ZERO CURRENT
+ 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 ;
- /************************************
- FRONT PANEL BOARD
- *************************************/
- char front_panel_init_value = 236;
- getSerial(0)->printf("Sending init command to arduino..\n");
- ser_send(getSerial(2), &front_panel_init_value, 1);
- wait_us(100000);
- getSerial(0)->printf("Waiting for response...\n");
- char response = 0;
- if(ser_read(getSerial(2), &response, 1) < 1) getSerial(0)->printf("Got nothing from arduino. Is it on?\n");
- if(response == 118)
- {
- getSerial(0)->printf("Got a good response from arduino.\n");
- arduino_connected = true;
- }
- else
- {
- getSerial(0)->printf("Got a bad response from arduino.\n");
- }
-
+ 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 = 0xf1f1;
+ 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 *getSerial(int port)
+Serial *get_serial(int port)
{
switch(port)
{
case 0:
return &pc_ser;
case 1:
- return &debug_ser;
+ //return &debug_ser;
case 2:
- return &panel_ser;
+ // 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) {
- if(getEnabled() && !getError()) sampleCurrent();
- //else go turn off enabled pin.
+ //togle debugging pin
+ dout = !dout;
+ //run control loop
+ motor_control();
}
+ //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;
- duty_cycle = fmaxf(.04f, duty_cycle);
- duty_cycle = fminf(.96f, duty_cycle);
- float width_us = 50.0f*duty_cycle; //this needs to change if I slow down from 20 kHz!
- return(width_us * tick_const + 0.5f); //add .5 to avoid ever setting zero.
+ //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(Inverter* invt)
+void set_inverter(float v_a, float v_b, float v_c)
{
- TIM1->CCR1 = volts_to_ticks(invt->v_a);
- TIM1->CCR2 = volts_to_ticks(invt->v_b);
- TIM1->CCR3 = volts_to_ticks(invt->v_c);
+ TIM1->CCR1 = volts_to_ticks(v_a);
+ TIM1->CCR2 = volts_to_ticks(v_b);
+ TIM1->CCR3 = volts_to_ticks(v_c);
}
-//why doesn't mbed have something like this....
-//not everybody wants asynchronous serial reads/writes and callbacks....
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
{
- ser->putc(*data_ptr++);//go write to the serial port and move the buffer ptr forward
+ //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.
}
}
@@ -216,4 +218,47 @@
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;
}
\ No newline at end of file