WANG YUCHAO
/
Motor_DRV8323RH_for_20190
mit
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 bayleyw, nkirkby, scolton, 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 00006 #define REST_MODE 0 00007 #define CALIBRATION_MODE 1 00008 #define MOTOR_MODE 2 00009 #define SETUP_MODE 4 00010 #define ENCODER_MODE 5 00011 00012 #define VERSION_NUM "1.6" 00013 00014 00015 float __float_reg[64]; // Floats stored in flash 00016 int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table 00017 00018 #include "mbed.h" 00019 #include "PositionSensor.h" 00020 #include "structs.h" 00021 #include "foc.h" 00022 #include "calibration.h" 00023 #include "hw_setup.h" 00024 #include "math_ops.h" 00025 #include "current_controller_config.h" 00026 #include "hw_config.h" 00027 #include "motor_config.h" 00028 #include "stm32f4xx_flash.h" 00029 #include "FlashWriter.h" 00030 #include "user_config.h" 00031 #include "PreferenceWriter.h" 00032 #include "CAN_com.h" 00033 00034 00035 PreferenceWriter prefs(6); 00036 00037 GPIOStruct gpio; 00038 ControllerStruct controller; 00039 COMStruct com; 00040 ObserverStruct observer; 00041 Serial pc(PA_2, PA_3); 00042 00043 00044 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name, 1000kbps 00045 CANMessage rxMsg; 00046 CANMessage txMsg; 00047 00048 00049 00050 PositionSensorAM5147 spi(16384, 0.0, NPP); //14 bits encoder, 21 NPP 00051 00052 //volatile int count = 0; 00053 volatile int state = REST_MODE; 00054 volatile int state_change; 00055 00056 void onMsgReceived() { 00057 //msgAvailable = true; 00058 00059 can.read(rxMsg); 00060 if((rxMsg.id == CAN_ID)){ 00061 controller.timeout = 0; 00062 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))){ 00063 state = MOTOR_MODE; 00064 state_change = 1; 00065 } 00066 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))){ 00067 state = REST_MODE; 00068 state_change = 1; 00069 gpio.led->write(0);; 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]==0xFE))){ 00072 spi.ZeroPosition(); 00073 } 00074 else if(state == MOTOR_MODE){ 00075 unpack_cmd(rxMsg, &controller); 00076 } 00077 pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT); 00078 can.write(txMsg); 00079 } 00080 00081 } 00082 00083 void enter_menu_state(void){ 00084 printf("\n\r\n\r\n\r"); 00085 printf(" Commands:\n\r"); 00086 wait_us(10); 00087 printf(" m - Motor Mode\n\r"); 00088 wait_us(10); 00089 printf(" c - Calibrate Encoder\n\r"); 00090 wait_us(10); 00091 printf(" s - Setup\n\r"); 00092 wait_us(10); 00093 printf(" e - Display Encoder\n\r"); 00094 wait_us(10); 00095 printf(" z - Set Zero Position\n\r"); 00096 wait_us(10); 00097 printf(" esc - Exit to Menu\n\r"); 00098 wait_us(10); 00099 state_change = 0; 00100 gpio.enable->write(0); 00101 gpio.led->write(0); 00102 } 00103 00104 void enter_setup_state(void){ 00105 printf("\n\r\n\r Configuration Options \n\r\n\n"); 00106 wait_us(10); 00107 printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value"); 00108 wait_us(10); 00109 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW); 00110 wait_us(10); 00111 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID); 00112 wait_us(10); 00113 printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER); 00114 wait_us(10); 00115 printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT); 00116 wait_us(10); 00117 printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT); 00118 wait_us(10); 00119 printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r"); 00120 wait_us(10); 00121 state_change = 0; 00122 } 00123 00124 void enter_torque_mode(void){ 00125 controller.ovp_flag = 0; 00126 gpio.enable->write(1); // Enable gate drive 00127 reset_foc(&controller); // Tesets integrators, and other control loop parameters 00128 wait(.001); 00129 controller.i_d_ref = 0; 00130 controller.i_q_ref = 0; // Current Setpoints 00131 gpio.led->write(1); // Turn on status LED 00132 state_change = 0; 00133 printf("\n\r Entering Motor Mode \n\r"); 00134 } 00135 00136 void calibrate(void){ 00137 gpio.enable->write(1); // Enable gate drive 00138 gpio.led->write(1); // Turn on status LED 00139 order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering 00140 calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure 00141 gpio.led->write(0);; // Turn off status LED 00142 wait(.2); 00143 gpio.enable->write(0); // Turn off gate drive 00144 printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r"); 00145 state_change = 0; 00146 } 00147 00148 void print_encoder(void){ 00149 printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); 00150 wait(.05); 00151 } 00152 00153 /// Current Sampling Interrupt /// 00154 /// This runs at 40 kHz, regardless of of the mode the controller is in /// 00155 extern "C" void TIM1_UP_TIM10_IRQHandler(void) { 00156 if (TIM1->SR & TIM_SR_UIF ) { 00157 00158 ///Sample current always /// 00159 ADC1->CR2 |= 0x40000000; // Begin sample and conversion 00160 //volatile int delay; 00161 //for (delay = 0; delay < 55; delay++); 00162 controller.adc2_raw = ADC2->DR; // Read ADC Data Registers 00163 controller.adc1_raw = ADC1->DR; 00164 controller.adc3_raw = ADC3->DR; 00165 spi.Sample(DT); // sample position sensor 00166 controller.theta_elec = spi.GetElecPosition(); 00167 controller.theta_mech = (1.0f/GR)*spi.GetMechPosition(); 00168 controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity(); 00169 controller.dtheta_elec = spi.GetElecVelocity(); 00170 controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE; 00171 /// 00172 00173 /// Check state machine state, and run the appropriate function /// 00174 switch(state){ 00175 case REST_MODE: // Do nothing 00176 if(state_change){ 00177 enter_menu_state(); 00178 } 00179 break; 00180 00181 case CALIBRATION_MODE: // Run encoder calibration procedure 00182 if(state_change){ 00183 calibrate(); 00184 } 00185 break; 00186 00187 case MOTOR_MODE: // Run torque control 00188 if(state_change){ 00189 enter_torque_mode(); 00190 // count = 0; 00191 } 00192 else{ 00193 /* 00194 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 00195 gpio.enable->write(0); 00196 controller.ovp_flag = 1; 00197 state = REST_MODE; 00198 state_change = 1; 00199 printf("OVP Triggered!\n\r"); 00200 } 00201 */ 00202 00203 torque_control(&controller); 00204 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){ 00205 controller.i_d_ref = 0; 00206 controller.i_q_ref = 0; 00207 controller.kp = 0; 00208 controller.kd = 0; 00209 controller.t_ff = 0; 00210 } 00211 commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop 00212 controller.timeout += 1; 00213 00214 /* 00215 count++; 00216 if(count == 4000){ 00217 printf("%.4f\n\r", controller.dtheta_mech); 00218 count = 0; 00219 } 00220 */ 00221 00222 00223 } 00224 break; 00225 case SETUP_MODE: 00226 if(state_change){ 00227 enter_setup_state(); 00228 } 00229 break; 00230 case ENCODER_MODE: 00231 print_encoder(); 00232 break; 00233 } 00234 } 00235 TIM1->SR = 0x0; // reset the status register 00236 } 00237 00238 00239 char cmd_val[8] = {0}; 00240 char cmd_id = 0; 00241 char char_count = 0; 00242 00243 /// Manage state machine with commands from serial terminal or configurator gui /// 00244 /// Called when data received over serial /// 00245 void serial_interrupt(void){ 00246 while(pc.readable()){ 00247 char c = pc.getc(); 00248 if(c == 27){ 00249 state = REST_MODE; 00250 state_change = 1; 00251 char_count = 0; 00252 cmd_id = 0; 00253 gpio.led->write(0);; 00254 for(int i = 0; i<8; i++){cmd_val[i] = 0;} 00255 } 00256 if(state == REST_MODE){ 00257 switch (c){ 00258 case 'c': 00259 state = CALIBRATION_MODE; 00260 state_change = 1; 00261 break; 00262 case 'm': 00263 state = MOTOR_MODE; 00264 state_change = 1; 00265 break; 00266 case 'e': 00267 state = ENCODER_MODE; 00268 state_change = 1; 00269 break; 00270 case 's': 00271 state = SETUP_MODE; 00272 state_change = 1; 00273 break; 00274 case 'z': 00275 spi.SetMechOffset(0); 00276 spi.Sample(DT); 00277 wait_us(20); 00278 M_OFFSET = spi.GetMechPosition(); 00279 if (!prefs.ready()) prefs.open(); 00280 prefs.flush(); // Write new prefs to flash 00281 prefs.close(); 00282 prefs.load(); 00283 spi.SetMechOffset(M_OFFSET); 00284 printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET); 00285 00286 break; 00287 } 00288 00289 } 00290 else if(state == SETUP_MODE){ 00291 if(c == 13){ 00292 switch (cmd_id){ 00293 case 'b': 00294 I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f); 00295 break; 00296 case 'i': 00297 CAN_ID = atoi(cmd_val); 00298 break; 00299 case 'm': 00300 CAN_MASTER = atoi(cmd_val); 00301 break; 00302 case 'l': 00303 TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f); 00304 break; 00305 case 't': 00306 CAN_TIMEOUT = atoi(cmd_val); 00307 break; 00308 default: 00309 printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id); 00310 break; 00311 } 00312 00313 if (!prefs.ready()) prefs.open(); 00314 prefs.flush(); // Write new prefs to flash 00315 prefs.close(); 00316 prefs.load(); 00317 state_change = 1; 00318 char_count = 0; 00319 cmd_id = 0; 00320 for(int i = 0; i<8; i++){cmd_val[i] = 0;} 00321 } 00322 else{ 00323 if(char_count == 0){cmd_id = c;} 00324 else{ 00325 cmd_val[char_count-1] = c; 00326 00327 } 00328 pc.putc(c); 00329 char_count++; 00330 } 00331 } 00332 else if (state == ENCODER_MODE){ 00333 switch (c){ 00334 case 27: 00335 state = REST_MODE; 00336 state_change = 1; 00337 break; 00338 } 00339 } 00340 00341 } 00342 } 00343 00344 int main() { 00345 00346 controller.v_bus = V_BUS; 00347 controller.mode = 0; 00348 Init_All_HW(&gpio); // Setup PWM, ADC, GPIO 00349 00350 wait(.1); 00351 gpio.enable->write(1); 00352 TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles 00353 TIM1->CCR2 = PWM_ARR*(1.0f); 00354 TIM1->CCR1 = PWM_ARR*(1.0f); 00355 zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset 00356 gpio.enable->write(0); 00357 reset_foc(&controller); // Reset current controller 00358 TIM1->CR1 ^= TIM_CR1_UDIS; 00359 //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt 00360 00361 wait(.1); 00362 NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication 00363 00364 NVIC_SetPriority(CAN1_RX0_IRQn, 3); 00365 can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); 00366 00367 txMsg.id = CAN_MASTER; 00368 txMsg.len = 6; 00369 rxMsg.len = 8; 00370 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler 00371 00372 prefs.load(); // Read flash 00373 if(isnan(E_OFFSET)){E_OFFSET = 0.0f;} 00374 if(isnan(M_OFFSET)){M_OFFSET = 0.0f;} 00375 spi.SetElecOffset(E_OFFSET); // Set position sensor offset 00376 spi.SetMechOffset(M_OFFSET); 00377 int lut[128] = {0}; 00378 memcpy(&lut, &ENCODER_LUT, sizeof(lut)); 00379 spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table 00380 00381 pc.baud(115200);//pc.baud(921600); // set serial baud rate 00382 wait(.01); 00383 pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r"); 00384 wait(.01); 00385 printf("\n\r Debug Info:\n\r"); 00386 printf(" Firmware Version: %s\n\r", VERSION_NUM); 00387 printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset); 00388 printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET); 00389 printf(" Output Zero Position: %.4f\n\r", M_OFFSET); 00390 printf(" CAN ID: %d\n\r", CAN_ID); 00391 00392 pc.attach(&serial_interrupt); // attach serial interrupt 00393 00394 state_change = 1; 00395 00396 00397 while(1) { 00398 if(state == MOTOR_MODE) 00399 { 00400 //printf(" %.3f %.3f %.3f \n\r", controller.p_des,controller.p_des-controller.theta_mech,controller.v_des-controller.dtheta_mech); //spi.GetMechPosition 00401 //printf(" %.3f %d %d \n\r", controller.v_bus,controller.adc1_raw,controller.adc2_raw); //spi.GetMechPosition 00402 // printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance); 00403 //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); 00404 // printf("%.3f\n\r", controller.dtheta_mech); 00405 wait(.002); 00406 } 00407 } 00408 }
Generated on Wed Jul 13 2022 03:55:42 by 1.7.2