kinoko john
hh
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 00036 PreferenceWriter prefs(6); 00037 00038 GPIOStruct gpio; 00039 ControllerStruct controller; 00040 ObserverStruct observer; 00041 COMStruct com; 00042 Serial pc(PA_2, PA_3); 00043 00044 00045 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name 00046 CANMessage rxMsg; 00047 CANMessage txMsg; 00048 00049 00050 SPI drv_spi(PA_7, PA_6, PA_5); 00051 DigitalOut drv_cs(PA_4); 00052 //DigitalOut drv_en_gate(PA_11); 00053 DRV832x drv(&drv_spi, &drv_cs); 00054 00055 PositionSensorAM5147 spi(16384, 0.0, NPP); 00056 00057 volatile int count = 0; 00058 volatile int state = REST_MODE; 00059 volatile int state_change; 00060 00061 void onMsgReceived() { 00062 //msgAvailable = true; 00063 //printf("%d\n\r", rxMsg.id); 00064 can.read(rxMsg); 00065 if((rxMsg.id == CAN_ID)){ 00066 controller.timeout = 0; 00067 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))){ 00068 state = MOTOR_MODE; 00069 state_change = 1; 00070 } 00071 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))){ 00072 state = REST_MODE; 00073 state_change = 1; 00074 gpio.led->write(0);; 00075 } 00076 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))){ 00077 spi.ZeroPosition(); 00078 } 00079 else if(state == MOTOR_MODE){ 00080 unpack_cmd(rxMsg, &controller); 00081 } 00082 pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); 00083 can.write(txMsg); 00084 } 00085 00086 } 00087 00088 void enter_menu_state(void){ 00089 drv.disable_gd(); 00090 //gpio.enable->write(0); 00091 printf("\n\r\n\r\n\r"); 00092 printf(" Commands:\n\r"); 00093 wait_us(10); 00094 printf(" m - Motor Mode\n\r"); 00095 wait_us(10); 00096 printf(" c - Calibrate Encoder\n\r"); 00097 wait_us(10); 00098 printf(" s - Setup\n\r"); 00099 wait_us(10); 00100 printf(" e - Display Encoder\n\r"); 00101 wait_us(10); 00102 printf(" z - Set Zero Position\n\r"); 00103 wait_us(10); 00104 printf(" esc - Exit to Menu\n\r"); 00105 wait_us(10); 00106 state_change = 0; 00107 gpio.led->write(0); 00108 } 00109 00110 void enter_setup_state(void){ 00111 printf("\n\r\n\r Configuration Options \n\r\n\n"); 00112 wait_us(10); 00113 printf(" %-4s %-31s %-5s %-6s %-2s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); 00114 wait_us(10); 00115 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); 00116 wait_us(10); 00117 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); 00118 wait_us(10); 00119 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); 00120 wait_us(10); 00121 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Current Limit (A)", "0.0", "40.0", I_MAX); 00122 wait_us(10); 00123 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "f", "FW Current Limit (A)", "0.0", "33.0", I_FW_MAX); 00124 wait_us(10); 00125 printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); 00126 wait_us(10); 00127 printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); 00128 wait_us(10); 00129 state_change = 0; 00130 } 00131 00132 void enter_torque_mode(void){ 00133 drv.enable_gd(); 00134 //gpio.enable->write(1); 00135 controller.ovp_flag = 0; 00136 reset_foc(&controller); // Tesets integrators, and other control loop parameters 00137 wait(.001); 00138 controller.i_d_ref = 0; 00139 controller.i_q_ref = 0; // Current Setpoints 00140 gpio.led->write(1); // Turn on status LED 00141 state_change = 0; 00142 printf("\n\r Entering Motor Mode \n\r"); 00143 } 00144 00145 void calibrate(void){ 00146 drv.enable_gd(); 00147 //gpio.enable->write(1); 00148 gpio.led->write(1); // Turn on status LED 00149 order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering 00150 calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure 00151 gpio.led->write(0);; // Turn off status LED 00152 wait(.2); 00153 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); 00154 drv.disable_gd(); 00155 //gpio.enable->write(0); 00156 state_change = 0; 00157 } 00158 00159 void print_encoder(void){ 00160 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); 00161 //printf("%d\n\r", spi.GetRawPosition()); 00162 wait(.001); 00163 } 00164 00165 /// Current Sampling Interrupt /// 00166 /// This runs at 40 kHz, regardless of of the mode the controller is in /// 00167 extern "C" void TIM1_UP_TIM10_IRQHandler(void) { 00168 if (TIM1->SR & TIM_SR_UIF ) { 00169 00170 ///Sample current always /// 00171 ADC1->CR2 |= 0x40000000; // Begin sample and conversion 00172 //volatile int delay; 00173 //for (delay = 0; delay < 55; delay++); 00174 00175 spi.Sample(DT); // sample position sensor 00176 controller.adc2_raw = ADC2->DR; // Read ADC Data Registers 00177 controller.adc1_raw = ADC1->DR; 00178 controller.adc3_raw = ADC3->DR; 00179 controller.theta_elec = spi.GetElecPosition(); 00180 controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); 00181 controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); 00182 controller.dtheta_elec = spi.GetElecVelocity(); 00183 controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; //filter the dc link voltage measurement 00184 /// 00185 00186 /// Check state machine state, and run the appropriate function /// 00187 switch(state){ 00188 case REST_MODE: // Do nothing 00189 if(state_change){ 00190 enter_menu_state(); 00191 } 00192 break; 00193 00194 case CALIBRATION_MODE: // Run encoder calibration procedure 00195 if(state_change){ 00196 calibrate(); 00197 } 00198 break; 00199 00200 case MOTOR_MODE: // Run torque control 00201 if(state_change){ 00202 enter_torque_mode(); 00203 count = 0; 00204 } 00205 else{ 00206 /* 00207 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 00208 gpio. 00209 ->write(0); 00210 controller.ovp_flag = 1; 00211 state = REST_MODE; 00212 state_change = 1; 00213 printf("OVP Triggered!\n\r"); 00214 } 00215 */ 00216 00217 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ 00218 controller.i_d_ref = 0; 00219 controller.i_q_ref = 0; 00220 controller.kp = 0; 00221 controller.kd = 0; 00222 controller.t_ff = 0; 00223 } 00224 00225 torque_control(&controller); 00226 commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop 00227 00228 controller.timeout++; 00229 count++; 00230 00231 } 00232 break; 00233 case SETUP_MODE: 00234 if(state_change){ 00235 enter_setup_state(); 00236 } 00237 break; 00238 case ENCODER_MODE: 00239 print_encoder(); 00240 break; 00241 } 00242 } 00243 TIM1->SR = 0x0; // reset the status register 00244 } 00245 00246 00247 char cmd_val[8] = {0}; 00248 char cmd_id = 0; 00249 char char_count = 0; 00250 00251 /// Manage state machine with commands from serial terminal or configurator gui /// 00252 /// Called when data received over serial /// 00253 void serial_interrupt(void){ 00254 while(pc.readable()){ 00255 char c = pc.getc(); 00256 if(c == 27){ 00257 state = REST_MODE; 00258 state_change = 1; 00259 char_count = 0; 00260 cmd_id = 0; 00261 gpio.led->write(0);; 00262 for(int i = 0; i<8; i++){cmd_val[i] = 0;} 00263 } 00264 if(state == REST_MODE){ 00265 switch (c){ 00266 case 'c': 00267 state = CALIBRATION_MODE; 00268 state_change = 1; 00269 break; 00270 case 'm': 00271 state = MOTOR_MODE; 00272 state_change = 1; 00273 break; 00274 case 'e': 00275 state = ENCODER_MODE; 00276 state_change = 1; 00277 break; 00278 case 's': 00279 state = SETUP_MODE; 00280 state_change = 1; 00281 break; 00282 case 'z': 00283 spi.SetMechOffset(0); 00284 spi.Sample(DT); 00285 wait_us(20); 00286 M_OFFSET = spi.GetMechPosition(); 00287 if (!prefs.ready()) prefs.open(); 00288 prefs.flush(); // Write new prefs to flash 00289 prefs.close(); 00290 prefs.load(); 00291 spi.SetMechOffset(M_OFFSET); 00292 printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); 00293 00294 break; 00295 } 00296 00297 } 00298 else if(state == SETUP_MODE){ 00299 if(c == 13){ 00300 switch (cmd_id){ 00301 case 'b': 00302 I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); 00303 break; 00304 case 'i': 00305 CAN_ID = atoi(cmd_val); 00306 break; 00307 case 'm': 00308 CAN_MASTER = atoi(cmd_val); 00309 break; 00310 case 'l': 00311 I_MAX = fmaxf(fminf(atof(cmd_val), 40.0f), 0.0f); 00312 break; 00313 case 'f': 00314 I_FW_MAX = fmaxf(fminf(atof(cmd_val), 33.0f), 0.0f); 00315 break; 00316 case 't': 00317 CAN_TIMEOUT = atoi(cmd_val); 00318 break; 00319 default: 00320 printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); 00321 break; 00322 } 00323 00324 if (!prefs.ready()) prefs.open(); 00325 prefs.flush(); // Write new prefs to flash 00326 prefs.close(); 00327 prefs.load(); 00328 state_change = 1; 00329 char_count = 0; 00330 cmd_id = 0; 00331 for(int i = 0; i<8; i++){cmd_val[i] = 0;} 00332 } 00333 else{ 00334 if(char_count == 0){cmd_id = c;} 00335 else{ 00336 cmd_val[char_count-1] = c; 00337 00338 } 00339 pc.putc(c); 00340 char_count++; 00341 } 00342 } 00343 else if (state == ENCODER_MODE){ 00344 switch (c){ 00345 case 27: 00346 state = REST_MODE; 00347 state_change = 1; 00348 break; 00349 } 00350 } 00351 else if (state == MOTOR_MODE){ 00352 switch (c){ 00353 case 'd': 00354 controller.i_q_ref = 0; 00355 controller.i_d_ref = 0; 00356 } 00357 } 00358 00359 } 00360 } 00361 00362 int main() { 00363 controller.v_bus = V_BUS; 00364 controller.mode = 0; 00365 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO 00366 wait(.1); 00367 00368 gpio.enable->write(1); 00369 wait_us(100); 00370 drv.calibrate(); 00371 wait_us(100); 00372 drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); 00373 wait_us(100); 00374 drv.write_CSACR(0x0, 0x1, 0x0, CSA_GAIN_40, 0x0, 0x0, 0x0, 0x0, SEN_LVL_1_0); 00375 wait_us(100); 00376 drv.write_OCPCR(TRETRY_4MS, DEADTIME_200NS, OCP_RETRY, OCP_DEG_8US, VDS_LVL_1_88); 00377 00378 //drv.enable_gd(); 00379 zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset 00380 drv.disable_gd(); 00381 00382 wait(.1); 00383 /* 00384 gpio.enable->write(1); 00385 TIM1->CCR3 = 0x708*(1.0f); // Write duty cycles 00386 TIM1->CCR2 = 0x708*(1.0f); 00387 TIM1->CCR1 = 0x708*(1.0f); 00388 gpio.enable->write(0); 00389 */ 00390 reset_foc(&controller); // Reset current controller 00391 reset_observer(&observer); // Reset observer 00392 TIM1->CR1 ^= TIM_CR1_UDIS; 00393 //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt 00394 00395 wait(.1); 00396 NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication 00397 00398 NVIC_SetPriority(CAN1_RX0_IRQn, 3); 00399 // attach 'CAN receive-complete' interrupt handler 00400 00401 // If preferences haven't been user configured yet, set defaults 00402 prefs.load(); // Read flash 00403 00404 can.filter(CAN_ID , 0xFFF, CANStandard, 0); 00405 00406 txMsg.id = CAN_MASTER; 00407 txMsg.len = 6; 00408 rxMsg.len = 8; 00409 can.attach(&onMsgReceived); 00410 00411 if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} 00412 if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} 00413 if(isnan(I_BW) || I_BW==-1){I_BW = 1000;} 00414 if(isnan(I_MAX) || I_MAX ==-1){I_MAX=40;} 00415 if(isnan(I_FW_MAX) || I_FW_MAX ==-1){I_FW_MAX=0;} 00416 if(isnan(CAN_ID) || CAN_ID==-1){CAN_ID = 1;} 00417 if(isnan(CAN_MASTER) || CAN_MASTER==-1){CAN_MASTER = 0;} 00418 if(isnan(CAN_TIMEOUT) || CAN_TIMEOUT==-1){CAN_TIMEOUT = 0;} 00419 spi.SetElecOffset(E_OFFSET); // Set position sensor offset 00420 spi.SetMechOffset(M_OFFSET); 00421 int lut[128] = {0}; 00422 memcpy(&lut, &ENCODER_LUT, sizeof(lut)); 00423 spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table 00424 init_controller_params(&controller); 00425 00426 pc.baud(921600); // set serial baud rate 00427 wait(.01); 00428 pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); 00429 wait(.01); 00430 printf("\n\r Debug Info:\n\r"); 00431 printf(" Firmware Version: %s\n\r", VERSION_NUM); 00432 printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); 00433 printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); 00434 printf(" Output Zero Position: %.4f\n\r", M_OFFSET); 00435 printf(" CAN ID: %d\n\r", CAN_ID); 00436 00437 00438 00439 00440 //printf(" %d\n\r", drv.read_register(DCR)); 00441 //wait_us(100); 00442 //printf(" %d\n\r", drv.read_register(CSACR)); 00443 //wait_us(100); 00444 //printf(" %d\n\r", drv.read_register(OCPCR)); 00445 //drv.disable_gd(); 00446 00447 pc.attach(&serial_interrupt); // attach serial interrupt 00448 00449 state_change = 1; 00450 00451 00452 int counter = 0; 00453 while(1) { 00454 drv.print_faults(); 00455 wait(.1); 00456 //printf("%.4f\n\r", controller.v_bus); 00457 /* 00458 if(state == MOTOR_MODE) 00459 { 00460 //printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); 00461 //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); 00462 //printf("%.3f\n\r", controller.dtheta_mech); 00463 wait(.002); 00464 } 00465 */ 00466 00467 } 00468 }
Generated on Fri Jul 22 2022 01:43:47 by
1.7.2