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
00045 CANMessage   rxMsg;
00046 CANMessage   txMsg;
00047 
00048 
00049 
00050 PositionSensorAM5147 spi(16384, 0.0, 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(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 
00399     }
00400 }