Winnie Liu
modified 0511
Dependencies: mbed-dev FastPWM3
Diff: main.cpp
- Revision:
- 62:d43fcdd2d48b
- Parent:
- 61:29371056a3de
- Child:
- 63:5946297ba2b0
--- a/main.cpp Wed Mar 02 10:01:40 2022 +0000
+++ b/main.cpp Mon Mar 07 01:39:20 2022 +0000
@@ -5,19 +5,19 @@
/// Version for the TI DRV8323 Everything Chip
#define REST_MODE 0
-#define CALIBRATION_MODE 1
-#define MOTOR_MODE 2
-#define SETUP_MODE 4
+#define MOTOR_MODE 1
+#define HALL_CALIBRATE 2 //hall sensor calibration
+#define SETUP_MODE 3
+#define CALIBRATION_MODE 4
#define ENCODER_MODE 5
-#define HALL_CALIBRATE 6 //hall sensor calibration
-#define HALL_OFFSET 7
#define VERSION_NUM 5
//float __float_reg[64]; // Floats stored in flash
float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
-int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table
+int __int_reg[257]; //新增calibrate旋轉方向(+1: 逆時針旋轉、-1: 順時針旋轉)
+//int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table
#include "mbed.h"
#include "PositionSensor.h"
@@ -35,9 +35,6 @@
#include "PreferenceWriter.h"
#include "CAN_com.h"
#include "DRV.h"
-#include "math.h"
-
-using namespace FastMath;
//DigitalOut *CAN_DEBUG = new DigitalOut(PB_15);
//DigitalOut *GPIO_PID_DEBUG = new DigitalOut(PC_6);
@@ -82,10 +79,10 @@
volatile float hall_in_pos = 0; //讀到1->0的位置(磁鐵進入hall sensor範圍)
volatile float hall_out_pos = 0; //讀到0->1的位置(磁鐵出hall sensor範圍)
volatile float hall_mid_pos = 0;
-volatile int calibrate_dir = -1;
volatile float calibrate_offset = 0; //rad
volatile int calibrate_count = 0;
-volatile int calibrate_finish = 0;
+volatile int calibrate_state = 0;
+//
void onMsgReceived() {
//static int can_state = 0;
@@ -97,6 +94,7 @@
controller.timeout = 0;
//printf("%X\n\r",0x55);
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]==0xFC))){
+ //printf("can motor\n\r");
state = MOTOR_MODE;
state_change = 1;
}
@@ -117,26 +115,26 @@
complete_changepara = true;
}
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]==0xFA))){
- printf("Hall Calibration\n\r");
- calibrate_finish = 0;
+ //printf("Hall Calibration\n\r");
+ state = HALL_CALIBRATE;
+ calibrate_count = 0;
+ calibrate_state = 1; // calibrating
//step1:讀目前位置
/*----- convert theta_mech to 0~359.9999deg -----*/
hall_presentpos = controller.theta_mech;
+ cal_pcmd = controller.theta_mech;
float _f_cal_round;
- modf(hall_presentpos/(2*PI),&_f_cal_round);
- hall_presentpos = hall_presentpos - _f_cal_round*2*PI;
- if(hall_presentpos < 0)
- hall_presentpos = hall_presentpos + 2*PI;
+ modf(cal_pcmd/(2*PI),&_f_cal_round);
+ cal_pcmd = cal_pcmd - _f_cal_round*2*PI;
+ if(cal_pcmd < 0)
+ cal_pcmd = cal_pcmd + 2*PI;
- printf("hall_presentpos: %f\n\r", hall_presentpos);
- cal_pcmd = hall_presentpos;
- state = HALL_CALIBRATE;
state_change = 1;
}
else if(state == MOTOR_MODE){
unpack_cmd(rxMsg, &controller);
}
- pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, hall_input);
+ pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, calibrate_state, state);
can.write(txMsg);
//can_state = can.write(txMsg);
//CAN_DEBUG->write(0);
@@ -187,7 +185,7 @@
wait_us(10);
state_change = 0;
gpio.led->write(0);
- }
+}
void enter_setup_state(void){
printf("\n\r\n\r Configuration Options \n\r\n\n");
@@ -207,11 +205,14 @@
printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
wait_us(10);
- printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
+ printf(" %-4s %-31s %-5s %-6s %d\n\r", "c", "Calibration Direction", "-1", "1", CALIBRATE_DIR);
wait_us(10);
- printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
+
+ printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
wait_us(10);
- printf(" %-4s %-31s %.2fs %.2fs %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD);
+ printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
+ wait_us(10);
+ printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD);
wait_us(10);
printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
@@ -279,8 +280,19 @@
switch(state){
case REST_MODE: // Do nothing
if(state_change){
- enter_menu_state();
+ if(calibrate_state != 2) //success
+ enter_menu_state();
+ else{
+// if(calibrate_count != 0){
+ drv.disable_gd();
+ gpio.enable->write(0);
+ state_change = 0;
+ gpio.led->write(0);
+// }
+// state = MOTOR_MODE;
+// state_change = 1;
}
+ }
break;
case CALIBRATION_MODE: // Run encoder calibration procedure
@@ -333,75 +345,82 @@
if(state_change){
//step2:馬達激磁
enter_torque_mode();
+ calibrate_state = 1;
count = 0;
}
else{
- //step3: 馬達正反轉讀hall sensor(1: 沒有感應到磁鐵，0:有感應到磁鐵)
- //記住1->0瞬間的位置(in_pos)，繼續旋轉
- //記住0->1瞬間的位置(out_pos)，停止旋轉，計算in_pos與out_pos的平均值，再讓馬達慢慢轉到位置
- if(hall_input != hall_preinput){
- calibrate_count += 1;
- if(hall_input == 0)
- hall_in_pos = controller.theta_mech;
- else{
- hall_out_pos = controller.theta_mech;
- hall_mid_pos = (hall_in_pos + hall_out_pos)/2;
- }
- }
-
- if(calibrate_count <= 1){
- cal_pcmd = cal_pcmd + calibrate_dir*(1.0f/(40000.0f)*calibrate_speed);
+ //若轉超過一圈就算失敗
+ if((CALIBRATE_DIR == 1 && controller.theta_mech >= hall_presentpos + 2*PI) || (CALIBRATE_DIR == -1 && controller.theta_mech <= hall_presentpos - 2*PI)){
+ calibrate_state = 3;
+ state = REST_MODE;
+ state_change = 1;
}
else{
- if(calibrate_dir == 1){
- if(controller.theta_mech >= hall_mid_pos) //keep turning
- cal_pcmd = cal_pcmd - calibrate_dir*1.0f/40000.0f*calibrate_speed;
- else{//stop
- cal_pcmd = controller.theta_mech;
- calibrate_finish = 1;
- state = REST_MODE;
- state_change = 1;
- spi.ZeroPosition();
- controller.p_des = 0;
- //歸零
- calibrate_count = 0;
+ //step3: 馬達正反轉讀hall sensor(1: 沒有感應到磁鐵，0:有感應到磁鐵)
+ //記住1->0瞬間的位置(in_pos)，繼續旋轉
+ //記住0->1瞬間的位置(out_pos)，停止旋轉，計算in_pos與out_pos的平均值，再讓馬達慢慢轉到位置
+ if(hall_input != hall_preinput){
+ calibrate_count += 1;
+ if(hall_input == 0)
+ hall_in_pos = controller.theta_mech;
+ else{
+ hall_out_pos = controller.theta_mech;
+ hall_mid_pos = (hall_in_pos + hall_out_pos)/2;
+ }
+ }
+
+ if(calibrate_count <= 1){
+ cal_pcmd = cal_pcmd + CALIBRATE_DIR*(1.0f/(40000.0f)*calibrate_speed);
}
- }
- else if(calibrate_dir == -1){
- if(controller.theta_mech <= hall_mid_pos) //keep turning
- cal_pcmd = cal_pcmd - calibrate_dir*(1.0f/(40000.0f)*calibrate_speed);
- else{//stop
- cal_pcmd = controller.theta_mech;
- calibrate_finish = 1;
- spi.ZeroPosition();
- controller.p_des = 0;
- state = REST_MODE;
- state_change = 1;
- //歸零
- calibrate_count = 0;
+ else{
+ if(CALIBRATE_DIR == 1){
+ if(controller.theta_mech >= hall_mid_pos) //keep turning
+ cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*calibrate_speed;
+ else{//stop
+ cal_pcmd = 0;
+ calibrate_state = 2; //success
+ spi.ZeroPosition();
+ controller.p_des = 0;
+ state = REST_MODE;
+ state_change = 1;
+ //count = 0;
+ //歸零
+ calibrate_count = 0;
+
+ }
+ }
+ else if(CALIBRATE_DIR == -1){
+ if(controller.theta_mech <= hall_mid_pos) //keep turning
+ cal_pcmd = cal_pcmd - CALIBRATE_DIR*(1.0f/(40000.0f)*calibrate_speed);
+ else{//stop
+ cal_pcmd = controller.theta_mech;
+ calibrate_state = 2; //success
+ spi.ZeroPosition();
+ controller.p_des = 0;
+ //歸零
+ calibrate_count = 0;
+ }
+ }
}
- }
-
+
+ cal_pcmd = (cal_pcmd>2*PI) ? cal_pcmd-=2*PI : cal_pcmd;
+ cal_pcmd = (cal_pcmd < 0) ? cal_pcmd+=2*PI : cal_pcmd;
+ controller.p_des = cal_pcmd;
+ if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
+ controller.i_d_ref = 0;
+ controller.i_q_ref = 0;
+ controller.kp = 0;
+ controller.ki = 0;
+ controller.kd = 0;
+ controller.t_ff = 0;
+ }
+ torque_control(&controller);
+ commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
+
+ controller.timeout++;
+ count++;
}
- cal_pcmd = (cal_pcmd>2*PI) ? cal_pcmd-=2*PI : cal_pcmd;
- cal_pcmd = (cal_pcmd < 0) ? cal_pcmd+=2*PI : cal_pcmd;
- controller.p_des = cal_pcmd;
-
-
- if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
- controller.i_d_ref = 0;
- controller.i_q_ref = 0;
- controller.kp = 0;
- controller.kd = 0;
- controller.t_ff = 0;
- }
-
- torque_control(&controller);
- commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
-
- controller.timeout++;
- count++;
}
break;
case ENCODER_MODE:
@@ -487,6 +506,9 @@
case 't':
CAN_TIMEOUT = atoi(cmd_val);
break;
+ case 'c':
+ CALIBRATE_DIR = atoi(cmd_val);
+ break;
case 'p':
MOTOR_KP = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);;
break;
@@ -614,6 +636,7 @@
if(isnan(MOTOR_KP) || MOTOR_KP==-1){MOTOR_KP = 5;}
if(isnan(MOTOR_KI) || MOTOR_KI==-1){MOTOR_KI = 0;}
if(isnan(MOTOR_KD) || MOTOR_KD==-1){MOTOR_KD = 1.25;}
+ if(CALIBRATE_DIR != -1 && CALIBRATE_DIR != 1){CALIBRATE_DIR = 1;}
spi.SetElecOffset(E_OFFSET); // Set position sensor offset
spi.SetMechOffset(M_OFFSET);
@@ -637,8 +660,9 @@
controller.kp = MOTOR_KP;
controller.ki = MOTOR_KI;
controller.kd = MOTOR_KD;
- printf("PID controller parameters \n\r");
- printf("KP: %.3f, KI: %.3f, KD: %.3f \n\r", controller.kp, controller.ki, controller.kd);
+ printf(" PID controller parameters \n\r");
+ printf(" KP: %.3f, KI: %.3f, KD: %.3f \n\r", controller.kp, controller.ki, controller.kd);
+ printf(" Calibrate Direction: %d\n\r", CALIBRATE_DIR);
//printf(" %d\n\r", drv.read_register(DCR));
//wait_us(100);
@@ -650,6 +674,9 @@
pc.attach(&serial_interrupt); // attach serial interrupt
state_change = 1;
+
+ // calibrate
+ calibrate_state = 0;
int counter = 0;
while(1) {
@@ -662,7 +689,7 @@
state = REST_MODE;
state_change = 1;
}
-
+
// printf("Hall_input(PC6): %d\n\r", hall_input);
// printf("Hall_preinput(PC6): %d\n\r", hall_preinput);
@@ -671,6 +698,7 @@
// printf("pos_in: %f\n\r",hall_in_pos);
// printf("pos_out: %f\n\r",hall_out_pos);
// printf("theta_mech: %f\n\r",controller.theta_mech);
+// printf("controller.pdes: %f\n\r", controller.p_des);
// printf("state: %d\n\r",state);
// wait(1);