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