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