main.cpp

00001 ////////////////////////////////////////
00002 //      Rotating Platform             //
00003 //  Arkadiraf@gmail.com - 24/05/2017  //
00004 ////////////////////////////////////////
00005 
00006 /*
00007    Parts:
00008     Nucleo STM32F401RE
00009     X-NUCLEO-IHM01A1 - 3 Stepper motor controller
00010 */
00011 
00012 /*
00013  Improvements:
00014  Move all constant parameters to #define,
00015  Implement control loop (other than proportional)
00016  Implement Velocity command saturation with position control (implements max speed limits)
00017  Implement better stop condition for position control
00018 */
00019 /*
00020     Pinout:
00021     Nucleo STM32F401RE
00022     PA_5 --> led (DigitalOut)
00023 
00024     PC - Serial 2
00025     PA_2 (Tx) --> STLINK
00026     PA_3 (Rx) --> STLINK
00027 
00028     X-NUCLEO-IHM01A1 (http://www.st.com/content/ccc/resource/technical/document/data_brief/59/ff/d0/16/94/ff/49/85/DM00122463.pdf/files/DM00122463.pdf/jcr:content/translations/en.DM00122463.pdf)
00029     SPI:
00030     PA_7 (D11) --> mosi
00031     PA_9 (D12) --> miso
00032     PA_8 (D13) --> sclk
00033 
00034     Motor 1
00035     PA_10(D2) --> flag_irq  (DigitalOut)
00036     PA_9 (D8) --> Standby   (DigitalOut)
00037     PA_8 (D7) --> MOT1Dir  (DigitalOut)
00038     PC_7 (D9) --> MOT1Step (PWM)
00039     PB_6 (D10)--> ssel      (DigitalOut)
00040 
00041     Motor 2
00042     PA_10(D2) --> flag_irq  (DigitalOut)
00043     PA_9 (D8) --> Standby   (DigitalOut)
00044     PB_5 (D4) --> MOT2Dir   (DigitalOut)
00045     PB_3 (D3) --> MOT2Step  (PWM)
00046     PB_6 (D10)--> ssel      (DigitalOut)
00047 
00048     Motor 3
00049     PA_10(D2) --> flag_irq  (DigitalOut)
00050     PA_9 (D8) --> Standby   (DigitalOut)
00051     PB_4 (D5) --> MOT3Dir   (DigitalOut)
00052     PB_10(D6) --> MOT3Step  (PWM)
00053     PB_6 (D10)--> ssel      (DigitalOut)
00054 
00055 
00056 */
00057 
00058 ///////////////
00059 // Libraries //
00060 ///////////////
00061 #include "mbed.h"
00062 #include "BufferedSerial.h"  // solves issues of loosing data. alternative doing it yourself
00063 #include "l6474_class.h" // stepper library
00064 
00065 
00066 ///////////////
00067 // #defines  //
00068 ///////////////
00069 #define DEBUG_MSG
00070 #define Motor_Control_Interval 10000 // 100Hz
00071 #define TimeoutCommand 2000 // 2 second (ms units)
00072 #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
00073 
00074 // control loop
00075 
00076 #define PROPORTIONAL_GAIN 0.0027777f // 1*1/360; (// gain for rotations/sec
00077 
00078 /////////////
00079 // Objects //
00080 /////////////
00081 
00082 // X-NUCLEO-IHM01A1
00083 DigitalOut MOT1Dir(D7);
00084 DigitalOut MOT1Step(D9);
00085 DigitalOut MOT2Dir(D4);
00086 DigitalOut MOT2Step(D3);
00087 DigitalOut MOT3Dir(D5);
00088 DigitalOut MOT3Step(D6);
00089 
00090 /* Motor Control Component. */
00091 L6474 *motor1;
00092 L6474 *motor2;
00093 L6474 *motor3;
00094 
00095 // Led
00096 DigitalOut led(LED1);
00097 
00098 // serial
00099 BufferedSerial pc(USBTX, USBRX);
00100 
00101 // Define Ticker for Motor Motion Control Ticker
00102 Ticker Platform_Control_Ticker;
00103 
00104 // Define Ticker for Steps control
00105 Ticker Motor1_Step_Ticker;
00106 Ticker Motor2_Step_Ticker;
00107 Ticker Motor3_Step_Ticker;
00108 
00109 // Timer for clock counter
00110 Timer time_timer;
00111 
00112 
00113 ///////////////
00114 // variables //
00115 ///////////////
00116 // Send pos update
00117 bool Pos_Update_Flag=0;
00118 
00119 // Driver Flag status, enabled / disabled
00120 bool EN_Stepper_Flag=0;
00121 
00122 // flag to indicate control mode; 1 - SPD , 0 - Position
00123 volatile bool SpdPos_Flag=0;
00124 
00125 // Motor Speed control
00126 volatile float CMD_Motor_SPD[3]= {0}; // rotations / sec
00127 volatile float Motor_SPD[3]= {0}; // rotations / sec
00128 
00129 // Motor Position control
00130 volatile float CMD_Motor_Pos[3]= {0}; // command motor angle in degrees
00131 volatile float Motor_Pos[3]= {0}; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
00132 
00133 // variable to store motor position
00134 volatile int Motor_Steps[3] = {0}; // motor steps performed
00135 
00136 // timeout command time stamp
00137 volatile int CMDTimeStamp=0;
00138 
00139 ///////////////
00140 // Functions //
00141 ///////////////
00142 
00143 // Incoming Message parser
00144 void Parse_Message(char inbyte);
00145 
00146 // Init Platform
00147 void Platform_Init();
00148 
00149 // Platform Motion Set
00150 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3);
00151 
00152 // Platform Motion Control
00153 void Platform_Motion_Control();
00154 
00155 // Motor1 Step Control
00156 void Motor1_Step_Control();
00157 
00158 // Motor2 Step Control
00159 void Motor2_Step_Control();
00160 
00161 // Motor3 Step Control
00162 void Motor3_Step_Control();
00163 
00164 ////////////////////////
00165 //  Main Code Setup : //
00166 ////////////////////////
00167 int main()
00168 {
00169     //Initializing SPI bus.
00170     DevSPI dev_spi(D11, D12, D13);
00171 
00172     //Initializing Motor Control Components.
00173     motor1 = new L6474(D2, D8, D10, dev_spi);
00174     motor2 = new L6474(D2, D8, D10, dev_spi);
00175     motor3 = new L6474(D2, D8, D10, dev_spi);
00176 
00177     // Setup serial
00178     pc.baud(256000);
00179 
00180     // Init shika shuka
00181     Platform_Init();
00182 
00183     // initil time timer:
00184     time_timer.start();
00185 
00186     ///////////////////////
00187     //  Main Code Loop : //
00188     ///////////////////////
00189     while(1) {
00190         // loop time stamp
00191         int Timer_TimeStamp_ms=time_timer.read_ms();
00192 
00193         // receive Motor Command
00194         while (pc.readable()) {
00195             char InChar=pc.getc();
00196             pc.printf("%c" ,InChar); // debug check/
00197             Parse_Message(InChar);
00198         }//end serial
00199 
00200         // set time out on commad and stop motion
00201         if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand) {
00202 #ifdef DEBUG_MSG
00203             // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);
00204 #endif /* DEBUG_MSG */
00205             CMDTimeStamp=Timer_TimeStamp_ms;
00206             motor1->Disable();
00207             motor2->Disable();
00208             motor3->Disable();
00209             CMD_Motor_SPD[0]=0;
00210             CMD_Motor_SPD[1]=0;
00211             CMD_Motor_SPD[2]=0;
00212             Motor_SPD[0]=0;
00213             Motor_SPD[1]=0;
00214             Motor_SPD[2]=0;
00215             EN_Stepper_Flag=0;
00216 
00217             // reset motor position
00218             Motor_Steps[0]=0;
00219             Motor_Steps[1]=0;
00220             Motor_Steps[2]=0;
00221             CMD_Motor_Pos[0]=0;
00222             CMD_Motor_Pos[1]=0;
00223             CMD_Motor_Pos[2]=0;
00224         } // end timeout
00225         // send position message
00226         if (Pos_Update_Flag) {
00227             Pos_Update_Flag=0;
00228             int Temp_TimeStamp_ms=time_timer.read_ms();
00229             static uint32_t MSG_Counter=0;
00230             MSG_Counter++;
00231             pc.printf("POS:%d,%d,%d,%d,%d\r\n" ,Temp_TimeStamp_ms,MSG_Counter,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // send motors position
00232         }// end position update
00233     }// End Main Loop
00234 }// End Main
00235 
00236 
00237 ///////////////
00238 // Functions //
00239 ///////////////
00240 // Incoming Message parser  Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
00241 void Parse_Message(char inbyte)
00242 {
00243     static const uint16_t BufferCMDSize=32;
00244     static const uint16_t BufferCMD_ValuesSize=4;
00245 
00246     static float CMD_Values[BufferCMD_ValuesSize]= {0};
00247     static char BufferCMD[BufferCMDSize]= {0};
00248     static uint16_t BufferIndex=0;
00249     static uint16_t Values_Index=0;
00250 
00251     BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
00252     Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler
00253 
00254     BufferCMD[BufferIndex]=inbyte;
00255     BufferIndex++;
00256 
00257     // parse incoming message
00258     if (inbyte=='$') { // start of message
00259         BufferIndex=0; // initialize to start of parser
00260         Values_Index=0; // index for values position
00261     } else if (inbyte==',') { // seperator char
00262         CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
00263         BufferIndex=0; // initialize to start of parser
00264         Values_Index++;
00265     } else if(inbyte=='\r') { // end of message // parse message
00266         // Update last value
00267         CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
00268 
00269         BufferIndex=0; // initialize to start of parser
00270         Values_Index=0; // reset values index
00271 
00272         // set time stamp on time out commad
00273         CMDTimeStamp=time_timer.read_ms();
00274 
00275         //0 - Speed Control 1 - Position Control
00276         SpdPos_Flag=(bool)CMD_Values[0];
00277 
00278         // update command
00279         Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]);
00280 
00281 #ifdef DEBUG_MSG
00282         pc.printf("CMD: %d ,%.3f ,%.3f ,%.3f \r\n" ,SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
00283         //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
00284         led = !led;
00285 #endif /* DEBUG_MSG */
00286 
00287     }//end parser
00288 }// end parser function
00289 
00290 
00291 // Init shika shuka
00292 void Platform_Init()
00293 {
00294     //Initializing Motor Control Components.
00295     if (motor1->Init() != COMPONENT_OK)
00296         exit(EXIT_FAILURE);
00297     if (motor2->Init() != COMPONENT_OK)
00298         exit(EXIT_FAILURE);
00299     if (motor3->Init() != COMPONENT_OK)
00300         exit(EXIT_FAILURE);
00301 
00302     /*----- Changing motor setting. -----*/
00303     /* Setting High Impedance State to update L6474's registers. */
00304     motor1->SoftHiZ();
00305     motor2->SoftHiZ();
00306     motor3->SoftHiZ();
00307     // Disabling motor
00308     motor1->Disable();
00309     motor2->Disable();
00310     motor3->Disable();
00311     /* Changing step mode. */
00312     motor1->SetStepMode(STEP_MODE_1_16);
00313     motor2->SetStepMode(STEP_MODE_1_16);
00314     motor3->SetStepMode(STEP_MODE_1_16);
00315 
00316     /* Increasing the torque regulation current. */
00317     motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
00318     motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
00319     motor3->SetParameter(L6474_TVAL, 300);  // Limit 0.28A
00320 
00321     /* Increasing the max threshold overcurrent. */
00322     motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
00323     motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
00324     motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);
00325 
00326     // Enabling motor
00327     motor1->Enable();
00328     motor2->Enable();
00329     motor3->Enable();
00330 
00331     // Initialize Control Pins
00332     MOT1Dir.write(0);
00333     MOT1Step.write(0);
00334     MOT2Dir.write(0);
00335     MOT2Step.write(0);
00336     MOT3Dir.write(0);
00337     MOT3Step.write(0);
00338 
00339     // Start Moition Control  // need implementation
00340     Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval);
00341 
00342 }// End Init shika shuka
00343 
00344 // ShikaShuka Motion Set
00345 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3)
00346 {
00347     static const float MaxSPDCMD=5.0f;
00348     static const float DeadZoneCMD=0.0001f;
00349     static const float MaxAngle=180.0f;
00350 
00351     // Velocity control
00352     if(SpdPos_Flag) {
00353         // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
00354         if (Set_M1 > MaxSPDCMD)  Set_M1 = MaxSPDCMD;
00355         if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD;
00356         if (abs(Set_M1)  < DeadZoneCMD)  Set_M1 = 0;
00357 
00358         if (Set_M2 > MaxSPDCMD)  Set_M2 = MaxSPDCMD;
00359         if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD;
00360         if (abs(Set_M2)  < DeadZoneCMD)  Set_M2 = 0;
00361 
00362         if (Set_M3 > MaxSPDCMD)  Set_M3 = MaxSPDCMD;
00363         if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD;
00364         if (abs(Set_M3)  < DeadZoneCMD)  Set_M3 = 0;
00365         // enable stepper drivers
00366         if (EN_Stepper_Flag==0) {
00367             motor1->Enable();
00368             motor2->Enable();
00369             motor3->Enable();
00370             EN_Stepper_Flag=1;
00371         }
00372         // update motor speed command
00373         CMD_Motor_SPD[0]=Set_M1;
00374         CMD_Motor_SPD[1]=Set_M2;
00375         CMD_Motor_SPD[2]=Set_M3;
00376 
00377     } else { // end velocity control -  Start position control
00378 
00379         // calculate position based on steps:
00380         // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
00381         if (Set_M1 > MaxAngle)  Set_M1 = MaxAngle;
00382         if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;
00383 
00384         if (Set_M2 > MaxAngle)  Set_M2 = MaxAngle;
00385         if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle;
00386 
00387         if (Set_M3 > MaxAngle)  Set_M3 = MaxAngle;
00388         if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle;
00389         // enable stepper drivers
00390         if (EN_Stepper_Flag==0) {
00391             motor1->Enable();
00392             motor2->Enable();
00393             motor3->Enable();
00394             EN_Stepper_Flag=1;
00395         }
00396         // update motor speed command
00397         CMD_Motor_Pos[0]=Set_M1;
00398         CMD_Motor_Pos[1]=Set_M2;
00399         CMD_Motor_Pos[2]=Set_M3;
00400 
00401     }// end position control
00402 }// End Platform Motion Set
00403 
00404 // Platform Motion Control
00405 void Platform_Motion_Control()
00406 {
00407     // variable limits: (-100>SPD_M>100)
00408     static const float MaxSPD=0.5f; // rounds per second
00409     static const float DeadZone=0.0001f;
00410 
00411     // update max acceleration calculation !!!!!!
00412     static const float MaxACC=0.5f/(1000000/Motor_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec
00413 
00414     static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz
00415 
00416     // calculate motor pos:
00417     Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
00418     Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
00419     Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
00420 
00421     // position control
00422     if (SpdPos_Flag == 0) {
00423         // implement control loop based on desired position and current position
00424         // update velocity commands based on position control loop
00425         CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN;
00426         CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN;
00427         CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN;
00428 
00429     }
00430     //  update velocity commands
00431     // implement control loop here: (basic control with max acceleration limit)
00432     if(1) {
00433         if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
00434             CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
00435         } else {
00436             Motor_SPD[0]=CMD_Motor_SPD[0];
00437         }
00438         if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
00439             CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
00440         } else {
00441             Motor_SPD[1]=CMD_Motor_SPD[1];
00442         }
00443         if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) {
00444             CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC;
00445         } else {
00446             Motor_SPD[2]=CMD_Motor_SPD[2];
00447         }
00448     }
00449     // update driver frequency
00450     if (1) {
00451         // Start Moition Control
00452         // motor 1
00453 
00454         // update driver direction
00455         if (Motor_SPD[0]>0) {
00456             MOT1Dir.write(1);
00457         } else {
00458             MOT1Dir.write(0);
00459         }
00460 
00461         // check if SPD is higher than minimum value
00462         if (abs(Motor_SPD[0])<DeadZone) {
00463             // disable pulsing, Set speed to zero
00464             Motor1_Step_Ticker.detach();
00465             SetMotorSPD[0]=0;
00466 
00467         } else {
00468             // Set Pulse rate based on pulses per second
00469             SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION);
00470             if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
00471             if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency
00472             Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0])));
00473         }
00474 
00475         // motor 2
00476 
00477         // update driver direction
00478         if (Motor_SPD[1]>0) {
00479             MOT2Dir.write(1);
00480         } else {
00481             MOT2Dir.write(0);
00482         }
00483 
00484         // check if SPD is higher than minimum value
00485         if (abs(Motor_SPD[1])<DeadZone) {
00486             // disable pulsing, Set speed to zero
00487             Motor2_Step_Ticker.detach();
00488             SetMotorSPD[1]=0;
00489 
00490         } else {
00491             // Set Pulse rate based on pulses per second
00492             SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION);
00493             if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
00494             if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency
00495             Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1])));
00496         }
00497 
00498         // Motor 3
00499         // update driver direction
00500         if (Motor_SPD[2]>0) {
00501             MOT3Dir.write(1);
00502         } else {
00503             MOT3Dir.write(0);
00504         }
00505 
00506         // check if SPD is higher than minimum value
00507         if (abs(Motor_SPD[2])<DeadZone) {
00508             // disable pulsing, Set speed to zero
00509             Motor3_Step_Ticker.detach();
00510             SetMotorSPD[2]=0;
00511 
00512         } else {
00513             // Set Pulse rate based on pulses per second
00514             SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION);
00515             if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
00516             if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency
00517             Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2])));
00518         }
00519     }
00520 #ifdef DEBUG_MSG
00521     //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
00522     led = !led;
00523 #endif /* DEBUG_MSG */
00524 // update position
00525     Pos_Update_Flag=1;
00526 }// End Platform Motion Control
00527 
00528 
00529 // Motor 1 Ticker step control
00530 void Motor1_Step_Control()
00531 {
00532     MOT1Step=!MOT1Step;
00533     if (MOT1Step) {
00534         MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
00535     }
00536 }// end motor 1 Step control
00537 
00538 // Motor 2 Ticker step control
00539 void Motor2_Step_Control()
00540 {
00541     MOT2Step=!MOT2Step;
00542     if (MOT2Step) {
00543         MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
00544     }
00545 }// end motor 2 Step control
00546 
00547 // Motor 3 Ticker step control
00548 void Motor3_Step_Control()
00549 {
00550     MOT3Step=!MOT3Step;
00551     if (MOT3Step) {
00552         MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
00553     }
00554 }// end motor 3 Step control
00555