Winnie Liu
modified 0511
Dependencies: mbed-dev FastPWM3
Diff: main.cpp
- Revision:
- 63:5946297ba2b0
- Parent:
- 62:d43fcdd2d48b
- Child:
- 64:fd695fb9865b
--- a/main.cpp Mon Mar 07 01:39:20 2022 +0000
+++ b/main.cpp Tue Mar 08 12:01:01 2022 +0000
@@ -3,22 +3,24 @@
/// 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
/// Version for the TI DRV8323 Everything Chip
-
+
#define REST_MODE 0
#define MOTOR_MODE 1
#define HALL_CALIBRATE 2 //hall sensor calibration
#define SETUP_MODE 3
#define CALIBRATION_MODE 4
#define ENCODER_MODE 5
-
-#define VERSION_NUM 5
-
-
+
+#define VERSION_NUM 6
+
+
//float __float_reg[64]; // Floats stored in flash
-float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
+//float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
+//float __float_reg[68]; // add calibrate offset
+float __float_reg[69]; // add calibrate speed
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"
#include "structs.h"
@@ -35,41 +37,40 @@
#include "PreferenceWriter.h"
#include "CAN_com.h"
#include "DRV.h"
-
+
//DigitalOut *CAN_DEBUG = new DigitalOut(PB_15);
//DigitalOut *GPIO_PID_DEBUG = new DigitalOut(PC_6);
-
+
DigitalIn *HALL_IO = new DigitalIn(PC_6);
-
+
PreferenceWriter prefs(6);
-
+
GPIOStruct gpio;
ControllerStruct controller;
ObserverStruct observer;
COMStruct com;
Serial pc(PA_2, PA_3);
-
-
+
+
CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
CANMessage rxMsg;
CANMessage txMsg;
-
-
+
+
SPI drv_spi(PA_7, PA_6, PA_5);
DigitalOut drv_cs(PA_4);
//DigitalOut drv_en_gate(PA_11);
DRV832x drv(&drv_spi, &drv_cs);
-
+
PositionSensorAM5147 spi(16384, 0.0, NPP);
-
+
volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;
-
+
void ChangeParameter(CANMessage msg) ;
bool complete_changepara = false;
-bool set_zero = 0;
-
+
/*Hall sensor calibration*/
volatile int hall_input = 1;
volatile int hall_preinput = 1;
@@ -83,7 +84,7 @@
volatile int calibrate_count = 0;
volatile int calibrate_state = 0;
//
-
+
void onMsgReceived() {
//static int can_state = 0;
//msgAvailable = true;
@@ -104,10 +105,8 @@
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))){
- set_zero = 1;
spi.ZeroPosition();
controller.p_des = 0;
- set_zero = 0;
}
else if(state == REST_MODE && rxMsg.data[0]==0xFE){
printf("Change Parameters\n\r");
@@ -116,7 +115,6 @@
}
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");
- state = HALL_CALIBRATE;
calibrate_count = 0;
calibrate_state = 1; // calibrating
//step1:讀目前位置
@@ -126,9 +124,9 @@
float _f_cal_round;
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;
-
+ if(cal_pcmd < 0) cal_pcmd = cal_pcmd + 2*PI;
+
+ state = HALL_CALIBRATE ;
state_change = 1;
}
else if(state == MOTOR_MODE){
@@ -142,7 +140,7 @@
}
}
-
+
//Use CAN Bus to change parameters
void ChangeParameter(CANMessage msg){
int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
@@ -164,7 +162,7 @@
controller.ki = MOTOR_KI;
controller.kd = MOTOR_KD;
}
-
+
void enter_menu_state(void){
drv.disable_gd();
gpio.enable->write(0);
@@ -186,7 +184,7 @@
state_change = 0;
gpio.led->write(0);
}
-
+
void enter_setup_state(void){
printf("\n\r\n\r Configuration Options \n\r\n\n");
wait_us(10);
@@ -208,6 +206,12 @@
printf(" %-4s %-31s %-5s %-6s %d\n\r", "c", "Calibration Direction", "-1", "1", CALIBRATE_DIR);
wait_us(10);
+ printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "e", "Calibration offset", "0.0", "143.0", CALIBRATE_OFFSET);
+ wait_us(10);
+
+ printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "s", "Calibration Speed", "0.0", "10.0", CALIBRATE_SPEED);
+ wait_us(10);
+
printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
wait_us(10);
printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
@@ -252,19 +256,18 @@
//printf("%d\n\r", spi.GetRawPosition());
wait(.001);
}
-
+
/// Current Sampling Interrupt ///
/// 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(); // read hall sensor
//GPIO_PID_DEBUG->write(1);
-
+
///Sample current always ///
ADC1->CR2 |= 0x40000000; // Begin sample and conversion
//volatile int delay;
//for (delay = 0; delay < 55; delay++);
-
+
spi.Sample(DT); // sample position sensor
controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
controller.adc1_raw = ADC1->DR;
@@ -274,7 +277,99 @@
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
controller.dtheta_elec = spi.GetElecVelocity();
controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
- ///
+
+ // Calibration function
+ if(calibrate_state == 0 || calibrate_state >= 2 ) ;
+ else{
+ // read hall sensor
+ hall_input = HALL_IO->read() ;
+ // calculate new pos
+ 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{
+ // 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.0f ;
+ }
+ }
+
+ if(calibrate_count <= 1) cal_pcmd = cal_pcmd + CALIBRATE_DIR*(1.0f/(40000.0f)*CALIBRATE_SPEED ) ;
+ else{
+ if(CALIBRATE_DIR == 1 ){
+ if(CALIBRATE_OFFSET == 0){
+ //keep turning
+ if(controller.theta_mech >= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
+ else{//stop
+ cal_pcmd = 0.0f;
+ calibrate_state = 2; //success
+ spi.ZeroPosition();
+ //count = 0;
+ //歸零
+ calibrate_count = 0 ;
+ state = MOTOR_MODE;
+ }
+ }
+ else{
+ if(controller.theta_mech <= hall_mid_pos + CALIBRATE_OFFSET*PI/180) cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
+ else{ //stop
+ cal_pcmd = 0.0f;
+ calibrate_state = 2; //success
+ spi.ZeroPosition();
+ //歸零
+ calibrate_count = 0 ;
+ state = MOTOR_MODE;
+ }
+
+
+ }
+ }
+ else if(CALIBRATE_DIR == -1){
+ if(CALIBRATE_OFFSET == 0){
+ //keep turning
+ if(controller.theta_mech <= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
+ else{//stop
+ cal_pcmd = 0.0f;
+ calibrate_state = 2; //success
+ spi.ZeroPosition();
+ //歸零
+ calibrate_count = 0 ;
+ state = MOTOR_MODE;
+ }
+ }
+ else{ //calibrate_offset != 0
+ if(controller.theta_mech >= hall_mid_pos - CALIBRATE_OFFSET*PI/180) cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
+ else{ //stop
+ cal_pcmd = 0.0f;
+ calibrate_state = 2; //success
+ spi.ZeroPosition();
+ //歸零
+ calibrate_count = 0 ;
+ state = MOTOR_MODE;
+ }
+
+ }
+
+
+
+ }
+ }
+
+ 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 ;
+ }
+
+ hall_preinput = hall_input ;
+ }
/// Check state machine state, and run the appropriate function ///
switch(state){
@@ -283,14 +378,10 @@
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;
+ drv.disable_gd();
+ gpio.enable->write(0);
+ state_change = 0;
+ gpio.led->write(0);
}
}
break;
@@ -317,7 +408,7 @@
printf("OVP Triggered!\n\r");
}
*/
-
+
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
@@ -325,119 +416,56 @@
controller.kd = 0;
controller.t_ff = 0;
}
+
+ torque_control(&controller);
+ commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
- if(!set_zero){
+ controller.timeout++;
+ count++;
+
+ }
+ break;
+ case HALL_CALIBRATE:
+ if(state_change){
+ enter_torque_mode();
+ count = 0;
+ }
+ else{
+ 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 SETUP_MODE:
if(state_change){
enter_setup_state();
}
break;
- case HALL_CALIBRATE:
- if(state_change){
- //step2:馬達激磁
- enter_torque_mode();
- calibrate_state = 1;
- count = 0;
- }
- else{
- //若轉超過一圈就算失敗
- 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{
- //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 = 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++;
- }
- }
- break;
case ENCODER_MODE:
print_encoder();
break;
}
}
TIM1->SR = 0x0; // reset the status register
- hall_preinput = hall_input;
//GPIO_PID_DEBUG->write(0);
}
-
-
+
+
char cmd_val[8] = {0};
char cmd_id = 0;
char char_count = 0;
-
+
/// Manage state machine with commands from serial terminal or configurator gui ///
/// Called when data received over serial ///
void serial_interrupt(void){
@@ -509,6 +537,12 @@
case 'c':
CALIBRATE_DIR = atoi(cmd_val);
break;
+ case 'e':
+ CALIBRATE_OFFSET = fmaxf(fminf(atof(cmd_val), 143.0f), 0.0f);
+ break;
+ case 's':
+ CALIBRATE_SPEED = fmaxf(fminf(atof(cmd_val), 10.0f), 0.0f);
+ break;
case 'p':
MOTOR_KP = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);;
break;
@@ -565,7 +599,7 @@
}
}
-
+
void GPIO_SETUP(){
//GPIOB->MODER |= 0x40000000;
//GPIOC->MODER |= 0x0000100f;
@@ -595,7 +629,7 @@
zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
drv.disable_gd();
gpio.enable->write(0);
-
+
wait(.1);
/*
gpio.enable->write(1);
@@ -637,6 +671,8 @@
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;}
+ if(isnan(CALIBRATE_OFFSET) || CALIBRATE_OFFSET==-1){CALIBRATE_OFFSET = 0.0;}
+ if(isnan(CALIBRATE_SPEED) || CALIBRATE_SPEED==-1){CALIBRATE_SPEED = 0.25;}
spi.SetElecOffset(E_OFFSET); // Set position sensor offset
spi.SetMechOffset(M_OFFSET);
@@ -644,7 +680,7 @@
memcpy(&lut, &ENCODER_LUT, sizeof(lut));
spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
init_controller_params(&controller);
-
+
pc.baud(115200); // set serial baud rate
wait(.01);
pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
@@ -663,7 +699,9 @@
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(" Calibrate Offset: %f\n\r", CALIBRATE_OFFSET);
+ printf(" Calibrate Speed: %f\n\r", CALIBRATE_SPEED);
+
//printf(" %d\n\r", drv.read_register(DCR));
//wait_us(100);
//printf(" %d\n\r", drv.read_register(CSACR));
@@ -677,7 +715,7 @@
// calibrate
calibrate_state = 0;
-
+
int counter = 0;
while(1) {
if(complete_changepara){
@@ -710,7 +748,7 @@
printf("helloworld\n\r");
wait(1);
*/
-
+
/*
CAN_DEBUG->write(1);
wait(0.1);
@@ -729,6 +767,6 @@
wait(.002);
}
*/
-
+
}
-}
+}
\ No newline at end of file