Modified Motor Driver Firmware to include Flash + Thermal
Dependencies: FastPWM3 mbed-dev-STM-lean
Diff: main.cpp
- Revision:
- 22:60276ba87ac6
- Parent:
- 20:bf9ea5125d52
- Child:
- 23:2adf23ee0305
diff -r 7d1f0a206668 -r 60276ba87ac6 main.cpp --- a/main.cpp Thu Mar 02 15:31:45 2017 +0000 +++ b/main.cpp Fri Mar 31 18:24:46 2017 +0000 @@ -1,3 +1,11 @@ +/// high-bandwidth 3-phase motor control, for robots +/// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others +/// Hardware documentation can be found at build-its.blogspot.com + +/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling + + + const unsigned int BOARDNUM = 0x2; //const unsigned int a_id = @@ -13,6 +21,7 @@ #include "PositionSensor.h" #include "structs.h" #include "foc.h" +#include "calibration.h" #include "hw_setup.h" #include "math_ops.h" #include "current_controller_config.h" @@ -22,6 +31,7 @@ GPIOStruct gpio; ControllerStruct controller; COMStruct com; +VelocityEstimatorStruct velocity; @@ -79,126 +89,141 @@ Serial pc(PA_2, PA_3); -PositionSensorAM5147 spi(16384, 4.7, NPP); ///1 I really need an eeprom or something to store this.... -//PositionSensorEncoder encoder(4096, 0, 21); +PositionSensorAM5147 spi(16384, -0.4658, NPP); ///1 I really need an eeprom or something to store this.... +PositionSensorEncoder encoder(4096, 0, 21); int count = 0; -void commutate(void){ - - count ++; +int mode = 0; - //pc.printf("%f\n\r", controller.theta_elec); - //Get rotor angle - //spi.GetMechPosition(); - controller.i_b = I_SCALE*(float)(controller.adc2_raw - controller.adc2_offset); //Calculate phase currents from ADC readings - controller.i_c = I_SCALE*(float)(controller.adc1_raw - controller.adc1_offset); - controller.i_a = -controller.i_b - controller.i_c; - - - dq0(controller.theta_elec, controller.i_a, controller.i_b, controller.i_c, &controller.i_d, &controller.i_q); //dq0 transform on currents - - ///Control Law/// - float i_d_error = controller.i_d_ref - controller.i_d; - float i_q_error = controller.i_q_ref - controller.i_q; - float v_d_ff = controller.i_d_ref*R_TOTAL; //feed-forward voltage - float v_q_ff = controller.i_q_ref*R_TOTAL; - controller.d_int += i_d_error; - controller.q_int += i_q_error; - - limit_norm(&controller.d_int, &controller.q_int, V_BUS/(K_Q*KI_Q)); - //controller.d_int = fminf(fmaxf(controller.d_int, -D_INT_LIM), D_INT_LIM); - //controller.q_int = fminf(fmaxf(controller.q_int, -Q_INT_LIM), Q_INT_LIM); - - - controller.v_d = K_D*i_d_error + K_D*KI_D*controller.d_int;// + v_d_ff; - controller.v_q = K_Q*i_q_error + K_Q*KI_Q*controller.q_int;// + v_q_ff; - //controller.v_d = 10*v_d_ff; - //controller.v_q = 10*v_q_ff; - limit_norm(&controller.v_d, &controller.v_q, controller.v_bus); - - abc(controller.theta_elec, controller.v_d, controller.v_q, &controller.v_u, &controller.v_v, &controller.v_w); //inverse dq0 transform on voltages - svm(controller.v_bus, controller.v_u, controller.v_v, controller.v_w, &controller.dtc_u, &controller.dtc_v, &controller.dtc_w); //space vector modulation + +float velocity_estimate(void){ + velocity.vel_2 = encoder.GetMechVelocity(); + velocity.vel_1 = spi.GetMechVelocity(); + + velocity.ts = .01f; + velocity.vel_1_est = velocity.ts*velocity.vel_1 + (1-velocity.ts)*velocity.vel_1_est; //LPF + velocity.vel_2_est = (1-velocity.ts)*(velocity.vel_2_est + velocity.vel_2 - velocity.vel_2_old); //HPF + velocity.est = velocity.vel_1_est + velocity.vel_2_est; + + velocity.vel_1_old = velocity.vel_1; + velocity.vel_2_old = velocity.vel_2; + return velocity.est; + } - gpio.pwm_u->write(1.0f-controller.dtc_u); //write duty cycles - gpio.pwm_v->write(1.0f-controller.dtc_v); - gpio.pwm_w->write(1.0f-controller.dtc_w); - - controller.theta_elec = spi.GetElecPosition(); - //TIM1->CCR1 = (int)(controller.dtc_u * 0x8CA);//gpio.pwm_u->write(1.0f-controller.dtc_u); //write duty cycles - //TIM1->CCR2 = (int)(controller.dtc_v * 0x8CA);//gpio.pwm_v->write(1.0f-controller.dtc_v); - //TIM1->CCR3 = (int)(controller.dtc_w * 0x8CA);//gpio.pwm_w->write(1.0f-controller.dtc_w); - - //gpio.pwm_u->write(1.0f - .1f); //write duty cycles - //gpio.pwm_v->write(1.0f - .1f); - //gpio.pwm_w->write(1.0f - .15f); - - - if(count >1000){ - controller.i_q_ref = -controller.i_q_ref; - count = 0; - //pc.printf("%f\n\r", controller.theta_elec); - //pc.printf("%f %f %f\n\r", controller.i_a, controller.i_b, controller.i_c); - //pc.printf("%f %f\n\r", controller.i_d, controller.i_q); - //pc.printf("%d %d\n\r", controller.adc1_raw, controller.adc2_raw); - } - } - - -// Current Sampling IRQ +// Current Sampling Interrupt extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { //toggle = 1; - ADC1->CR2 |= 0x40000000; - //volatile int delay; + count++; + ADC1->CR2 |= 0x40000000; //Begin sample and conversion + //volatile int delay; //for (delay = 0; delay < 55; delay++); - controller.adc2_raw = ADC2->DR; - controller.adc1_raw = ADC1->DR; - //toggle = 0; - commutate(); + if(controller.mode == 2){ + controller.adc2_raw = ADC2->DR; + controller.adc1_raw = ADC1->DR; + + //toggle = 0; + + spi.Sample(); + controller.theta_elec = spi.GetElecPosition(); + commutate(&controller, &gpio, controller.theta_elec); + } + + + //controller.dtheta_mech = spi.GetMechVelocity(); + //controller.dtheta_elec = encoder.GetElecVelocity(); + //ontroller.dtheta_mech = encoder.GetMechVelocity(); + //controller.i_q_ref = 2.0f*controller.dtheta_mech; + + //float v1 = encoder.GetMechVelocity(); + //float v2 = spi.GetMechVelocity(); + + + if(count > 100){ + count = 0; + //for (int i = 1; i<16; i++){ + //pc.printf("%d\n\r ", spi.GetRawPosition()); + // } + //pc.printf("\n\r"); + //pc.printf("%.4f %.4f %.4f\n\r",velocity.vel_1 ,velocity.vel_2, velocity.est ); + + } + + } TIM1->SR = 0x0; // reset the status register } - + +void enter_torque_mode(void){ + controller.mode = 2; + TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupts + controller.i_d_ref = 0; + controller.i_q_ref = 1; + reset_foc(&controller); //resets integrators, and other control loop parameters + gpio.enable->write(1); + GPIOC->ODR ^= (1 << 5); //turn on LED + } + +void enter_calibration_mode(void){ + controller.mode = 1; + TIM1->CR1 ^= TIM_CR1_UDIS; + gpio.enable->write(1); + GPIOC->ODR ^= (1 << 5); + //calibrate_encoder(&spi); + order_phases(&spi, &gpio); + calibrate(&spi, &gpio); + GPIOC->ODR ^= (1 << 5); + wait(.2); + gpio.enable->write(0); + TIM1->CR1 ^= TIM_CR1_UDIS; + controller.mode = 0; + } + int main() { controller.v_bus = V_BUS; - spi.ZeroPosition(); - Init_All_HW(&gpio); + controller.mode = 0; + //spi.ZeroPosition(); + Init_All_HW(&gpio); // Setup PWM, ADC, GPIO wait(.1); //TIM1->CR1 |= TIM_CR1_UDIS; gpio.enable->write(1); - gpio.pwm_u->write(1.0f); //write duty cycles + gpio.pwm_u->write(1.0f); //write duty cycles gpio.pwm_v->write(1.0f); gpio.pwm_w->write(1.0f); - zero_current(&controller.adc1_offset, &controller.adc2_offset); + zero_current(&controller.adc1_offset, &controller.adc2_offset); //Measure current sensor zero-offset + //gpio.enable->write(0); reset_foc(&controller); - TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt - gpio.enable->write(1); - //gpio.pwm_u->write(1.0f - .05f); //write duty cycles - //gpio.pwm_v->write(1.0f - .05f); - //gpio.pwm_w->write(1.0f - .1f); + + //TIM1->CR1 ^= TIM_CR1_UDIS; //enable interrupt + //gpio.enable->write(1); + //gpio.pwm_u->write(1.0f - .05f); //write duty cycles + //gpio.pwm_v->write(1.0f - .05f); + //gpio.pwm_w->write(1.0f - .1f); wait(.1); - NVIC_SetPriority(TIM5_IRQn, 2); - //loop.attach(&commutate, .000025); - can.frequency(1000000); // set bit rate to 1Mbps - can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler + NVIC_SetPriority(TIM5_IRQn, 2); // set interrupt priority + + can.frequency(1000000); // set bit rate to 1Mbps + can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler can.filter(0x020 << 25, 0xF0000004, CANAny, 0); - pc.baud(921600); + pc.baud(115200); wait(.01); pc.printf("HobbyKing Cheetah v1.1\n\r"); pc.printf("ADC1 Offset: %d ADC2 Offset: %d", controller.adc1_offset, controller.adc2_offset); wait(.01); + + enter_calibration_mode(); + enter_torque_mode(); - controller.i_d_ref = 0; - controller.i_q_ref = 0; + while(1) { }