Tony Stark
motor control code by JYB
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 47:f4ecf3e0576a
- Parent:
- 45:aadebe074af6
- Child:
- 48:e9f6441bacf5
--- a/main.cpp Mon Jul 30 20:33:23 2018 +0000
+++ b/main.cpp Wed May 13 09:53:27 2020 +0000
@@ -8,6 +8,7 @@
#define MOTOR_MODE 2
#define SETUP_MODE 4
#define ENCODER_MODE 5
+#define TEST_MODE 6 // this mode is used to test motor, which allow user control motor's torque directly on the GUI software
#define VERSION_NUM "1.6"
@@ -30,7 +31,7 @@
#include "user_config.h"
#include "PreferenceWriter.h"
#include "CAN_com.h"
-
+#include <string>
PreferenceWriter prefs(6);
@@ -38,24 +39,31 @@
ControllerStruct controller;
COMStruct com;
ObserverStruct observer;
-Serial pc(PA_2, PA_3);
+// Serial pc(PA_2, PA_3);
+Serial pc(PC_6, PC_7);
+
+DataPackage datapackage;
CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
CANMessage rxMsg;
CANMessage txMsg;
-
+DigitalOut led_debug(PB_1);
PositionSensorAM5147 spi(16384, 0.0, NPP);
-volatile int count = 0;
+//volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;
+volatile float test_tff,test_kp,test_kd,test_pos_des,test_vel_des;
+volatile float test_ia,test_ib,test_ic,test_id,test_iq;
+volatile float test_mech_angle,test_mech_speed;
+
void onMsgReceived() {
//msgAvailable = true;
-
+
can.read(rxMsg);
if((rxMsg.id == CAN_ID)){
controller.timeout = 0;
@@ -66,7 +74,7 @@
else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
state = REST_MODE;
state_change = 1;
- gpio.led->write(0);;
+ gpio.led->write(0);
}
else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
spi.ZeroPosition();
@@ -81,20 +89,22 @@
}
void enter_menu_state(void){
- printf("\n\r\n\r\n\r");
- printf(" Commands:\n\r");
+ pc.printf("\n\r\n\r\n\r");
+ pc.printf(" Commands:\n\r");
wait_us(10);
- printf(" m - Motor Mode\n\r");
+ pc.printf(" m - Motor Mode\n\r");
wait_us(10);
- printf(" c - Calibrate Encoder\n\r");
+ pc.printf(" c - Calibrate Encoder\n\r");
wait_us(10);
- printf(" s - Setup\n\r");
+ pc.printf(" s - Setup\n\r");
wait_us(10);
- printf(" e - Display Encoder\n\r");
+ pc.printf(" e - Display Encoder\n\r");
wait_us(10);
- printf(" z - Set Zero Position\n\r");
+ pc.printf(" z - Set Zero Position\n\r");
wait_us(10);
- printf(" esc - Exit to Menu\n\r");
+ pc.printf(" t - Motor Test Mode\n\r");
+ wait_us(10);
+ pc.printf(" esc - Exit to Menu\n\r");
wait_us(10);
state_change = 0;
gpio.enable->write(0);
@@ -102,21 +112,39 @@
}
void enter_setup_state(void){
- printf("\n\r\n\r Configuration Options \n\r\n\n");
+ pc.printf("\n\r\n\r Configuration Options \n\r\n\n");
+ wait_us(10);
+ pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
wait_us(10);
- printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+ wait_us(10);
+ pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
wait_us(10);
- printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
+ pc.printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
+ wait_us(10);
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
wait_us(10);
- printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
+ pc.printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
+ wait_us(10);
+ pc.printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
wait_us(10);
- printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
+ state_change = 0;
+ }
+
+void enter_test_state(void){
+ pc.printf("\n\r\n\r Control Options \n\r\n\n");
wait_us(10);
- printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
+ pc.printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
wait_us(10);
- printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "t", "Torque", "-18", "18", controller.t_ff);
+ wait_us(10);
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "k", "Stiffness", "0", "500", controller.kp);
wait_us(10);
- printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "d", "Damping", "0", "5.0", controller.kd);
+ wait_us(10);
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "p", "Position", "-12.5", "12.5", controller.p_des);
+ wait_us(10);
+ pc.printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Velocity", "-45", "45", controller.v_des);
wait_us(10);
state_change = 0;
}
@@ -130,7 +158,7 @@
controller.i_q_ref = 0; // Current Setpoints
gpio.led->write(1); // Turn on status LED
state_change = 0;
- printf("\n\r Entering Motor Mode \n\r");
+ pc.printf("\n\r Entering Motor Mode \n\r");
}
void calibrate(void){
@@ -141,17 +169,21 @@
gpio.led->write(0);; // Turn off status LED
wait(.2);
gpio.enable->write(0); // Turn off gate drive
- printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
+ pc.printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
state_change = 0;
}
void print_encoder(void){
- printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+ pc.printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
wait(.05);
}
/// Current Sampling Interrupt ///
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
+int LLLL = 0;
+bool led_flag = 0;
+bool flag_test_first = true;
+
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
@@ -169,12 +201,20 @@
controller.dtheta_elec = spi.GetElecVelocity();
controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
///
+ LLLL ++;
+ if(LLLL==10000){
+ LLLL=0;
+ led_flag = 1-led_flag;
+ led_debug=(led_flag);
+ // pc.printf("in interrupt\n\r");
+ }
/// Check state machine state, and run the appropriate function ///
switch(state){
case REST_MODE: // Do nothing
if(state_change){
enter_menu_state();
+ flag_test_first = true;
}
break;
@@ -187,7 +227,7 @@
case MOTOR_MODE: // Run torque control
if(state_change){
enter_torque_mode();
- count = 0;
+// count = 0;
}
else{
/*
@@ -199,7 +239,7 @@
printf("OVP Triggered!\n\r");
}
*/
-
+ // controller.t_ff = 1;
torque_control(&controller);
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
@@ -227,6 +267,45 @@
enter_setup_state();
}
break;
+
+ case TEST_MODE:
+ if(flag_test_first){
+ // enter_test_state();
+ // enter_torque_mode();
+ controller.ovp_flag = 0;
+ gpio.enable->write(1); // Enable gate drive
+ reset_foc(&controller); // Tesets integrators, and other control loop parameters
+ wait(.001);
+ controller.i_d_ref = 0;
+ controller.i_q_ref = 0; // Current Setpoints
+ gpio.led->write(1);
+ flag_test_first = false; // Turn on status LED
+ }
+ else{
+ controller.t_ff = test_tff;
+ controller.kp = test_kp;
+ controller.p_des = test_pos_des;
+ controller.kd = test_kd;
+ controller.v_des = test_vel_des;
+
+ torque_control(&controller);
+ commutate(&controller, &observer, &gpio, controller.theta_elec);
+ test_mech_angle = controller.theta_mech;
+ test_mech_speed = controller.dtheta_mech;
+ test_ia = controller.i_a;
+ test_ib = controller.i_b;
+ test_ic = controller.i_c;
+// test_ia = observer.i_q_est;
+// test_ib = controller.v_d;
+// test_ic = controller.v_q;
+ test_id = controller.i_d;
+ test_iq = controller.i_q;
+// test_iq = observer.i_q_est;
+// test_id = observer.i_d_est;
+// test_iq = controller->i_q_filt；
+ }
+ break;
+
case ENCODER_MODE:
print_encoder();
break;
@@ -281,9 +360,13 @@
prefs.close();
prefs.load();
spi.SetMechOffset(M_OFFSET);
- printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET);
+ pc.printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET);
break;
+ case 't':
+ // this state is used to test motor through GUI
+ state = TEST_MODE;
+ state_change = 1;
}
}
@@ -306,7 +389,7 @@
CAN_TIMEOUT = atoi(cmd_val);
break;
default:
- printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
+ pc.printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
break;
}
@@ -337,7 +420,48 @@
break;
}
}
-
+ else if(state == TEST_MODE){
+ if(c == 13){
+ switch(cmd_id){
+ case 't':
+ // controller.t_ff = fmaxf(fminf(atof(cmd_val), TORQUE_LIMIT), -TORQUE_LIMIT);
+ test_tff = fmaxf(fminf(atof(cmd_val), TORQUE_LIMIT), -TORQUE_LIMIT);
+ break;
+ case 'k':
+ // controller.kp = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);
+ test_kp = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);
+ break;
+ case 'd':
+ // controller.kd = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN);
+ test_kd = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN);
+ break;
+ case 'p':
+ // controller.p_des = fmaxf(fminf(atof(cmd_val), P_MAX), P_MIN);
+ test_pos_des = fmaxf(fminf(atof(cmd_val), P_MAX), P_MIN);
+ break;
+ case 'v':
+ // controller.v_des = fmaxf(fminf(atof(cmd_val), V_MAX), V_MIN);
+ test_vel_des = fmaxf(fminf(atof(cmd_val), V_MAX), V_MIN);
+ break;
+ default:
+ // pc.printf("\n\r '%c' Not a valid command prefix\n\r", cmd_id);
+ break;
+ }
+ state_change = 1;
+ char_count = 0;
+ cmd_id = 0;
+ for(int i = 0; i<8; i++){cmd_val[i] = 0;}
+ }
+ else{
+ if(char_count == 0){cmd_id = c;}
+ else{
+ cmd_val[char_count-1] = c;
+
+ }
+ // pc.putc(c);
+ char_count++;
+ }
+ }
}
}
@@ -379,22 +503,77 @@
spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
pc.baud(921600); // set serial baud rate
+ // pc.baud(460800);
+
wait(.01);
- pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
+ pc.printf("\n\r\n\r 3.3, With obs, vdff, no cogging\n\r\n\r");
wait(.01);
- printf("\n\r Debug Info:\n\r");
- printf(" Firmware Version: %s\n\r", VERSION_NUM);
- printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
- printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
- printf(" Output Zero Position: %.4f\n\r", M_OFFSET);
- printf(" CAN ID: %d\n\r", CAN_ID);
+ pc.printf("\n\r Debug Info:\n\r");
+ pc.printf(" Firmware Version: %s\n\r", VERSION_NUM);
+ pc.printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
+ pc.printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
+ pc.printf(" Output Zero Position: %.4f\n\r", M_OFFSET);
+ pc.printf(" CAN ID: %d\n\r", CAN_ID);
pc.attach(&serial_interrupt); // attach serial interrupt
+ // =============================================================================================================
+ // add watch dog here
+// Watchdog &watchdog = Watchdog::get_instance();
+ // =============================================================================================================
+
state_change = 1;
-
+ datapackage.info.init_flag1 = 0xffffffff;
+ datapackage.info.init_flag2 = 0xfffefdfc;
+ datapackage.info.check_num = 0;
while(1) {
+ // pc.printf("hello_wooden");
+ // pc.printf("I's still working\n\r");
+ // pc.printf("adc1: %d, adc2: %d, adc3: %d",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw);
+ if(state == TEST_MODE){
+// datapackage.info.I_a = controller.i_a;
+// datapackage.info.I_b = controller.i_b;
+// datapackage.info.I_c = controller.i_c;
+// datapackage.info.I_q = controller.i_q;
+// datapackage.info.I_d = controller.i_d;
+// datapackage.info.Angle_mech = controller.theta_mech;
+// datapackage.info.Angle_elec = controller.theta_elec;
+// datapackage.info.Control_tff = controller.t_ff;
+// datapackage.info.Control_kp = controller.kp;
+// datapackage.info.Control_p = controller.theta_mech;
+// datapackage.info.Control_kd = controller.kd;
+// datapackage.info.Control_v = controller.dtheta_mech;
+
+ datapackage.info.I_a = test_ia;
+ datapackage.info.I_b = test_ib;
+ datapackage.info.I_c = test_ic;
+ datapackage.info.I_q = test_iq;
+ datapackage.info.I_d = test_id;
+ datapackage.info.Angle_mech = test_mech_angle;
+ datapackage.info.Angle_elec = test_mech_speed; // actually this return the speed
+ datapackage.info.Control_tff = test_tff;
+ datapackage.info.Control_kp = test_kp;
+ datapackage.info.Control_p = test_pos_des;
+ datapackage.info.Control_kd = test_kd;
+ datapackage.info.Control_v = test_vel_des;
+
+ // pc.write(datapackage.decode, 60);
+ // string s(datapackage.decode);
+// pc.printf((const char *)datapackage.decode );wait_us(900);
+
+ for(int i=0;i<60;i++){
+ pc.putc(datapackage.decode[i]);
+ // wait_us(30);
+ wait_us(300);
+ }
+ //wait_us(5000);
+ }
+ else{
+ // can.write(txMsg);
+ wait(1.0);
+ ;
+ }
}
}