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 #define SPEED_MODE 6
00012 #define Position_MODE 7
00013 
00014 #define VERSION_NUM "1.6"
00015 
00016 
00017 float __float_reg[64];                                                          // Floats stored in flash
00018 int __int_reg[256];                                                             // Ints stored in flash.  Includes position sensor calibration lookup table
00019 
00020 #include "mbed.h"
00021 #include "PositionSensor.h"
00022 #include "structs.h"
00023 #include "foc.h"
00024 #include "calibration.h"
00025 #include "hw_setup.h"
00026 #include "math_ops.h" 
00027 #include "current_controller_config.h"
00028 #include "hw_config.h"
00029 #include "motor_config.h"
00030 #include "stm32f4xx_flash.h"
00031 #include "FlashWriter.h"
00032 #include "user_config.h"
00033 #include "PreferenceWriter.h"
00034 #include "CAN_com.h"
00035 
00036  
00037 PreferenceWriter prefs(6);
00038 
00039 GPIOStruct gpio;
00040 ControllerStruct controller;
00041 COMStruct com;
00042 ObserverStruct observer;
00043 Serial pc(PA_2, PA_3);
00044 
00045 
00046 CAN          can(PB_8, PB_9, 800000);      // CAN Rx pin name, CAN Tx pin name, 1000kbps
00047 CANMessage   rxMsg;
00048 CANMessage   txMsg;
00049 
00050 
00051 
00052 PositionSensorAM5147 spi(16384, 0.0, NPP);    //14 bits encoder, 21 NPP
00053 
00054 volatile int count = 0;
00055 volatile int state = REST_MODE;
00056 volatile int state_change;
00057 
00058 void onMsgReceived() {
00059     //msgAvailable = true;
00060    
00061     can.read(rxMsg);  
00062     if((rxMsg.id == CAN_ID)){
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 //************WYC ADD **********2021.11.04**************//
00074         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]==0xFA))){
00075             state = SPEED_MODE;
00076             state_change = 1;
00077             gpio.led->write(1);
00078             }
00079 //************WYC ADD **********2021.11.04**************//
00080 //************YZ ADD **********2021.11.05**************//
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]==0xFB))){
00082             state = Position_MODE;
00083             state_change = 1;
00084             gpio.led->write(1);
00085             }
00086 //************YZ ADD **********2021.11.05**************//
00087         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))){
00088       /*
00089             quanshu=spi.aa;
00090             guanjieweizhi=spi.bb;
00091          if (!prefs.ready()) prefs.open();
00092                         prefs.flush();                                                  // Write new prefs to flash
00093                         prefs.close();    
00094                         prefs.load(); 
00095             */            
00096             spi.ZeroPosition();
00097             
00098             }
00099 /*
00100         else if(state == MOTOR_MODE){
00101             unpack_cmd(rxMsg, &controller);
00102             }
00103 */
00104         else if(state == MOTOR_MODE ||state == SPEED_MODE||state == Position_MODE)
00105             { //WYC ADD 2021.11.04
00106             unpack_cmd(rxMsg, &controller);
00107             }
00108         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
00109         can.write(txMsg);
00110         }
00111     
00112 }
00113 
00114 void enter_menu_state(void){
00115     printf("\n\r\n\r\n\r");
00116     printf(" Commands:\n\r");
00117     wait_us(10);
00118     printf(" m - Motor Mode\n\r");
00119     wait_us(10);
00120     printf(" c - Calibrate Encoder\n\r");
00121     wait_us(10);
00122     printf(" s - Setup\n\r");
00123     wait_us(10);
00124     printf(" e - Display Encoder\n\r");
00125     wait_us(10);
00126     printf(" z - Set Zero Position\n\r");
00127     wait_us(10);
00128     printf(" esc - Exit to Menu\n\r");
00129     wait_us(10);
00130     state_change = 0;
00131     gpio.enable->write(0);
00132     gpio.led->write(0);
00133     }
00134 
00135 void enter_setup_state(void){
00136     printf("\n\r\n\r Configuration Options \n\r\n\n");
00137     wait_us(10);
00138     printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
00139     wait_us(10);
00140     printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
00141     wait_us(10);
00142     printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
00143     wait_us(10);
00144     printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
00145     wait_us(10);
00146     printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
00147     wait_us(10);
00148     printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
00149     wait_us(10);
00150     printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
00151     wait_us(10);
00152     state_change = 0;
00153     }
00154     
00155 void enter_torque_mode(void){
00156     controller.ovp_flag = 0;
00157     gpio.enable->write(1);                                                      // Enable gate drive
00158     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
00159     wait(.001);
00160     controller.i_d_ref = 0;
00161     controller.i_q_ref = 0;                                                     // Current Setpoints
00162     gpio.led->write(1);                                                     // Turn on status LED
00163     state_change = 0;
00164     printf("\n\r Entering Motor Mode \n\r");
00165     }
00166 
00167 //****************WYC ADD ************2021.11.04**********//
00168     void enter_speed_mode(void){
00169     controller.ovp_flag = 0;
00170     gpio.enable->write(1);                                                      // Enable gate drive
00171     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
00172     wait(.001);
00173     controller.i_d_ref = 0;
00174     controller.i_q_ref = 0;                                                     // Current Setpoints
00175     gpio.led->write(1);                                                     // Turn on status LED
00176     state_change = 0;
00177     printf("\n\r Entering SPEED Mode \n\r");
00178     }
00179 //****************WYC ADD ************2021.11.04**********//
00180 
00181 //****************YZ ADD ************2021.11.05**********//
00182     void enter_Position_mode(void){
00183     controller.ovp_flag = 0;
00184     gpio.enable->write(1);                                                      // Enable gate drive
00185     reset_foc(&controller);                                                     // Tesets integrators, and other control loop parameters
00186     wait(.001);
00187     controller.i_d_ref = 0;
00188     controller.i_q_ref = 0;                                                     // Current Setpoints
00189     gpio.led->write(1);                                                     // Turn on status LED
00190     state_change = 0;
00191     printf("\n\r Entering Position Mode \n\r");
00192     }
00193 //****************YZ ADD ************2021.11.05**********//
00194     
00195 void calibrate(void){
00196     gpio.enable->write(1);                                                      // Enable gate drive
00197     gpio.led->write(1);                                                    // Turn on status LED
00198     order_phases(&spi, &gpio, &controller, &prefs);                             // Check phase ordering
00199     calibrate(&spi, &gpio, &controller, &prefs);                                // Perform calibration procedure
00200     gpio.led->write(0);;                                                   // Turn off status LED
00201     wait(.2);
00202     gpio.enable->write(0);                                                      // Turn off gate drive
00203     printf("\n\r Calibration complete.  Press 'esc' to return to menu\n\r");
00204      state_change = 0;
00205     }
00206     
00207 void print_encoder(void){
00208     printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
00209     wait(.05);
00210     }
00211 
00212 /// Current Sampling Interrupt ///
00213 /// This runs at 40 kHz, regardless of of the mode the controller is in ///
00214 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
00215   if (TIM1->SR & TIM_SR_UIF ) {
00216 
00217         ///Sample current always ///
00218         ADC1->CR2  |= 0x40000000;                                               // Begin sample and conversion
00219         //volatile int delay;   
00220         //for (delay = 0; delay < 55; delay++);
00221         
00222         
00223         
00224         controller.adc2_raw = ADC2->DR;                                         // Read ADC Data Registers
00225         controller.adc1_raw = ADC1->DR;
00226         controller.adc3_raw = ADC3->DR;
00227         spi.Sample(DT);                                                           // sample position sensor
00228         controller.theta_elec = spi.GetElecPosition();
00229         controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
00230         controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();  
00231         controller.dtheta_elec = spi.GetElecVelocity();
00232         controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
00233         ///
00234         
00235         /// Check state machine state, and run the appropriate function ///
00236         switch(state){
00237             case REST_MODE:                                                     // Do nothing
00238                 if(state_change){
00239                     enter_menu_state();
00240                     }
00241                 break;
00242             
00243              case CALIBRATION_MODE:                                              // Run encoder calibration procedure
00244                 if(state_change){
00245                     calibrate();
00246                     }
00247                 break;
00248              
00249             case MOTOR_MODE:                                                   // Run torque control
00250                 if(state_change){
00251                     enter_torque_mode();
00252                     count = 0;
00253                     }
00254                 else{
00255                 /*
00256                 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
00257                     gpio.enable->write(0);
00258                     controller.ovp_flag = 1;
00259                     state = REST_MODE;
00260                     state_change = 1;
00261                     printf("OVP Triggered!\n\r");
00262                     }
00263                     */  
00264 
00265                 torque_control(&controller);    
00266                 if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
00267                     controller.i_d_ref = 0;
00268                     controller.i_q_ref = 0;
00269                     controller.kp = 0;
00270                     controller.kd = 0;
00271                     controller.t_ff = 0;
00272                     } 
00273                 commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
00274                 controller.timeout += 1;
00275                 
00276                 /*
00277                 count++;
00278                 if(count == 4000){
00279                      printf("%.4f\n\r", controller.dtheta_mech);
00280                      count = 0;
00281                      }
00282                      */
00283                      
00284             
00285                 }     
00286                 break;
00287 ///     wyc////        
00288             case SPEED_MODE:                                                   // Run SPEED control WYC 2021.11.04
00289                 if(state_change){
00290                     enter_speed_mode();
00291                     }
00292                 else{
00293                     velocity_control(&controller);
00294                     if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
00295                         controller.i_d_ref = 0;
00296                         controller.i_q_ref = 0;
00297                         controller.kp = 0;
00298                         controller.kd = 0;
00299                         controller.t_ff = 0;
00300                     } 
00301                     commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
00302                     controller.timeout += 1;
00303                 }
00304                 break;
00305    ////wyc////////     
00306 
00307   ///     YZ////        
00308             case Position_MODE:                                                   // Run Position control WYC 2021.11.05
00309                 if(state_change){
00310                     enter_Position_mode();
00311                     }
00312                 else{
00313                     Position_control(&controller);
00314                     if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
00315                         controller.i_d_ref = 0;
00316                         controller.i_q_ref = 0;
00317                         controller.kp = 0;
00318                         controller.kd = 0;
00319                         controller.t_ff = 0;
00320                     } 
00321                     commutate(&controller, &observer, &gpio, controller.theta_elec);           // Run current loop
00322                     controller.timeout += 1;
00323                 }
00324                 break;
00325    ////YZ////////      
00326             case SETUP_MODE:
00327                 if(state_change){
00328                     enter_setup_state();
00329                 }
00330                 break;
00331             case ENCODER_MODE:
00332                 print_encoder();
00333                 break;
00334                 }                 
00335       }
00336   TIM1->SR = 0x0;                                                               // reset the status register
00337 }
00338 
00339 
00340 char cmd_val[8] = {0};
00341 char cmd_id = 0;
00342 char char_count = 0;
00343 
00344 /// Manage state machine with commands from serial terminal or configurator gui ///
00345 /// Called when data received over serial ///
00346 void serial_interrupt(void){
00347     while(pc.readable()){
00348         char c = pc.getc();
00349         if(c == 27){
00350                 state = REST_MODE;
00351                 state_change = 1;
00352                 char_count = 0;
00353                 cmd_id = 0;
00354                 gpio.led->write(0);; 
00355                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
00356                 }
00357         if(state == REST_MODE){
00358             switch (c){
00359                 case 'c':
00360                     state = CALIBRATION_MODE;
00361                     state_change = 1;
00362                     break;
00363                 case 'm':
00364                     state = MOTOR_MODE;
00365                     state_change = 1;
00366                     break;
00367                 case 'e':
00368                     state = ENCODER_MODE;
00369                     state_change = 1;
00370                     break;
00371                 case 's':
00372                     state = SETUP_MODE;
00373                     state_change = 1;
00374                     break;
00375                 case 'z':
00376                     spi.SetMechOffset(0);
00377                     spi.Sample(DT);
00378                     wait_us(20);
00379                     M_OFFSET = spi.GetMechPosition();
00380                     if (!prefs.ready()) prefs.open();
00381                         prefs.flush();                                                  // Write new prefs to flash
00382                         prefs.close();    
00383                         prefs.load(); 
00384                     spi.SetMechOffset(M_OFFSET);
00385                     printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
00386                     
00387                     break;
00388                 }
00389                 
00390                 }
00391         else if(state == SETUP_MODE){
00392             if(c == 13){
00393                 switch (cmd_id){
00394                     case 'b':
00395                         I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
00396                         break;
00397                     case 'i':
00398                         CAN_ID = atoi(cmd_val);
00399                         break;
00400                     case 'm':
00401                         CAN_MASTER = atoi(cmd_val);
00402                         break;
00403                     case 'l':
00404                         TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
00405                         break;
00406                     case 't':
00407                         CAN_TIMEOUT = atoi(cmd_val);
00408                         break;
00409                     default:
00410                         printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
00411                         break;
00412                     }
00413                     
00414                 if (!prefs.ready()) prefs.open();
00415                 prefs.flush();                                                  // Write new prefs to flash
00416                 prefs.close();    
00417                 prefs.load();                                              
00418                 state_change = 1;
00419                 char_count = 0;
00420                 cmd_id = 0;
00421                 for(int i = 0; i<8; i++){cmd_val[i] = 0;}
00422                 }
00423             else{
00424                 if(char_count == 0){cmd_id = c;}
00425                 else{
00426                     cmd_val[char_count-1] = c;
00427                     
00428                 }
00429                 pc.putc(c);
00430                 char_count++;
00431                 }
00432             }
00433         else if (state == ENCODER_MODE){
00434             switch (c){
00435                 case 27:
00436                     state = REST_MODE;
00437                     state_change = 1;
00438                     break;
00439                     }
00440             }
00441             
00442         }
00443     }
00444        
00445 int main() {
00446     
00447     controller.v_bus = V_BUS;
00448     controller.mode = 0;
00449     Init_All_HW(&gpio);                                                         // Setup PWM, ADC, GPIO
00450 
00451     wait(.1);
00452     gpio.enable->write(1);
00453     TIM1->CCR3 = PWM_ARR*(1.0f);                        // Write duty cycles
00454     TIM1->CCR2 = PWM_ARR*(1.0f);
00455     TIM1->CCR1 = PWM_ARR*(1.0f);
00456     zero_current(&controller.adc1_offset, &controller.adc2_offset);             // Measure current sensor zero-offset
00457     gpio.enable->write(0);
00458     reset_foc(&controller);                                                     // Reset current controller
00459     TIM1->CR1 ^= TIM_CR1_UDIS;
00460     //TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
00461     
00462     wait(.1);
00463     NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2);                                             // commutation > communication
00464     
00465     NVIC_SetPriority(CAN1_RX0_IRQn, 3);
00466     can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
00467                                                                     
00468     txMsg.id = CAN_MASTER;
00469     txMsg.len = 6;
00470     rxMsg.len = 8;
00471     can.attach(&onMsgReceived);                                     // attach 'CAN receive-complete' interrupt handler    
00472     
00473     prefs.load();                                                               // Read flash
00474     if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
00475     if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
00476     spi.SetElecOffset(E_OFFSET);                                                // Set position sensor offset
00477     spi.SetMechOffset(M_OFFSET);
00478     int lut[128] = {0};
00479     memcpy(&lut, &ENCODER_LUT, sizeof(lut));
00480     spi.WriteLUT(lut);                                                          // Set potision sensor nonlinearity lookup table
00481     
00482     pc.baud(115200);//pc.baud(921600);                                                            // set serial baud rate
00483     wait(.01);
00484     pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
00485     wait(.01);
00486     printf("\n\r Debug Info:\n\r");
00487     printf(" Firmware Version: %s\n\r", VERSION_NUM);
00488     printf(" ADC1 Offset: %d    ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
00489     printf(" Position Sensor Electrical Offset:   %.4f\n\r", E_OFFSET);
00490     printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
00491     printf(" CAN ID:  %d\n\r", CAN_ID);
00492         
00493     pc.attach(&serial_interrupt);                                               // attach serial interrupt
00494     
00495     state_change = 1;
00496 
00497     
00498     while(1) {
00499         if(state == MOTOR_MODE ||state == SPEED_MODE||state == Position_MODE)
00500              {
00501           
00502           //  printf("J: %.3f  Mec: %.3f   Jerr: %.3f   JVerr: %.3f   Kp: %.3f   Kd: %.3f  \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd);
00503             //printf("Jraw:%.3f   J: %.3f  Mec: %.3f   N: %.3d   Nmod: %.3f   Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
00504 
00505               //printf("Pdes: %.3f  Vdes: %.3f   Kp: %.3f   Kd: %.3f   Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff);
00506              // pc.printf("Vdes: %.3f\n\rPrel: %.3f  Vrel: %.3f   T: %.3f \n\r",controller.v_des,controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
00507               //printf("tor: %.3f\n\r",controller.i_q_filt*KT_OUT)
00508              // count = 0;
00509              pc.printf("%d\n\r",spi.aa);
00510              
00511 
00512         }
00513     }
00514 }