Winnie Liu / Hobbyking_Cheetah_0511

modified 0511

Dependencies:   mbed-dev FastPWM3

main.cpp@64:fd695fb9865b, 2022-03-22 (annotated)

Committer:
WinnieLiu
Date:
Tue Mar 22 15:02:43 2022 +0000
Revision:
64:fd695fb9865b
Parent:
63:5946297ba2b0
Child:
65:cb3497b549e9
change setup menu;

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