Winnie Liu
modified 0511
Dependencies: mbed-dev FastPWM3
Diff: main.cpp
- Revision:
- 61:29371056a3de
- Parent:
- 60:f009e39e913e
- Child:
- 62:d43fcdd2d48b
--- a/main.cpp Tue Mar 01 12:29:08 2022 +0000
+++ b/main.cpp Wed Mar 02 10:01:40 2022 +0000
@@ -9,8 +9,10 @@
#define MOTOR_MODE 2
#define SETUP_MODE 4
#define ENCODER_MODE 5
+#define HALL_CALIBRATE 6 //hall sensor calibration
+#define HALL_OFFSET 7
-#define VERSION_NUM 4
+#define VERSION_NUM 5
//float __float_reg[64]; // Floats stored in flash
@@ -33,6 +35,9 @@
#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);
@@ -68,7 +73,19 @@
bool complete_changepara = false;
bool set_zero = 0;
+/*Hall sensor calibration*/
volatile int hall_input = 1;
+volatile int hall_preinput = 1;
+volatile float cal_pcmd = 0;
+volatile float calibrate_speed = 0.25; // rad/s
+volatile float hall_presentpos = 0; //calibrate之前encoder的位置
+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;
void onMsgReceived() {
//static int can_state = 0;
@@ -99,6 +116,23 @@
ChangeParameter(rxMsg);
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;
+ //step1:讀目前位置
+ /*----- convert theta_mech to 0~359.9999deg -----*/
+ hall_presentpos = 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;
+
+ 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);
}
@@ -222,7 +256,7 @@
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
- hall_input = HALL_IO->read();
+ hall_input = HALL_IO->read(); // read hall sensor
//GPIO_PID_DEBUG->write(1);
///Sample current always ///
@@ -295,12 +329,88 @@
enter_setup_state();
}
break;
+ case HALL_CALIBRATE:
+ if(state_change){
+ //step2:馬達激磁
+ enter_torque_mode();
+ 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);
+ }
+ 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;
+ }
+ }
+ 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;
+ }
+ }
+
+
+ }
+ 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:
print_encoder();
break;
}
}
- TIM1->SR = 0x0; // reset the status register
+ TIM1->SR = 0x0; // reset the status register
+ hall_preinput = hall_input;
//GPIO_PID_DEBUG->write(0);
}
@@ -541,7 +651,6 @@
state_change = 1;
-
int counter = 0;
while(1) {
if(complete_changepara){
@@ -553,10 +662,17 @@
state = REST_MODE;
state_change = 1;
}
- //printf("p_des: %f\n\r", controller.p_des);
+
- //printf("Hall_input(PC6): %d\n\r", HALL_IN->read());
- //wait(1);
+// printf("Hall_input(PC6): %d\n\r", hall_input);
+// printf("Hall_preinput(PC6): %d\n\r", hall_preinput);
+// printf("calibrate count: %d\n\r",calibrate_count);
+// printf("cal_pcmd: %.4f\n\r",cal_pcmd);
+// 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("state: %d\n\r",state);
+// wait(1);
/*
CAN_DEBUG->write(1);