Winnie Liu / Hobbyking_Cheetah_0511

modified 0511

Dependencies:   mbed-dev FastPWM3

main.cpp@62:d43fcdd2d48b, 2022-03-07 (annotated)

Committer:
WinnieLiu
Date:
Mon Mar 07 01:39:20 2022 +0000
Revision:
62:d43fcdd2d48b
Parent:
61:29371056a3de
Child:
63:5946297ba2b0
add calibration mode

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