Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /// high-bandwidth 3-phase motor control, for robots 00002 /// Written by benkatz, with much inspiration from Bayley Wang, Nick Kirkby, Shane Colton, David Otten, and others 00003 /// Hardware documentation can be found at build-its.blogspot.com 00004 /// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling 00005 /// Version for the TI DRV8323 Everything Chip 00006 00007 #define REST_MODE 0 00008 #define CALIBRATION_MODE 1 00009 #define MOTOR_MODE 2 00010 #define SETUP_MODE 4 00011 #define ENCODER_MODE 5 00012 00013 #define VERSION_NUM "1.9" 00014 00015 00016 float __float_reg[64]; // Floats stored in flash 00017 int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table 00018 00019 #include "mbed.h" 00020 #include "PositionSensor.h" 00021 #include "structs.h" 00022 #include "foc.h" 00023 #include "calibration.h" 00024 #include "hw_setup.h" 00025 #include "math_ops.h" 00026 #include "current_controller_config.h" 00027 #include "hw_config.h" 00028 #include "motor_config.h" 00029 #include "stm32f4xx_flash.h" 00030 #include "FlashWriter.h" 00031 #include "user_config.h" 00032 #include "PreferenceWriter.h" 00033 #include "CAN_com.h" 00034 #include "DRV.h" 00035 #include "Position_Torque_Sensor.h" 00036 00037 PreferenceWriter prefs(6); 00038 00039 GPIOStruct gpio; 00040 ControllerStruct controller; 00041 ControllerStruct waijie_controller; 00042 ObserverStruct observer; 00043 COMStruct com; 00044 Serial pc(PA_2, PA_3); 00045 00046 00047 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name 00048 CANMessage rxMsg; 00049 CANMessage txMsg; 00050 00051 00052 SPI drv_spi(PC_12, PC_11, PC_10); //MOSI MISO CLK (Electrical Board spi3) 00053 DigitalOut drv_cs(PA_15); 00054 //DigitalOut drv_en_gate(PC_9); 00055 DRV832x drv(&drv_spi, &drv_cs); 00056 00057 PositionSensorAM5147 spi(16384, 0.0, NPP); 00058 SensorAM5147 spi1(16384,0.0,NPP); 00059 00060 volatile int count = 0; 00061 volatile int state = REST_MODE; 00062 volatile int state_change; 00063 volatile int counthjb = 0; 00064 volatile int testcount = 0; 00065 void onMsgReceived() { 00066 //msgAvailable = true; 00067 counthjb++; 00068 if(counthjb>100) 00069 {printf("%df\n\r", rxMsg.id); 00070 counthjb=0; 00071 } 00072 can.read(rxMsg); 00073 if((rxMsg.id == CAN_ID)){ 00074 controller.timeout = 0; 00075 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))){ 00076 state = MOTOR_MODE; 00077 state_change = 1; 00078 } 00079 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))){ 00080 state = REST_MODE; 00081 state_change = 1; 00082 gpio.led->write(0);; 00083 } 00084 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))){ 00085 spi.ZeroPosition(); 00086 } 00087 else if(state == MOTOR_MODE){ 00088 unpack_cmd(rxMsg, &controller); 00089 //Test for Can Communication 00090 printf("p_des :%.3f\n\r",controller.p_des); 00091 printf("v_des :%.3f\n\r",controller.v_des); 00092 printf("kp :%.3f\n\r",controller.kp); 00093 printf("kd :%.3f\n\r",controller.kd); 00094 printf("t_ff :%.3f\n\r",controller.t_ff); 00095 } 00096 pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); 00097 can.write(txMsg); 00098 } 00099 00100 } 00101 00102 void enter_menu_state(void){ 00103 drv.disable_gd(); 00104 //gpio.enable->write(0); 00105 printf("\n\r\n\r\n\r"); 00106 printf(" Commands:\n\r"); 00107 wait_us(10); 00108 printf(" m - Motor Mode\n\r"); 00109 wait_us(10); 00110 printf(" c - Calibrate Encoder\n\r"); 00111 wait_us(10); 00112 printf(" s - Setup\n\r"); 00113 wait_us(10); 00114 printf(" e - Display Encoder\n\r"); 00115 wait_us(10); 00116 printf(" z - Set Zero Position\n\r"); 00117 wait_us(10); 00118 printf(" esc - Exit to Menu\n\r"); 00119 wait_us(10); 00120 state_change = 0; 00121 gpio.led->write(0); 00122 } 00123 00124 void enter_setup_state(void){ 00125 printf("\n\r\n\r Configuration Options \n\r\n\n"); 00126 wait_us(10); 00127 printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); 00128 wait_us(10); 00129 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); 00130 wait_us(10); 00131 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); 00132 wait_us(10); 00133 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); 00134 wait_us(10); 00135 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX); 00136 wait_us(10); 00137 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX); 00138 wait_us(10); 00139 printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); 00140 wait_us(10); 00141 printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); 00142 wait_us(10); 00143 state_change = 0; 00144 } 00145 00146 void enter_torque_mode(void){ 00147 drv.enable_gd(); 00148 //gpio.enable->write(1); 00149 controller.ovp_flag = 0; 00150 reset_foc(&controller); // Tesets integrators, and other control loop parameters 00151 wait(.001); 00152 controller.i_d_ref = 0; 00153 controller.i_q_ref = 0; // Current Setpoints 00154 gpio.led->write(1); // Turn on status LED 00155 state_change = 0; 00156 printf("\n\r Entering Motor Mode \n\r"); 00157 } 00158 00159 void calibrate(void){ 00160 drv.enable_gd(); 00161 //gpio.enable->write(1); 00162 gpio.led->write(1); // Turn on status LED 00163 order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering 00164 calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure 00165 gpio.led->write(0);; // Turn off status LED 00166 wait(.2); 00167 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); 00168 drv.disable_gd(); 00169 //gpio.enable->write(0); 00170 state_change = 0; 00171 } 00172 00173 void print_encoder(void){ 00174 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); 00175 //printf("%d\n\r", spi.GetRawPosition()); 00176 wait(.001); 00177 } 00178 00179 /// Current Sampling Interrupt /// 00180 /// This runs at 40 kHz, regardless of of the mode the controller is in /// 00181 extern "C" void TIM1_UP_TIM10_IRQHandler(void) { 00182 if (TIM1->SR & TIM_SR_UIF ) { 00183 00184 ///Sample current always /// 00185 ADC1->CR2 |= 0x40000000; // Begin sample and conversion 00186 //volatile int delay; 00187 //for (delay = 0; delay < 55; delay++); 00188 00189 spi.Sample(DT); // sample position sensor 00190 spi1.Sample(DT); // sample position sensor 00191 controller.adc2_raw = ADC2->DR; // Read ADC Data Registers 00192 controller.adc1_raw = ADC1->DR; 00193 controller.adc3_raw = ADC3->DR; 00194 controller.theta_elec = spi.GetElecPosition(); 00195 controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); 00196 controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); 00197 controller.dtheta_elec = spi.GetElecVelocity(); 00198 controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement 00199 /// 00200 /************Test for AM5147P Position Sensor*********/ 00201 00202 //controller.adc2_raw = ADC2->DR; // Read ADC Data Registers 00203 //controller.adc1_raw = ADC1->DR; 00204 //controller.adc3_raw = ADC3->DR; 00205 waijie_controller.theta_elec = spi1.GetElecPosition(); 00206 waijie_controller.theta_mech = spi1.GetMechPosition(); 00207 waijie_controller.dtheta_mech =spi1.GetMechVelocity(); 00208 waijie_controller.dtheta_elec = spi1.GetElecVelocity(); 00209 //controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement 00210 00211 testcount++; 00212 if(testcount==40000) 00213 { 00214 printf("waijie:%.3f Raw:%.3f\n\r", waijie_controller.theta_mech,spi1.GetRawPosition()); 00215 printf("neizhi:%.3f Raw:%.3f\n\r", GR*controller.theta_mech,spi.GetRawPosition()); 00216 testcount=0; 00217 } 00218 00219 /************Test for AM5147P Position Sensor*********/ 00220 /// Check state machine state, and run the appropriate function /// 00221 switch(state){ 00222 case REST_MODE: // Do nothing 00223 if(state_change){ 00224 enter_menu_state(); 00225 } 00226 break; 00227 00228 case CALIBRATION_MODE: // Run encoder calibration procedure 00229 if(state_change){ 00230 calibrate(); 00231 } 00232 break; 00233 00234 case MOTOR_MODE: // Run torque control 00235 if(state_change){ 00236 enter_torque_mode(); 00237 count = 0; 00238 } 00239 else{ 00240 /* 00241 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 00242 gpio. 00243 ->write(0); 00244 controller.ovp_flag = 1; 00245 state = REST_MODE; 00246 state_change = 1; 00247 printf("OVP Triggered!\n\r"); 00248 } 00249 */ 00250 00251 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ 00252 controller.i_d_ref = 0; 00253 controller.i_q_ref = 0; 00254 controller.kp = 0; 00255 controller.kd = 0; 00256 controller.t_ff = 0; 00257 } 00258 00259 torque_control(&controller); 00260 commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop 00261 00262 controller.timeout++; 00263 count++; 00264 00265 } 00266 break; 00267 case SETUP_MODE: 00268 if(state_change){ 00269 enter_setup_state(); 00270 } 00271 break; 00272 case ENCODER_MODE: 00273 print_encoder(); 00274 break; 00275 } 00276 } 00277 TIM1->SR = 0x0; // reset the status register 00278 } 00279 00280 00281 char cmd_val[8] = {0}; 00282 char cmd_id = 0; 00283 char char_count = 0; 00284 00285 /// Manage state machine with commands from serial terminal or configurator gui /// 00286 /// Called when data received over serial /// 00287 void serial_interrupt(void){ 00288 while(pc.readable()){ 00289 char c = pc.getc(); 00290 if(c == 27){ 00291 //===============================================HJB added====================================================// 00292 wait_us(100); //HJB add, to clear fault 00293 drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault 00294 //Init_pos = controller.theta_mech; //Input the local mechanical theta 00295 //===============================================HJB ended====================================================// 00296 state = REST_MODE; 00297 state_change = 1; 00298 char_count = 0; 00299 cmd_id = 0; 00300 gpio.led->write(0);; 00301 for(int i = 0; i<8; i++){cmd_val[i] = 0;} 00302 } 00303 if(state == REST_MODE){ 00304 switch (c){ 00305 case 'c': 00306 state = CALIBRATION_MODE; 00307 state_change = 1; 00308 break; 00309 case 'm': 00310 state = MOTOR_MODE; 00311 state_change = 1; 00312 break; 00313 case 'e': 00314 state = ENCODER_MODE; 00315 state_change = 1; 00316 break; 00317 case 's': 00318 state = SETUP_MODE; 00319 state_change = 1; 00320 break; 00321 case 'z': 00322 spi.SetMechOffset(0); 00323 spi.Sample(DT); 00324 wait_us(20); 00325 M_OFFSET = spi.GetMechPosition(); 00326 if (!prefs.ready()) prefs.open(); 00327 prefs.flush(); // Write new prefs to flash 00328 prefs.close(); 00329 prefs.load(); 00330 spi.SetMechOffset(M_OFFSET); 00331 printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); 00332 00333 break; 00334 } 00335 00336 } 00337 else if(state == SETUP_MODE){ 00338 if(c == 13){ 00339 switch (cmd_id){ 00340 case 'b': 00341 I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); 00342 break; 00343 case 'i': 00344 CAN_ID = atoi(cmd_val); 00345 break; 00346 case 'm': 00347 CAN_MASTER = atoi(cmd_val); 00348 break; 00349 case 'l': 00350 I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f); 00351 break; 00352 case 'f': 00353 I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f); 00354 break; 00355 case 't': 00356 CAN_TIMEOUT = atoi(cmd_val); 00357 break; 00358 default: 00359 printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); 00360 break; 00361 } 00362 00363 if (!prefs.ready()) prefs.open(); 00364 prefs.flush(); // Write new prefs to flash 00365 prefs.close(); 00366 prefs.load(); 00367 state_change = 1; 00368 char_count = 0; 00369 cmd_id = 0; 00370 for(int i = 0; i<8; i++){cmd_val[i] = 0;} 00371 } 00372 else{ 00373 if(char_count == 0){cmd_id = c;} 00374 else{ 00375 cmd_val[char_count-1] = c; 00376 00377 } 00378 pc.putc(c); 00379 char_count++; 00380 } 00381 } 00382 else if (state == ENCODER_MODE){ 00383 switch (c){ 00384 case 27: 00385 state = REST_MODE; 00386 state_change = 1; 00387 break; 00388 } 00389 } 00390 else if (state == MOTOR_MODE){ 00391 switch (c){ 00392 case 'd': 00393 controller.i_q_ref = 0; 00394 controller.i_d_ref = 0; 00395 } 00396 } 00397 00398 } 00399 } 00400 00401 int main() { 00402 wait(1); //hjb added 00403 controller.v_bus = V_BUS; 00404 controller.mode = 0; 00405 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO 00406 wait(.1); 00407 00408 gpio.enable->write(1); 00409 wait_us(100); 00410 drv.calibrate(); 00411 wait_us(100); 00412 drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); 00413 wait_us(100); 00414 drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0); 00415 wait_us(100); 00416 drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88); 00417 00418 //drv.enable_gd(); 00419 zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset 00420 drv.disable_gd(); 00421 00422 wait(.1); 00423 /* 00424 gpio.enable->write(1); 00425 TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles 00426 TIM1->CCR2 = 0x708*(1.0f); 00427 TIM1->CCR1 = 0x708*(1.0f); 00428 gpio.enable->write(0); 00429 */ 00430 reset_foc(&controller); // Reset current controller 00431 reset_observer(&observer); // Reset observer 00432 TIM1->CR1 ^= TIM_CR1_UDIS; 00433 //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt 00434 00435 wait(.1); 00436 NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication 00437 00438 NVIC_SetPriority(CAN1_RX0_IRQn, 3); 00439 can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); 00440 00441 txMsg.id = CAN_MASTER; 00442 txMsg.len = 6; 00443 rxMsg.len = 8; 00444 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler 00445 00446 // If preferences haven't been user configured yet, set defaults 00447 prefs.load(); // Read flash 00448 if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} 00449 if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} 00450 if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} 00451 if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;} 00452 if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;} 00453 if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} 00454 if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} 00455 if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;} 00456 spi.SetElecOffset(E_OFFSET); // Set position sensor offset 00457 spi.SetMechOffset(M_OFFSET); 00458 int lut[128] = {0}; 00459 memcpy(&lut, &ENCODER_LUT, sizeof(lut)); 00460 spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table 00461 init_controller_params(&controller); 00462 00463 //pc.baud(460800); 00464 pc.baud(115200); // set serial baud rate 00465 wait(.01); 00466 pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); 00467 wait(.01); 00468 printf("\n\r Debug Info:\n\r"); 00469 printf(" Firmware Version: %s\n\r", VERSION_NUM); 00470 printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); 00471 printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); 00472 printf(" Output Zero Position: %.4f\n\r", M_OFFSET); 00473 printf(" CAN ID: %d\n\r", CAN_ID); 00474 00475 00476 00477 00478 //printf(" %d\n\r", drv.read_register(DCR)); 00479 //wait_us(100); 00480 //printf(" %d\n\r", drv.read_register(CSACR)); 00481 //wait_us(100); 00482 //printf(" %d\n\r", drv.read_register(OCPCR)); 00483 //drv.disable_gd(); 00484 00485 pc.attach(&serial_interrupt); // attach serial interrupt 00486 00487 state_change = 1; 00488 00489 00490 int counter = 0; 00491 //===============================================HJB added====================================================// 00492 wait_us(100); //HJB add, to clear fault 00493 drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault 00494 //Init_pos = controller.theta_mech; //Input the local mechanical theta 00495 //===============================================HJB ended====================================================// 00496 while(1) { 00497 drv.print_faults(); 00498 wait(.1); 00499 //printf("%.4f\n\r", controller.v_bus); 00500 00501 if(state == MOTOR_MODE) 00502 { 00503 //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); 00504 //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); 00505 //printf("%.3f\n\r", controller.dtheta_mech); 00506 /********Test for AM5147P Position Sensor********/ 00507 // printf("%.3f\n\r", GR*waijie_controller.theta_mech); 00508 /********Test for AM5147P Position Sensor********/ 00509 printf("enter enter enter\n\r"); 00510 wait(.002); 00511 } 00512 00513 //pc.printf("%.3f\n\r", GR*waijie_controller.theta_mech); 00514 //wait(.1); 00515 } 00516 }
Generated on Wed Jul 13 2022 19:53:03 by
1.7.2