Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 22:60276ba87ac6
- Parent:
- 20:bf9ea5125d52
- Child:
- 23:2adf23ee0305
--- 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) {
}