Winnie Liu / Hobbyking_Cheetah_0511

modified 0511

Dependencies:   mbed-dev FastPWM3

main.cpp@65:cb3497b549e9, 22 months ago (annotated)

Committer:
WinnieLiu
Date:
Fri Aug 19 07:46:05 2022 +0000
Revision:
65:cb3497b549e9
Parent:
64:fd695fb9865b 
latest code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 22:60276ba87ac6 1 /// high-bandwidth 3-phase motor control, for robots
benkatz 45:26801179208e 2 /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others
benkatz 22:60276ba87ac6 3 /// Hardware documentation can be found at build-its.blogspot.com
benkatz 22:60276ba87ac6 4 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
benkatz 47:e1196a851f76 5 /// Version for the TI DRV8323 Everything Chip
WinnieLiu 65:cb3497b549e9 6
WinnieLiu 65:cb3497b549e9 7 // ----- CAN and UART -----
benkatz 23:2adf23ee0305 8 #define REST_MODE 0
WinnieLiu 65:cb3497b549e9 9 #define HALL_CALIBRATE 1 //hall sensor calibration
WinnieLiu 65:cb3497b549e9 10 #define MOTOR_MODE 2
WinnieLiu 65:cb3497b549e9 11 #define SET_ZERO 3
WinnieLiu 65:cb3497b549e9 12
WinnieLiu 65:cb3497b549e9 13 //----- UART only ------
WinnieLiu 65:cb3497b549e9 14 #define SETUP_MODE 4
WinnieLiu 65:cb3497b549e9 15 #define CALIBRATION_MODE 5
WinnieLiu 65:cb3497b549e9 16 #define ENCODER_MODE 6
WinnieLiu 63:5946297ba2b0 17
WinnieLiu 65:cb3497b549e9 18 #define VERSION_NUM 3
WinnieLiu 65:cb3497b549e9 19
WinnieLiu 65:cb3497b549e9 20 #define VER2_0
WinnieLiu 65:cb3497b549e9 21 #define PRINT_UART
WinnieLiu 65:cb3497b549e9 22
WinnieLiu 63:5946297ba2b0 23
WinnieLiu 55:b97b90d06ffa 24 //float __float_reg[64]; // Floats stored in flash
WinnieLiu 63:5946297ba2b0 25 //float __float_reg[67]; // Floats stored in flash(add three floats: kp, ki, kd)
WinnieLiu 63:5946297ba2b0 26 //float __float_reg[68]; // add calibrate offset
WinnieLiu 63:5946297ba2b0 27 float __float_reg[69]; // add calibrate speed
WinnieLiu 64:fd695fb9865b 28 //float __float_reg[66];
WinnieLiu 62:d43fcdd2d48b 29 int __int_reg[257]; //新增calibrate旋轉方向(+1: 逆時針旋轉、-1: 順時針旋轉)
WinnieLiu 62:d43fcdd2d48b 30 //int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table
WinnieLiu 63:5946297ba2b0 31
benkatz 0:4e1c4df6aabd 32 #include "mbed.h"
benkatz 0:4e1c4df6aabd 33 #include "PositionSensor.h"
benkatz 20:bf9ea5125d52 34 #include "structs.h"
benkatz 20:bf9ea5125d52 35 #include "foc.h"
benkatz 22:60276ba87ac6 36 #include "calibration.h"
benkatz 20:bf9ea5125d52 37 #include "hw_setup.h"
benkatz 23:2adf23ee0305 38 #include "math_ops.h"
benkatz 20:bf9ea5125d52 39 #include "current_controller_config.h"
benkatz 20:bf9ea5125d52 40 #include "hw_config.h"
benkatz 20:bf9ea5125d52 41 #include "motor_config.h"
benkatz 23:2adf23ee0305 42 #include "stm32f4xx_flash.h"
benkatz 23:2adf23ee0305 43 #include "FlashWriter.h"
benkatz 23:2adf23ee0305 44 #include "user_config.h"
benkatz 23:2adf23ee0305 45 #include "PreferenceWriter.h"
benkatz 42:738fa01b0346 46 #include "CAN_com.h"
benkatz 44:8040fa2fcb0d 47 #include "DRV.h"
WinnieLiu 63:5946297ba2b0 48
WinnieLiu 60:f009e39e913e 49 //DigitalOut *CAN_DEBUG = new DigitalOut(PB_15);
WinnieLiu 60:f009e39e913e 50 //DigitalOut *GPIO_PID_DEBUG = new DigitalOut(PC_6);
WinnieLiu 63:5946297ba2b0 51
WinnieLiu 60:f009e39e913e 52 DigitalIn *HALL_IO = new DigitalIn(PC_6);
WinnieLiu 63:5946297ba2b0 53
benkatz 23:2adf23ee0305 54 PreferenceWriter prefs(6);
WinnieLiu 63:5946297ba2b0 55
benkatz 20:bf9ea5125d52 56 GPIOStruct gpio;
benkatz 20:bf9ea5125d52 57 ControllerStruct controller;
benkatz 48:74a40481740c 58 ObserverStruct observer;
benkatz 20:bf9ea5125d52 59 COMStruct com;
benkatz 43:dfb72608639c 60 Serial pc(PA_2, PA_3);
WinnieLiu 63:5946297ba2b0 61
WinnieLiu 63:5946297ba2b0 62
benkatz 45:26801179208e 63 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 26:2b865c00d7e9 64 CANMessage rxMsg;
benkatz 26:2b865c00d7e9 65 CANMessage txMsg;
WinnieLiu 63:5946297ba2b0 66
WinnieLiu 63:5946297ba2b0 67
benkatz 44:8040fa2fcb0d 68 SPI drv_spi(PA_7, PA_6, PA_5);
benkatz 44:8040fa2fcb0d 69 DigitalOut drv_cs(PA_4);
benkatz 44:8040fa2fcb0d 70 //DigitalOut drv_en_gate(PA_11);
benkatz 44:8040fa2fcb0d 71 DRV832x drv(&drv_spi, &drv_cs);
WinnieLiu 63:5946297ba2b0 72
benkatz 26:2b865c00d7e9 73 PositionSensorAM5147 spi(16384, 0.0, NPP);
WinnieLiu 63:5946297ba2b0 74
benkatz 23:2adf23ee0305 75 volatile int count = 0;
benkatz 23:2adf23ee0305 76 volatile int state = REST_MODE;
benkatz 23:2adf23ee0305 77 volatile int state_change;
WinnieLiu 63:5946297ba2b0 78
WinnieLiu 60:f009e39e913e 79 void ChangeParameter(CANMessage msg) ;
WinnieLiu 64:fd695fb9865b 80 //bool complete_changepara = false;
WinnieLiu 63:5946297ba2b0 81
WinnieLiu 61:29371056a3de 82 /*Hall sensor calibration*/
WinnieLiu 60:f009e39e913e 83 volatile int hall_input = 1;
WinnieLiu 61:29371056a3de 84 volatile int hall_preinput = 1;
WinnieLiu 61:29371056a3de 85 volatile float cal_pcmd = 0;
WinnieLiu 61:29371056a3de 86 volatile float calibrate_speed = 0.25; // rad/s
WinnieLiu 61:29371056a3de 87 volatile float hall_presentpos = 0; //calibrate之前encoder的位置
WinnieLiu 61:29371056a3de 88 volatile float hall_in_pos = 0; //讀到1->0的位置(磁鐵進入hall sensor範圍)
WinnieLiu 61:29371056a3de 89 volatile float hall_out_pos = 0; //讀到0->1的位置(磁鐵出hall sensor範圍)
WinnieLiu 61:29371056a3de 90 volatile float hall_mid_pos = 0;
WinnieLiu 61:29371056a3de 91 volatile float calibrate_offset = 0; //rad
WinnieLiu 61:29371056a3de 92 volatile int calibrate_count = 0;
WinnieLiu 62:d43fcdd2d48b 93 volatile int calibrate_state = 0;
WinnieLiu 62:d43fcdd2d48b 94 //
WinnieLiu 63:5946297ba2b0 95
benkatz 26:2b865c00d7e9 96 void onMsgReceived() {
WinnieLiu 60:f009e39e913e 97 //static int can_state = 0;
benkatz 26:2b865c00d7e9 98 //msgAvailable = true;
benkatz 53:e85efce8c1eb 99 //printf("%d\n\r", rxMsg.id);
WinnieLiu 60:f009e39e913e 100 //CAN_DEBUG->write(1);
benkatz 26:2b865c00d7e9 101 can.read(rxMsg);
benkatz 28:8c7e29f719c5 102 if((rxMsg.id == CAN_ID)){
benkatz 28:8c7e29f719c5 103 controller.timeout = 0;
WinnieLiu 59:d53a7ccaae9a 104 //printf("%X\n\r",0x55);
benkatz 28:8c7e29f719c5 105 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))){
WinnieLiu 62:d43fcdd2d48b 106 //printf("can motor\n\r");
benkatz 28:8c7e29f719c5 107 state = MOTOR_MODE;
benkatz 28:8c7e29f719c5 108 state_change = 1;
WinnieLiu 60:f009e39e913e 109 }
benkatz 28:8c7e29f719c5 110 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))){
benkatz 28:8c7e29f719c5 111 state = REST_MODE;
benkatz 28:8c7e29f719c5 112 state_change = 1;
WinnieLiu 60:f009e39e913e 113 gpio.led->write(0);
WinnieLiu 60:f009e39e913e 114 }
benkatz 28:8c7e29f719c5 115 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))){
benkatz 28:8c7e29f719c5 116 spi.ZeroPosition();
WinnieLiu 60:f009e39e913e 117 controller.p_des = 0;
WinnieLiu 65:cb3497b549e9 118 state = SET_ZERO;
WinnieLiu 60:f009e39e913e 119 }
WinnieLiu 64:fd695fb9865b 120 // else if(state == REST_MODE && rxMsg.data[0]==0xFE){
WinnieLiu 64:fd695fb9865b 121 // printf("Change Parameters\n\r");
WinnieLiu 64:fd695fb9865b 122 // ChangeParameter(rxMsg);
WinnieLiu 64:fd695fb9865b 123 // complete_changepara = true;
WinnieLiu 64:fd695fb9865b 124 // }
WinnieLiu 61:29371056a3de 125 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))){
WinnieLiu 62:d43fcdd2d48b 126 //printf("Hall Calibration\n\r");
WinnieLiu 62:d43fcdd2d48b 127 calibrate_count = 0;
WinnieLiu 62:d43fcdd2d48b 128 calibrate_state = 1; // calibrating
WinnieLiu 61:29371056a3de 129 //step1:讀目前位置
WinnieLiu 61:29371056a3de 130 /*----- convert theta_mech to 0~359.9999deg -----*/
WinnieLiu 61:29371056a3de 131 hall_presentpos = controller.theta_mech;
WinnieLiu 62:d43fcdd2d48b 132 cal_pcmd = controller.theta_mech;
WinnieLiu 65:cb3497b549e9 133 static float _f_cal_round;
WinnieLiu 62:d43fcdd2d48b 134 modf(cal_pcmd/(2*PI),&_f_cal_round);
WinnieLiu 62:d43fcdd2d48b 135 cal_pcmd = cal_pcmd - _f_cal_round*2*PI;
WinnieLiu 63:5946297ba2b0 136 if(cal_pcmd < 0) cal_pcmd = cal_pcmd + 2*PI;
WinnieLiu 63:5946297ba2b0 137
WinnieLiu 63:5946297ba2b0 138 state = HALL_CALIBRATE ;
WinnieLiu 61:29371056a3de 139 state_change = 1;
WinnieLiu 61:29371056a3de 140 }
benkatz 28:8c7e29f719c5 141 else if(state == MOTOR_MODE){
benkatz 28:8c7e29f719c5 142 unpack_cmd(rxMsg, &controller);
WinnieLiu 60:f009e39e913e 143 }
WinnieLiu 65:cb3497b549e9 144 pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM, calibrate_state, state, controller.i_q_ref);
benkatz 37:c0f352d6e8e3 145 can.write(txMsg);
WinnieLiu 60:f009e39e913e 146 //can_state = can.write(txMsg);
WinnieLiu 60:f009e39e913e 147 //CAN_DEBUG->write(0);
WinnieLiu 60:f009e39e913e 148 //CAN_DEBUG->write(can_state);
benkatz 28:8c7e29f719c5 149 }
benkatz 26:2b865c00d7e9 150
benkatz 26:2b865c00d7e9 151 }
WinnieLiu 63:5946297ba2b0 152
WinnieLiu 60:f009e39e913e 153 //Use CAN Bus to change parameters
WinnieLiu 64:fd695fb9865b 154 //void ChangeParameter(CANMessage msg){
WinnieLiu 64:fd695fb9865b 155 // int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
WinnieLiu 64:fd695fb9865b 156 // int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
WinnieLiu 64:fd695fb9865b 157 // int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
WinnieLiu 64:fd695fb9865b 158 //
WinnieLiu 64:fd695fb9865b 159 // CAN_ID = msg.data[1];
WinnieLiu 64:fd695fb9865b 160 // MOTOR_KP = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
WinnieLiu 64:fd695fb9865b 161 // MOTOR_KI = uint_to_float(ki_int, KI_MIN, KI_MAX, 12);
WinnieLiu 64:fd695fb9865b 162 // MOTOR_KD = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
WinnieLiu 64:fd695fb9865b 163 //
WinnieLiu 64:fd695fb9865b 164 // // save to flash
WinnieLiu 64:fd695fb9865b 165 // if (!prefs.ready()) prefs.open();
WinnieLiu 64:fd695fb9865b 166 // prefs.flush(); // Write new prefs to flash
WinnieLiu 64:fd695fb9865b 167 // prefs.close();
WinnieLiu 64:fd695fb9865b 168 // prefs.load();
WinnieLiu 64:fd695fb9865b 169 // /*----- change new pid controller parameter -----*/
WinnieLiu 64:fd695fb9865b 170 // controller.kp = MOTOR_KP;
WinnieLiu 64:fd695fb9865b 171 // controller.ki = MOTOR_KI;
WinnieLiu 64:fd695fb9865b 172 // controller.kd = MOTOR_KD;
WinnieLiu 64:fd695fb9865b 173 //}
WinnieLiu 63:5946297ba2b0 174
benkatz 23:2adf23ee0305 175 void enter_menu_state(void){
benkatz 44:8040fa2fcb0d 176 drv.disable_gd();
WinnieLiu 65:cb3497b549e9 177 #ifdef VER2_0
WinnieLiu 65:cb3497b549e9 178 gpio.enable->write(0);
WinnieLiu 65:cb3497b549e9 179 #endif
WinnieLiu 65:cb3497b549e9 180
WinnieLiu 65:cb3497b549e9 181 #ifdef PRINT_UART
WinnieLiu 65:cb3497b549e9 182 printf("\n\r\n\r\n\r");
WinnieLiu 65:cb3497b549e9 183 printf(" Commands:\n\r");
WinnieLiu 65:cb3497b549e9 184 wait_us(10);
WinnieLiu 65:cb3497b549e9 185 printf(" m - Motor Mode\n\r");
WinnieLiu 65:cb3497b549e9 186 wait_us(10);
WinnieLiu 65:cb3497b549e9 187 printf(" c - Calibrate Encoder\n\r");
WinnieLiu 65:cb3497b549e9 188 wait_us(10);
WinnieLiu 65:cb3497b549e9 189 printf(" s - Setup\n\r");
WinnieLiu 65:cb3497b549e9 190 wait_us(10);
WinnieLiu 65:cb3497b549e9 191 printf(" e - Display Encoder\n\r");
WinnieLiu 65:cb3497b549e9 192 wait_us(10);
WinnieLiu 65:cb3497b549e9 193 printf(" z - Set Zero Position\n\r");
WinnieLiu 65:cb3497b549e9 194 wait_us(10);
WinnieLiu 65:cb3497b549e9 195 printf(" esc - Exit to Menu\n\r");
WinnieLiu 65:cb3497b549e9 196 wait_us(10);
WinnieLiu 65:cb3497b549e9 197 #endif
benkatz 23:2adf23ee0305 198 state_change = 0;
benkatz 37:c0f352d6e8e3 199 gpio.led->write(0);
WinnieLiu 62:d43fcdd2d48b 200 }
WinnieLiu 63:5946297ba2b0 201
benkatz 24:58c2d7571207 202 void enter_setup_state(void){
benkatz 24:58c2d7571207 203 printf("\n\r\n\r Configuration Options \n\r\n\n");
benkatz 44:8040fa2fcb0d 204 wait_us(10);
benkatz 51:6cd89bd6fcaa 205 printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
benkatz 44:8040fa2fcb0d 206 wait_us(10);
benkatz 28:8c7e29f719c5 207 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
benkatz 44:8040fa2fcb0d 208 wait_us(10);
WinnieLiu 55:b97b90d06ffa 209 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "o", "CAN ID", "0", "127", CAN_ID);
benkatz 44:8040fa2fcb0d 210 wait_us(10);
benkatz 28:8c7e29f719c5 211 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
benkatz 44:8040fa2fcb0d 212 wait_us(10);
benkatz 51:6cd89bd6fcaa 213 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX);
benkatz 51:6cd89bd6fcaa 214 wait_us(10);
benkatz 51:6cd89bd6fcaa 215 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX);
benkatz 44:8040fa2fcb0d 216 wait_us(10);
benkatz 28:8c7e29f719c5 217 printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
benkatz 44:8040fa2fcb0d 218 wait_us(10);
WinnieLiu 55:b97b90d06ffa 219
WinnieLiu 64:fd695fb9865b 220 printf(" %-4s %-31s %-5s %-6s %d\n\r", "a", "Calibration Direction", "-1", "1", CALIBRATE_DIR);
WinnieLiu 55:b97b90d06ffa 221 wait_us(10);
WinnieLiu 62:d43fcdd2d48b 222
WinnieLiu 63:5946297ba2b0 223 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "e", "Calibration offset", "0.0", "143.0", CALIBRATE_OFFSET);
WinnieLiu 63:5946297ba2b0 224 wait_us(10);
WinnieLiu 63:5946297ba2b0 225
WinnieLiu 64:fd695fb9865b 226 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "v", "Calibration Speed", "0.0", "10.0", CALIBRATE_SPEED);
WinnieLiu 63:5946297ba2b0 227 wait_us(10);
WinnieLiu 63:5946297ba2b0 228
WinnieLiu 62:d43fcdd2d48b 229 printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
WinnieLiu 60:f009e39e913e 230 wait_us(10);
WinnieLiu 62:d43fcdd2d48b 231 printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
WinnieLiu 62:d43fcdd2d48b 232 wait_us(10);
WinnieLiu 62:d43fcdd2d48b 233 printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD);
WinnieLiu 55:b97b90d06ffa 234 wait_us(10);
WinnieLiu 55:b97b90d06ffa 235
benkatz 24:58c2d7571207 236 printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
benkatz 44:8040fa2fcb0d 237 wait_us(10);
benkatz 24:58c2d7571207 238 state_change = 0;
benkatz 24:58c2d7571207 239 }
benkatz 22:60276ba87ac6 240
benkatz 23:2adf23ee0305 241 void enter_torque_mode(void){
WinnieLiu 65:cb3497b549e9 242 float _f_round, _f_p_des;
WinnieLiu 65:cb3497b549e9 243 _f_p_des = controller.theta_mech;
WinnieLiu 65:cb3497b549e9 244 modf(_f_p_des/(2*PI),&_f_round);
WinnieLiu 65:cb3497b549e9 245 _f_p_des = _f_p_des - _f_round*2*PI;
WinnieLiu 65:cb3497b549e9 246 if(_f_p_des < 0) _f_p_des = _f_p_des + 2*PI;
WinnieLiu 65:cb3497b549e9 247 controller.p_des = _f_p_des;
benkatz 44:8040fa2fcb0d 248 drv.enable_gd();
WinnieLiu 65:cb3497b549e9 249 #ifdef VER2_0
WinnieLiu 65:cb3497b549e9 250 gpio.enable->write(1);
WinnieLiu 65:cb3497b549e9 251 #endif
benkatz 37:c0f352d6e8e3 252 controller.ovp_flag = 0;
benkatz 28:8c7e29f719c5 253 reset_foc(&controller); // Tesets integrators, and other control loop parameters
benkatz 28:8c7e29f719c5 254 wait(.001);
benkatz 23:2adf23ee0305 255 controller.i_d_ref = 0;
benkatz 50:ba72df25d10f 256 controller.i_q_ref = 0; // Current Setpoints
benkatz 37:c0f352d6e8e3 257 gpio.led->write(1); // Turn on status LED
benkatz 25:f5741040c4bb 258 state_change = 0;
benkatz 28:8c7e29f719c5 259 printf("\n\r Entering Motor Mode \n\r");
WinnieLiu 65:cb3497b549e9 260 }
benkatz 22:60276ba87ac6 261
benkatz 23:2adf23ee0305 262 void calibrate(void){
benkatz 44:8040fa2fcb0d 263 drv.enable_gd();
WinnieLiu 65:cb3497b549e9 264 #ifdef VER2_0
WinnieLiu 65:cb3497b549e9 265 gpio.enable->write(1);
WinnieLiu 65:cb3497b549e9 266 #endif
benkatz 37:c0f352d6e8e3 267 gpio.led->write(1); // Turn on status LED
benkatz 25:f5741040c4bb 268 order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
benkatz 25:f5741040c4bb 269 calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure
benkatz 37:c0f352d6e8e3 270 gpio.led->write(0);; // Turn off status LED
benkatz 23:2adf23ee0305 271 wait(.2);
benkatz 23:2adf23ee0305 272 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
benkatz 44:8040fa2fcb0d 273 drv.disable_gd();
WinnieLiu 65:cb3497b549e9 274 #ifdef VER2_0
WinnieLiu 65:cb3497b549e9 275 gpio.enable->write(0);
WinnieLiu 65:cb3497b549e9 276 #endif
benkatz 23:2adf23ee0305 277 state_change = 0;
benkatz 23:2adf23ee0305 278 }
benkatz 23:2adf23ee0305 279
benkatz 23:2adf23ee0305 280 void print_encoder(void){
benkatz 48:74a40481740c 281 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
benkatz 48:74a40481740c 282 //printf("%d\n\r", spi.GetRawPosition());
benkatz 47:e1196a851f76 283 wait(.001);
benkatz 22:60276ba87ac6 284 }
WinnieLiu 63:5946297ba2b0 285
benkatz 23:2adf23ee0305 286 /// Current Sampling Interrupt ///
benkatz 23:2adf23ee0305 287 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
benkatz 2:8724412ad628 288 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
benkatz 2:8724412ad628 289 if (TIM1->SR & TIM_SR_UIF ) {
WinnieLiu 60:f009e39e913e 290 //GPIO_PID_DEBUG->write(1);
WinnieLiu 63:5946297ba2b0 291
benkatz 23:2adf23ee0305 292 ///Sample current always ///
benkatz 25:f5741040c4bb 293 ADC1->CR2 |= 0x40000000; // Begin sample and conversion
benkatz 22:60276ba87ac6 294 //volatile int delay;
benkatz 20:bf9ea5125d52 295 //for (delay = 0; delay < 55; delay++);
WinnieLiu 63:5946297ba2b0 296
benkatz 47:e1196a851f76 297 spi.Sample(DT); // sample position sensor
benkatz 37:c0f352d6e8e3 298 controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
benkatz 23:2adf23ee0305 299 controller.adc1_raw = ADC1->DR;
benkatz 37:c0f352d6e8e3 300 controller.adc3_raw = ADC3->DR;
benkatz 37:c0f352d6e8e3 301 controller.theta_elec = spi.GetElecPosition();
benkatz 37:c0f352d6e8e3 302 controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
benkatz 37:c0f352d6e8e3 303 controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
benkatz 37:c0f352d6e8e3 304 controller.dtheta_elec = spi.GetElecVelocity();
benkatz 51:6cd89bd6fcaa 305 controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement
WinnieLiu 63:5946297ba2b0 306
WinnieLiu 63:5946297ba2b0 307 // Calibration function
WinnieLiu 63:5946297ba2b0 308 if(calibrate_state == 0 || calibrate_state >= 2 ) ;
WinnieLiu 63:5946297ba2b0 309 else{
WinnieLiu 63:5946297ba2b0 310 // read hall sensor
WinnieLiu 63:5946297ba2b0 311 hall_input = HALL_IO->read() ;
WinnieLiu 63:5946297ba2b0 312 // calculate new pos
WinnieLiu 63:5946297ba2b0 313 if((CALIBRATE_DIR == 1 && controller.theta_mech >= hall_presentpos + 2*PI) || (CALIBRATE_DIR == -1 && controller.theta_mech <= hall_presentpos - 2*PI)){
WinnieLiu 63:5946297ba2b0 314 calibrate_state = 3 ;
WinnieLiu 63:5946297ba2b0 315 state = REST_MODE ;
WinnieLiu 63:5946297ba2b0 316 state_change = 1 ;
WinnieLiu 63:5946297ba2b0 317 }
WinnieLiu 63:5946297ba2b0 318 else{
WinnieLiu 63:5946297ba2b0 319 // step3: 馬達正反轉讀hall sensor(1: 沒有感應到磁鐵，0:有感應到磁鐵)
WinnieLiu 63:5946297ba2b0 320 // 記住1->0瞬間的位置(in_pos)，繼續旋轉
WinnieLiu 63:5946297ba2b0 321 // 記住0->1瞬間的位置(out_pos)，停止旋轉，計算in_pos與out_pos的平均值，再讓馬達慢慢轉到位置
WinnieLiu 63:5946297ba2b0 322 if(hall_input != hall_preinput ) {
WinnieLiu 63:5946297ba2b0 323 calibrate_count += 1 ;
WinnieLiu 63:5946297ba2b0 324 if(hall_input == 0) hall_in_pos = controller.theta_mech ;
WinnieLiu 63:5946297ba2b0 325 else{
WinnieLiu 63:5946297ba2b0 326 hall_out_pos = controller.theta_mech ;
WinnieLiu 63:5946297ba2b0 327 hall_mid_pos = (hall_in_pos + hall_out_pos)/2.0f ;
WinnieLiu 63:5946297ba2b0 328 }
WinnieLiu 63:5946297ba2b0 329 }
WinnieLiu 63:5946297ba2b0 330
WinnieLiu 63:5946297ba2b0 331 if(calibrate_count <= 1) cal_pcmd = cal_pcmd + CALIBRATE_DIR*(1.0f/(40000.0f)*CALIBRATE_SPEED ) ;
WinnieLiu 63:5946297ba2b0 332 else{
WinnieLiu 63:5946297ba2b0 333 if(CALIBRATE_DIR == 1 ){
WinnieLiu 63:5946297ba2b0 334 if(CALIBRATE_OFFSET == 0){
WinnieLiu 63:5946297ba2b0 335 //keep turning
WinnieLiu 63:5946297ba2b0 336 if(controller.theta_mech >= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
WinnieLiu 63:5946297ba2b0 337 else{//stop
WinnieLiu 63:5946297ba2b0 338 cal_pcmd = 0.0f;
WinnieLiu 63:5946297ba2b0 339 calibrate_state = 2; //success
WinnieLiu 63:5946297ba2b0 340 spi.ZeroPosition();
WinnieLiu 63:5946297ba2b0 341 //count = 0;
WinnieLiu 63:5946297ba2b0 342 //歸零
WinnieLiu 63:5946297ba2b0 343 calibrate_count = 0 ;
WinnieLiu 63:5946297ba2b0 344 state = MOTOR_MODE;
WinnieLiu 63:5946297ba2b0 345 }
WinnieLiu 63:5946297ba2b0 346 }
WinnieLiu 63:5946297ba2b0 347 else{
WinnieLiu 63:5946297ba2b0 348 if(controller.theta_mech <= hall_mid_pos + CALIBRATE_OFFSET*PI/180) cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
WinnieLiu 63:5946297ba2b0 349 else{ //stop
WinnieLiu 63:5946297ba2b0 350 cal_pcmd = 0.0f;
WinnieLiu 63:5946297ba2b0 351 calibrate_state = 2; //success
WinnieLiu 63:5946297ba2b0 352 spi.ZeroPosition();
WinnieLiu 63:5946297ba2b0 353 //歸零
WinnieLiu 63:5946297ba2b0 354 calibrate_count = 0 ;
WinnieLiu 63:5946297ba2b0 355 state = MOTOR_MODE;
WinnieLiu 63:5946297ba2b0 356 }
WinnieLiu 63:5946297ba2b0 357
WinnieLiu 63:5946297ba2b0 358
WinnieLiu 63:5946297ba2b0 359 }
WinnieLiu 63:5946297ba2b0 360 }
WinnieLiu 63:5946297ba2b0 361 else if(CALIBRATE_DIR == -1){
WinnieLiu 63:5946297ba2b0 362 if(CALIBRATE_OFFSET == 0){
WinnieLiu 63:5946297ba2b0 363 //keep turning
WinnieLiu 63:5946297ba2b0 364 if(controller.theta_mech <= hall_mid_pos) cal_pcmd = cal_pcmd - CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
WinnieLiu 63:5946297ba2b0 365 else{//stop
WinnieLiu 63:5946297ba2b0 366 cal_pcmd = 0.0f;
WinnieLiu 63:5946297ba2b0 367 calibrate_state = 2; //success
WinnieLiu 63:5946297ba2b0 368 spi.ZeroPosition();
WinnieLiu 63:5946297ba2b0 369 //歸零
WinnieLiu 63:5946297ba2b0 370 calibrate_count = 0 ;
WinnieLiu 63:5946297ba2b0 371 state = MOTOR_MODE;
WinnieLiu 63:5946297ba2b0 372 }
WinnieLiu 63:5946297ba2b0 373 }
WinnieLiu 63:5946297ba2b0 374 else{ //calibrate_offset != 0
WinnieLiu 63:5946297ba2b0 375 if(controller.theta_mech >= hall_mid_pos - CALIBRATE_OFFSET*PI/180) cal_pcmd = cal_pcmd + CALIBRATE_DIR*1.0f/40000.0f*CALIBRATE_SPEED ;
WinnieLiu 63:5946297ba2b0 376 else{ //stop
WinnieLiu 63:5946297ba2b0 377 cal_pcmd = 0.0f;
WinnieLiu 63:5946297ba2b0 378 calibrate_state = 2; //success
WinnieLiu 63:5946297ba2b0 379 spi.ZeroPosition();
WinnieLiu 63:5946297ba2b0 380 //歸零
WinnieLiu 63:5946297ba2b0 381 calibrate_count = 0 ;
WinnieLiu 63:5946297ba2b0 382 state = MOTOR_MODE;
WinnieLiu 63:5946297ba2b0 383 }
WinnieLiu 63:5946297ba2b0 384
WinnieLiu 63:5946297ba2b0 385 }
WinnieLiu 63:5946297ba2b0 386
WinnieLiu 63:5946297ba2b0 387
WinnieLiu 63:5946297ba2b0 388
WinnieLiu 63:5946297ba2b0 389 }
WinnieLiu 63:5946297ba2b0 390 }
WinnieLiu 63:5946297ba2b0 391
WinnieLiu 63:5946297ba2b0 392 cal_pcmd = (cal_pcmd>2*PI) ? cal_pcmd-=2*PI : cal_pcmd ;
WinnieLiu 63:5946297ba2b0 393 cal_pcmd = (cal_pcmd < 0) ? cal_pcmd+=2*PI : cal_pcmd ;
WinnieLiu 63:5946297ba2b0 394 controller.p_des = cal_pcmd ;
WinnieLiu 63:5946297ba2b0 395 }
WinnieLiu 63:5946297ba2b0 396
WinnieLiu 63:5946297ba2b0 397 hall_preinput = hall_input ;
WinnieLiu 63:5946297ba2b0 398 }
benkatz 20:bf9ea5125d52 399
benkatz 23:2adf23ee0305 400 /// Check state machine state, and run the appropriate function ///
benkatz 23:2adf23ee0305 401 switch(state){
benkatz 37:c0f352d6e8e3 402 case REST_MODE: // Do nothing
benkatz 23:2adf23ee0305 403 if(state_change){
WinnieLiu 62:d43fcdd2d48b 404 if(calibrate_state != 2) //success
WinnieLiu 62:d43fcdd2d48b 405 enter_menu_state();
WinnieLiu 62:d43fcdd2d48b 406 else{
WinnieLiu 63:5946297ba2b0 407 drv.disable_gd();
WinnieLiu 65:cb3497b549e9 408 #ifdef VER2_0
WinnieLiu 65:cb3497b549e9 409 gpio.enable->write(0);
WinnieLiu 65:cb3497b549e9 410 #endif
WinnieLiu 63:5946297ba2b0 411 state_change = 0;
WinnieLiu 63:5946297ba2b0 412 gpio.led->write(0);
benkatz 23:2adf23ee0305 413 }
WinnieLiu 62:d43fcdd2d48b 414 }
benkatz 23:2adf23ee0305 415 break;
benkatz 22:60276ba87ac6 416
benkatz 23:2adf23ee0305 417 case CALIBRATION_MODE: // Run encoder calibration procedure
benkatz 23:2adf23ee0305 418 if(state_change){
benkatz 23:2adf23ee0305 419 calibrate();
benkatz 23:2adf23ee0305 420 }
benkatz 23:2adf23ee0305 421 break;
benkatz 23:2adf23ee0305 422
benkatz 26:2b865c00d7e9 423 case MOTOR_MODE: // Run torque control
benkatz 25:f5741040c4bb 424 if(state_change){
benkatz 25:f5741040c4bb 425 enter_torque_mode();
benkatz 28:8c7e29f719c5 426 count = 0;
benkatz 25:f5741040c4bb 427 }
benkatz 28:8c7e29f719c5 428 else{
benkatz 37:c0f352d6e8e3 429 /*
benkatz 37:c0f352d6e8e3 430 if(controller.v_bus>28.0f){ //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen
benkatz 47:e1196a851f76 431 gpio.
benkatz 47:e1196a851f76 432 ->write(0);
benkatz 37:c0f352d6e8e3 433 controller.ovp_flag = 1;
benkatz 37:c0f352d6e8e3 434 state = REST_MODE;
benkatz 37:c0f352d6e8e3 435 state_change = 1;
benkatz 37:c0f352d6e8e3 436 printf("OVP Triggered!\n\r");
benkatz 37:c0f352d6e8e3 437 }
benkatz 37:c0f352d6e8e3 438 */
WinnieLiu 63:5946297ba2b0 439
WinnieLiu 60:f009e39e913e 440 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
WinnieLiu 60:f009e39e913e 441 controller.i_d_ref = 0;
WinnieLiu 60:f009e39e913e 442 controller.i_q_ref = 0;
WinnieLiu 60:f009e39e913e 443 controller.kp = 0;
WinnieLiu 60:f009e39e913e 444 controller.kd = 0;
WinnieLiu 60:f009e39e913e 445 controller.t_ff = 0;
benkatz 28:8c7e29f719c5 446 }
WinnieLiu 63:5946297ba2b0 447
WinnieLiu 63:5946297ba2b0 448 torque_control(&controller);
WinnieLiu 63:5946297ba2b0 449 commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
benkatz 47:e1196a851f76 450
WinnieLiu 63:5946297ba2b0 451 controller.timeout++;
WinnieLiu 63:5946297ba2b0 452 count++;
WinnieLiu 63:5946297ba2b0 453
WinnieLiu 63:5946297ba2b0 454 }
WinnieLiu 63:5946297ba2b0 455 break;
WinnieLiu 63:5946297ba2b0 456 case HALL_CALIBRATE:
WinnieLiu 63:5946297ba2b0 457 if(state_change){
WinnieLiu 63:5946297ba2b0 458 enter_torque_mode();
WinnieLiu 63:5946297ba2b0 459 count = 0;
WinnieLiu 63:5946297ba2b0 460 }
WinnieLiu 63:5946297ba2b0 461 else{
WinnieLiu 63:5946297ba2b0 462 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
WinnieLiu 63:5946297ba2b0 463 controller.i_d_ref = 0;
WinnieLiu 63:5946297ba2b0 464 controller.i_q_ref = 0;
WinnieLiu 63:5946297ba2b0 465 controller.kp = 0;
WinnieLiu 63:5946297ba2b0 466 controller.kd = 0;
WinnieLiu 63:5946297ba2b0 467 controller.t_ff = 0;
WinnieLiu 63:5946297ba2b0 468 }
WinnieLiu 63:5946297ba2b0 469
WinnieLiu 60:f009e39e913e 470 torque_control(&controller);
WinnieLiu 60:f009e39e913e 471 commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
WinnieLiu 63:5946297ba2b0 472
WinnieLiu 60:f009e39e913e 473 controller.timeout++;
WinnieLiu 60:f009e39e913e 474 count++;
WinnieLiu 63:5946297ba2b0 475
WinnieLiu 60:f009e39e913e 476 }
benkatz 23:2adf23ee0305 477 break;
benkatz 23:2adf23ee0305 478 case SETUP_MODE:
benkatz 23:2adf23ee0305 479 if(state_change){
benkatz 24:58c2d7571207 480 enter_setup_state();
benkatz 23:2adf23ee0305 481 }
benkatz 23:2adf23ee0305 482 break;
benkatz 23:2adf23ee0305 483 case ENCODER_MODE:
benkatz 23:2adf23ee0305 484 print_encoder();
benkatz 23:2adf23ee0305 485 break;
benkatz 37:c0f352d6e8e3 486 }
benkatz 2:8724412ad628 487 }
WinnieLiu 61:29371056a3de 488 TIM1->SR = 0x0; // reset the status register
WinnieLiu 60:f009e39e913e 489 //GPIO_PID_DEBUG->write(0);
benkatz 2:8724412ad628 490 }
WinnieLiu 63:5946297ba2b0 491
WinnieLiu 63:5946297ba2b0 492
benkatz 24:58c2d7571207 493 char cmd_val[8] = {0};
benkatz 24:58c2d7571207 494 char cmd_id = 0;
benkatz 25:f5741040c4bb 495 char char_count = 0;
WinnieLiu 63:5946297ba2b0 496
benkatz 25:f5741040c4bb 497 /// Manage state machine with commands from serial terminal or configurator gui ///
benkatz 25:f5741040c4bb 498 /// Called when data received over serial ///
benkatz 23:2adf23ee0305 499 void serial_interrupt(void){
benkatz 23:2adf23ee0305 500 while(pc.readable()){
benkatz 23:2adf23ee0305 501 char c = pc.getc();
benkatz 25:f5741040c4bb 502 if(c == 27){
benkatz 25:f5741040c4bb 503 state = REST_MODE;
benkatz 25:f5741040c4bb 504 state_change = 1;
benkatz 25:f5741040c4bb 505 char_count = 0;
benkatz 25:f5741040c4bb 506 cmd_id = 0;
benkatz 37:c0f352d6e8e3 507 gpio.led->write(0);;
benkatz 25:f5741040c4bb 508 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
benkatz 25:f5741040c4bb 509 }
benkatz 24:58c2d7571207 510 if(state == REST_MODE){
benkatz 23:2adf23ee0305 511 switch (c){
benkatz 23:2adf23ee0305 512 case 'c':
benkatz 23:2adf23ee0305 513 state = CALIBRATION_MODE;
benkatz 23:2adf23ee0305 514 state_change = 1;
benkatz 23:2adf23ee0305 515 break;
benkatz 26:2b865c00d7e9 516 case 'm':
benkatz 26:2b865c00d7e9 517 state = MOTOR_MODE;
benkatz 23:2adf23ee0305 518 state_change = 1;
benkatz 23:2adf23ee0305 519 break;
benkatz 23:2adf23ee0305 520 case 'e':
benkatz 23:2adf23ee0305 521 state = ENCODER_MODE;
benkatz 23:2adf23ee0305 522 state_change = 1;
benkatz 23:2adf23ee0305 523 break;
benkatz 23:2adf23ee0305 524 case 's':
benkatz 23:2adf23ee0305 525 state = SETUP_MODE;
benkatz 23:2adf23ee0305 526 state_change = 1;
benkatz 23:2adf23ee0305 527 break;
benkatz 37:c0f352d6e8e3 528 case 'z':
benkatz 37:c0f352d6e8e3 529 spi.SetMechOffset(0);
benkatz 47:e1196a851f76 530 spi.Sample(DT);
benkatz 37:c0f352d6e8e3 531 wait_us(20);
benkatz 37:c0f352d6e8e3 532 M_OFFSET = spi.GetMechPosition();
benkatz 37:c0f352d6e8e3 533 if (!prefs.ready()) prefs.open();
benkatz 37:c0f352d6e8e3 534 prefs.flush(); // Write new prefs to flash
benkatz 37:c0f352d6e8e3 535 prefs.close();
benkatz 37:c0f352d6e8e3 536 prefs.load();
benkatz 37:c0f352d6e8e3 537 spi.SetMechOffset(M_OFFSET);
benkatz 37:c0f352d6e8e3 538 printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET);
benkatz 37:c0f352d6e8e3 539
benkatz 37:c0f352d6e8e3 540 break;
benkatz 37:c0f352d6e8e3 541 }
benkatz 37:c0f352d6e8e3 542
benkatz 24:58c2d7571207 543 }
benkatz 24:58c2d7571207 544 else if(state == SETUP_MODE){
benkatz 25:f5741040c4bb 545 if(c == 13){
benkatz 24:58c2d7571207 546 switch (cmd_id){
benkatz 24:58c2d7571207 547 case 'b':
benkatz 24:58c2d7571207 548 I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
benkatz 24:58c2d7571207 549 break;
WinnieLiu 55:b97b90d06ffa 550 case 'o':
benkatz 24:58c2d7571207 551 CAN_ID = atoi(cmd_val);
benkatz 24:58c2d7571207 552 break;
benkatz 26:2b865c00d7e9 553 case 'm':
benkatz 26:2b865c00d7e9 554 CAN_MASTER = atoi(cmd_val);
benkatz 26:2b865c00d7e9 555 break;
benkatz 24:58c2d7571207 556 case 'l':
benkatz 51:6cd89bd6fcaa 557 I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f);
benkatz 51:6cd89bd6fcaa 558 break;
benkatz 51:6cd89bd6fcaa 559 case 'f':
benkatz 51:6cd89bd6fcaa 560 I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f);
benkatz 24:58c2d7571207 561 break;
benkatz 28:8c7e29f719c5 562 case 't':
benkatz 28:8c7e29f719c5 563 CAN_TIMEOUT = atoi(cmd_val);
benkatz 28:8c7e29f719c5 564 break;
WinnieLiu 64:fd695fb9865b 565 case 'a':
WinnieLiu 62:d43fcdd2d48b 566 CALIBRATE_DIR = atoi(cmd_val);
WinnieLiu 62:d43fcdd2d48b 567 break;
WinnieLiu 63:5946297ba2b0 568 case 'e':
WinnieLiu 63:5946297ba2b0 569 CALIBRATE_OFFSET = fmaxf(fminf(atof(cmd_val), 143.0f), 0.0f);
WinnieLiu 63:5946297ba2b0 570 break;
WinnieLiu 64:fd695fb9865b 571 case 'v':
WinnieLiu 63:5946297ba2b0 572 CALIBRATE_SPEED = fmaxf(fminf(atof(cmd_val), 10.0f), 0.0f);
WinnieLiu 63:5946297ba2b0 573 break;
WinnieLiu 55:b97b90d06ffa 574 case 'p':
WinnieLiu 55:b97b90d06ffa 575 MOTOR_KP = fmaxf(fminf(atof(cmd_val), KP_MAX), KP_MIN);;
WinnieLiu 55:b97b90d06ffa 576 break;
WinnieLiu 55:b97b90d06ffa 577 case 'i':
WinnieLiu 55:b97b90d06ffa 578 MOTOR_KI = fmaxf(fminf(atof(cmd_val), KI_MAX), KI_MIN);;
WinnieLiu 55:b97b90d06ffa 579 break;
WinnieLiu 55:b97b90d06ffa 580 case 'd':
WinnieLiu 55:b97b90d06ffa 581 MOTOR_KD = fmaxf(fminf(atof(cmd_val), KD_MAX), KD_MIN);
WinnieLiu 55:b97b90d06ffa 582 break;
benkatz 24:58c2d7571207 583 default:
WinnieLiu 64:fd695fb9865b 584 printf("ASCII: %d\n\r", c);
WinnieLiu 64:fd695fb9865b 585 printf("\n\rASCII: %d\n\r", cmd_id);
benkatz 24:58c2d7571207 586 printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
benkatz 24:58c2d7571207 587 break;
benkatz 24:58c2d7571207 588 }
benkatz 24:58c2d7571207 589
benkatz 24:58c2d7571207 590 if (!prefs.ready()) prefs.open();
benkatz 24:58c2d7571207 591 prefs.flush(); // Write new prefs to flash
benkatz 24:58c2d7571207 592 prefs.close();
WinnieLiu 58:636e46484432 593 prefs.load();
WinnieLiu 58:636e46484432 594 /*----- change new pid controller parameter -----*/
WinnieLiu 59:d53a7ccaae9a 595 controller.kp = MOTOR_KP;
WinnieLiu 58:636e46484432 596 controller.ki = MOTOR_KI;
WinnieLiu 58:636e46484432 597 controller.kd = MOTOR_KD;
WinnieLiu 58:636e46484432 598
benkatz 24:58c2d7571207 599 state_change = 1;
benkatz 24:58c2d7571207 600 char_count = 0;
benkatz 24:58c2d7571207 601 cmd_id = 0;
benkatz 24:58c2d7571207 602 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
benkatz 24:58c2d7571207 603 }
benkatz 24:58c2d7571207 604 else{
benkatz 24:58c2d7571207 605 if(char_count == 0){cmd_id = c;}
benkatz 24:58c2d7571207 606 else{
benkatz 24:58c2d7571207 607 cmd_val[char_count-1] = c;
benkatz 24:58c2d7571207 608
benkatz 24:58c2d7571207 609 }
benkatz 24:58c2d7571207 610 pc.putc(c);
benkatz 24:58c2d7571207 611 char_count++;
benkatz 23:2adf23ee0305 612 }
benkatz 23:2adf23ee0305 613 }
benkatz 24:58c2d7571207 614 else if (state == ENCODER_MODE){
benkatz 24:58c2d7571207 615 switch (c){
benkatz 24:58c2d7571207 616 case 27:
benkatz 24:58c2d7571207 617 state = REST_MODE;
benkatz 24:58c2d7571207 618 state_change = 1;
benkatz 24:58c2d7571207 619 break;
benkatz 24:58c2d7571207 620 }
benkatz 24:58c2d7571207 621 }
benkatz 49:83d83040ea51 622 else if (state == MOTOR_MODE){
benkatz 49:83d83040ea51 623 switch (c){
benkatz 49:83d83040ea51 624 case 'd':
benkatz 49:83d83040ea51 625 controller.i_q_ref = 0;
benkatz 49:83d83040ea51 626 controller.i_d_ref = 0;
benkatz 49:83d83040ea51 627 }
benkatz 49:83d83040ea51 628 }
benkatz 24:58c2d7571207 629
benkatz 24:58c2d7571207 630 }
benkatz 22:60276ba87ac6 631 }
WinnieLiu 63:5946297ba2b0 632
WinnieLiu 60:f009e39e913e 633 void GPIO_SETUP(){
WinnieLiu 60:f009e39e913e 634 //GPIOB->MODER |= 0x40000000;
WinnieLiu 60:f009e39e913e 635 //GPIOC->MODER |= 0x0000100f;
WinnieLiu 60:f009e39e913e 636 GPIOC->MODER |= 0x00000000;
WinnieLiu 60:f009e39e913e 637 }
benkatz 0:4e1c4df6aabd 638
benkatz 0:4e1c4df6aabd 639 int main() {
benkatz 20:bf9ea5125d52 640 controller.v_bus = V_BUS;
benkatz 22:60276ba87ac6 641 controller.mode = 0;
benkatz 23:2adf23ee0305 642 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
WinnieLiu 60:f009e39e913e 643 //DEBUG_GPIO();
WinnieLiu 60:f009e39e913e 644 GPIO_SETUP();
WinnieLiu 60:f009e39e913e 645
benkatz 44:8040fa2fcb0d 646 wait(.1);
benkatz 44:8040fa2fcb0d 647
benkatz 44:8040fa2fcb0d 648 gpio.enable->write(1);
benkatz 44:8040fa2fcb0d 649 wait_us(100);
benkatz 45:26801179208e 650 drv.calibrate();
benkatz 45:26801179208e 651 wait_us(100);
benkatz 44:8040fa2fcb0d 652 drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
benkatz 44:8040fa2fcb0d 653 wait_us(100);
benkatz 46:2d4b1dafcfe3 654 drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0);
benkatz 44:8040fa2fcb0d 655 wait_us(100);
benkatz 49:83d83040ea51 656 drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88);
benkatz 45:26801179208e 657
benkatz 45:26801179208e 658 //drv.enable_gd();
benkatz 44:8040fa2fcb0d 659 zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
benkatz 47:e1196a851f76 660 drv.disable_gd();
WinnieLiu 65:cb3497b549e9 661 #ifdef VER2_0
WinnieLiu 65:cb3497b549e9 662 gpio.enable->write(0);
WinnieLiu 65:cb3497b549e9 663 #endif
WinnieLiu 63:5946297ba2b0 664
benkatz 9:d7eb815cb057 665 wait(.1);
benkatz 44:8040fa2fcb0d 666 /*
benkatz 26:2b865c00d7e9 667 gpio.enable->write(1);
benkatz 26:2b865c00d7e9 668 TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles
benkatz 26:2b865c00d7e9 669 TIM1->CCR2 = 0x708*(1.0f);
benkatz 26:2b865c00d7e9 670 TIM1->CCR1 = 0x708*(1.0f);
benkatz 26:2b865c00d7e9 671 gpio.enable->write(0);
benkatz 44:8040fa2fcb0d 672 */
benkatz 23:2adf23ee0305 673 reset_foc(&controller); // Reset current controller
benkatz 48:74a40481740c 674 reset_observer(&observer); // Reset observer
benkatz 26:2b865c00d7e9 675 TIM1->CR1 ^= TIM_CR1_UDIS;
benkatz 26:2b865c00d7e9 676 //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
benkatz 20:bf9ea5125d52 677
benkatz 20:bf9ea5125d52 678 wait(.1);
benkatz 37:c0f352d6e8e3 679 NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication
benkatz 43:dfb72608639c 680
benkatz 37:c0f352d6e8e3 681 NVIC_SetPriority(CAN1_RX0_IRQn, 3);
benkatz 53:e85efce8c1eb 682 // attach 'CAN receive-complete' interrupt handler
benkatz 53:e85efce8c1eb 683
benkatz 53:e85efce8c1eb 684 // If preferences haven't been user configured yet, set defaults
benkatz 53:e85efce8c1eb 685 prefs.load(); // Read flash
benkatz 53:e85efce8c1eb 686
benkatz 53:e85efce8c1eb 687 can.filter(CAN_ID , 0xFFF, CANStandard, 0);
benkatz 43:dfb72608639c 688
benkatz 28:8c7e29f719c5 689 txMsg.id = CAN_MASTER;
WinnieLiu 60:f009e39e913e 690 txMsg.len = 8;
benkatz 26:2b865c00d7e9 691 rxMsg.len = 8;
benkatz 53:e85efce8c1eb 692 can.attach(&onMsgReceived);
benkatz 23:2adf23ee0305 693
benkatz 37:c0f352d6e8e3 694 if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
benkatz 37:c0f352d6e8e3 695 if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
benkatz 48:74a40481740c 696 if(isnan(I_BW) || I_BW==-1){I_BW = 1000;}
benkatz 51:6cd89bd6fcaa 697 if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;}
benkatz 51:6cd89bd6fcaa 698 if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;}
benkatz 48:74a40481740c 699 if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;}
benkatz 48:74a40481740c 700 if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;}
benkatz 48:74a40481740c 701 if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;}
WinnieLiu 60:f009e39e913e 702 if(isnan(MOTOR_KP) || MOTOR_KP==-1){MOTOR_KP = 5;}
WinnieLiu 60:f009e39e913e 703 if(isnan(MOTOR_KI) || MOTOR_KI==-1){MOTOR_KI = 0;}
WinnieLiu 60:f009e39e913e 704 if(isnan(MOTOR_KD) || MOTOR_KD==-1){MOTOR_KD = 1.25;}
WinnieLiu 62:d43fcdd2d48b 705 if(CALIBRATE_DIR != -1 && CALIBRATE_DIR != 1){CALIBRATE_DIR = 1;}
WinnieLiu 63:5946297ba2b0 706 if(isnan(CALIBRATE_OFFSET) || CALIBRATE_OFFSET==-1){CALIBRATE_OFFSET = 0.0;}
WinnieLiu 63:5946297ba2b0 707 if(isnan(CALIBRATE_SPEED) || CALIBRATE_SPEED==-1){CALIBRATE_SPEED = 0.25;}
WinnieLiu 60:f009e39e913e 708
benkatz 25:f5741040c4bb 709 spi.SetElecOffset(E_OFFSET); // Set position sensor offset
benkatz 37:c0f352d6e8e3 710 spi.SetMechOffset(M_OFFSET);
benkatz 23:2adf23ee0305 711 int lut[128] = {0};
benkatz 23:2adf23ee0305 712 memcpy(&lut, &ENCODER_LUT, sizeof(lut));
benkatz 25:f5741040c4bb 713 spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
benkatz 45:26801179208e 714 init_controller_params(&controller);
WinnieLiu 63:5946297ba2b0 715
WinnieLiu 59:d53a7ccaae9a 716 pc.baud(115200); // set serial baud rate
benkatz 20:bf9ea5125d52 717 wait(.01);
benkatz 23:2adf23ee0305 718 pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
benkatz 20:bf9ea5125d52 719 wait(.01);
benkatz 23:2adf23ee0305 720 printf("\n\r Debug Info:\n\r");
WinnieLiu 60:f009e39e913e 721 printf(" Firmware Version: %d\n\r", VERSION_NUM);
benkatz 23:2adf23ee0305 722 printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
benkatz 23:2adf23ee0305 723 printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET);
benkatz 37:c0f352d6e8e3 724 printf(" Output Zero Position: %.4f\n\r", M_OFFSET);
benkatz 24:58c2d7571207 725 printf(" CAN ID: %d\n\r", CAN_ID);
benkatz 44:8040fa2fcb0d 726
WinnieLiu 55:b97b90d06ffa 727 /*----- set controller parameters -----*/
WinnieLiu 60:f009e39e913e 728 controller.kp = MOTOR_KP;
WinnieLiu 60:f009e39e913e 729 controller.ki = MOTOR_KI;
WinnieLiu 60:f009e39e913e 730 controller.kd = MOTOR_KD;
WinnieLiu 62:d43fcdd2d48b 731 printf(" PID controller parameters \n\r");
WinnieLiu 62:d43fcdd2d48b 732 printf(" KP: %.3f, KI: %.3f, KD: %.3f \n\r", controller.kp, controller.ki, controller.kd);
WinnieLiu 62:d43fcdd2d48b 733 printf(" Calibrate Direction: %d\n\r", CALIBRATE_DIR);
WinnieLiu 63:5946297ba2b0 734 printf(" Calibrate Offset: %f\n\r", CALIBRATE_OFFSET);
WinnieLiu 63:5946297ba2b0 735 printf(" Calibrate Speed: %f\n\r", CALIBRATE_SPEED);
WinnieLiu 63:5946297ba2b0 736
benkatz 47:e1196a851f76 737 //printf(" %d\n\r", drv.read_register(DCR));
benkatz 47:e1196a851f76 738 //wait_us(100);
benkatz 47:e1196a851f76 739 //printf(" %d\n\r", drv.read_register(CSACR));
benkatz 47:e1196a851f76 740 //wait_us(100);
benkatz 47:e1196a851f76 741 //printf(" %d\n\r", drv.read_register(OCPCR));
benkatz 47:e1196a851f76 742 //drv.disable_gd();
benkatz 44:8040fa2fcb0d 743
benkatz 23:2adf23ee0305 744 pc.attach(&serial_interrupt); // attach serial interrupt
benkatz 22:60276ba87ac6 745
benkatz 23:2adf23ee0305 746 state_change = 1;
WinnieLiu 62:d43fcdd2d48b 747
WinnieLiu 62:d43fcdd2d48b 748 // calibrate
WinnieLiu 62:d43fcdd2d48b 749 calibrate_state = 0;
WinnieLiu 63:5946297ba2b0 750
benkatz 44:8040fa2fcb0d 751 int counter = 0;
benkatz 0:4e1c4df6aabd 752 while(1) {
WinnieLiu 64:fd695fb9865b 753 // if(complete_changepara){
WinnieLiu 64:fd695fb9865b 754 // printf(" %-4s %-31s %d\n\r", "o", "CAN_ID", CAN_ID);
WinnieLiu 64:fd695fb9865b 755 // printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "p", "MOTOR_KP", KP_MIN, KP_MAX, MOTOR_KP);
WinnieLiu 64:fd695fb9865b 756 // printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "i", "MOTOR_KI", KI_MIN, KI_MAX, MOTOR_KI);
WinnieLiu 64:fd695fb9865b 757 // printf(" %-4s %-31s %.2f %.2f %.2f\n\r", "d", "MOTOR_KD", KD_MIN, KD_MAX, MOTOR_KD);
WinnieLiu 64:fd695fb9865b 758 // complete_changepara = false;
WinnieLiu 64:fd695fb9865b 759 // state = REST_MODE;
WinnieLiu 64:fd695fb9865b 760 // state_change = 1;
WinnieLiu 64:fd695fb9865b 761 // }
WinnieLiu 62:d43fcdd2d48b 762
WinnieLiu 60:f009e39e913e 763
WinnieLiu 61:29371056a3de 764 // printf("Hall_input(PC6): %d\n\r", hall_input);
WinnieLiu 61:29371056a3de 765 // printf("Hall_preinput(PC6): %d\n\r", hall_preinput);
WinnieLiu 61:29371056a3de 766 // printf("calibrate count: %d\n\r",calibrate_count);
WinnieLiu 61:29371056a3de 767 // printf("cal_pcmd: %.4f\n\r",cal_pcmd);
WinnieLiu 61:29371056a3de 768 // printf("pos_in: %f\n\r",hall_in_pos);
WinnieLiu 61:29371056a3de 769 // printf("pos_out: %f\n\r",hall_out_pos);
WinnieLiu 61:29371056a3de 770 // printf("theta_mech: %f\n\r",controller.theta_mech);
WinnieLiu 62:d43fcdd2d48b 771 // printf("controller.pdes: %f\n\r", controller.p_des);
WinnieLiu 61:29371056a3de 772 // printf("state: %d\n\r",state);
WinnieLiu 61:29371056a3de 773 // wait(1);
WinnieLiu 60:f009e39e913e 774
WinnieLiu 60:f009e39e913e 775 /*
WinnieLiu 60:f009e39e913e 776 CAN_DEBUG->write(1);
WinnieLiu 60:f009e39e913e 777 pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT, VERSION_NUM);
WinnieLiu 60:f009e39e913e 778 can_state = can.write(txMsg);
WinnieLiu 60:f009e39e913e 779 CAN_DEBUG->write(can_state);
WinnieLiu 60:f009e39e913e 780 printf("helloworld\n\r");
WinnieLiu 60:f009e39e913e 781 wait(1);
WinnieLiu 60:f009e39e913e 782 */
WinnieLiu 63:5946297ba2b0 783
WinnieLiu 60:f009e39e913e 784 /*
WinnieLiu 60:f009e39e913e 785 CAN_DEBUG->write(1);
WinnieLiu 60:f009e39e913e 786 wait(0.1);
WinnieLiu 60:f009e39e913e 787 CAN_DEBUG->write(0);
WinnieLiu 60:f009e39e913e 788 wait(0.1);
WinnieLiu 60:f009e39e913e 789 */
WinnieLiu 59:d53a7ccaae9a 790 //drv.print_faults();
WinnieLiu 59:d53a7ccaae9a 791 //wait(.1);
benkatz 48:74a40481740c 792 //printf("%.4f\n\r", controller.v_bus);
benkatz 50:ba72df25d10f 793 /*
benkatz 47:e1196a851f76 794 if(state == MOTOR_MODE)
benkatz 47:e1196a851f76 795 {
benkatz 48:74a40481740c 796 //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
benkatz 49:83d83040ea51 797 //printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
benkatz 49:83d83040ea51 798 //printf("%.3f\n\r", controller.dtheta_mech);
benkatz 49:83d83040ea51 799 wait(.002);
benkatz 47:e1196a851f76 800 }
benkatz 50:ba72df25d10f 801 */
WinnieLiu 63:5946297ba2b0 802
benkatz 0:4e1c4df6aabd 803 }
WinnieLiu 63:5946297ba2b0 804 }