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