A fork of foc-ed_in_the_bot_compact modified to test motors using bayleyw's prius inverter ECU
Fork of foc-ed_in_the_bot_compact by
Diff: main.cpp
- Revision:
- 8:70122bad5f90
- Parent:
- 7:caebf421f288
--- a/main.cpp Wed Mar 30 06:50:11 2016 +0000 +++ b/main.cpp Sat Apr 23 21:29:32 2016 +0000 @@ -9,34 +9,31 @@ FastPWM *b; FastPWM *c; DigitalOut en(EN); -//DigitalOut toggle(PC_10); +DigitalOut toggle(PC_10); PositionSensorEncoder pos(CPR, 0); -Serial pc(USBTX, USBRX); - int state = 0; int adval1, adval2; -float ia, ib, ic, alpha, beta, d, q, vd, vq, p = 0.0f; +float ia, ib, ic, alpha, beta, d, q, vd, vq, p; float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) -float d_integral = 0.0f, q_integral = 0.0f; -float last_d = 0.0f, last_q = 0.0f; -float d_ref = -0.0f, q_ref = 30.0f; +double d_integral = 0.0f, q_integral = 0.0f; +double last_d = 0.0f, last_q = 0.0f; +double d_ref = 0.0f, q_ref = -5.0f; void commutate(); void zero_current(); void config_globals(); -void startup_msg(); extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { - //toggle = 1; + toggle = 1; ADC1->CR2 |= 0x40000000; volatile int delay; for (delay = 0; delay < 35; delay++); - //toggle = 0; + toggle = 0; adval1 = ADC1->DR; adval2 = ADC2->DR; commutate(); @@ -57,9 +54,7 @@ ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET; } -void config_globals() { - pc.baud(115200); - +void config_globals() { //Enable clocks for GPIOs RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; @@ -74,13 +69,14 @@ NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt - TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up + //TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, TIM1->RCR |= 0x01; //update event once per up/down count of tim1 TIM1->EGR |= TIM_EGR_UG; TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock - TIM1->ARR = 0x1770; //15 Khz + TIM1->ARR = 0x2EE0; + //TIM1->ARR = 0x1770; //15 Khz TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on. TIM1->CR1 |= TIM_CR1_CEN; @@ -126,31 +122,16 @@ wait_ms(250); zero_current(); en = 1; -} - -void startup_msg() { - pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A."); - pc.printf("%s\n\r", "====Config Data===="); - pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET); - pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE); - pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE); - pc.printf("Loop KP: %f\n\r", KP); - pc.printf("Loop KI: %f\n\r", KI); - pc.printf("Ia offset: %f mV\n\r", ia_supp_offset); - pc.printf("Ib offset: %f mV\n\r", ib_supp_offset); - pc.printf("\n\r"); -} +} void commutate() { p = pos.GetElecPosition() - POS_OFFSET; - if (p < 0) p += 2 * PI; - if (p > 2 * PI) p -= 2 * PI; float sin_p = sinf(p); float cos_p = cosf(p); - //float pos_dac = 0.8f * p / (2 * PI) + 0.1f; + //float pos_dac = 0.85f * p / (2 * PI) + 0.05f; //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096); ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE; @@ -185,11 +166,11 @@ if (vq < -1.0f) vq = -1.0f; if (vq > 1.0f) vq = 1.0f; - //DAC->DHR12R2 = (unsigned int) (q * 20 + 2048); - //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048); + vd = 0.0f; + vq = 1.0f; - //vd = 0.0f; - //vq = 1.0f; + DAC->DHR12R2 = (unsigned int) (q * 20 + 2048); + //DAC->DHR12R2 = (unsigned int) (vq * 2000 + 2048); float valpha = vd * cos_p - vq * sin_p; float vbeta = vd * sin_p + vq * cos_p; @@ -198,8 +179,6 @@ float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta; float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta; - //DAC->DHR12R2 = (unsigned int) (-va * 1500 + 2048); - set_dtc(a, 0.5f + 0.5f * va); set_dtc(b, 0.5f + 0.5f * vb); set_dtc(c, 0.5f + 0.5f * vc); @@ -207,20 +186,7 @@ int main() { config_globals(); - startup_msg(); for (;;) { - //pc.printf("%f\n\r", p); - //wait_ms(100); - /* - q_ref = 0.0f; - wait(3); - toggle = state; - state = !state; - q_ref = -50.0f; - wait(3); - toggle = state; - state = !state; - */ } }