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