Thesis Rotating Platform, Uart Control
Dependencies: BufferedSerial X_NUCLEO_IHM01A1_Disabled_Control mbed
Fork of Demo_IHM01A1_3-Motors by
main.cpp
- Committer:
- Arkadi
- Date:
- 2017-06-14
- Revision:
- 28:32001ee473e0
- Parent:
- 27:8e0acf4ae4fd
File content as of revision 28:32001ee473e0:
//////////////////////////////////////// // Rotating Platform // // Arkadiraf@gmail.com - 24/05/2017 // //////////////////////////////////////// /* Parts: Nucleo STM32F401RE X-NUCLEO-IHM01A1 - 3 Stepper motor controller */ /* Improvements: Move all constant parameters to #define, Implement control loop (other than proportional) Implement Velocity command saturation with position control (implements max speed limits) Implement better stop condition for position control */ /* Pinout: Nucleo STM32F401RE PA_5 --> led (DigitalOut) PC - Serial 2 PA_2 (Tx) --> STLINK PA_3 (Rx) --> STLINK 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) SPI: PA_7 (D11) --> mosi PA_9 (D12) --> miso PA_8 (D13) --> sclk Motor 1 PA_10(D2) --> flag_irq (DigitalOut) PA_9 (D8) --> Standby (DigitalOut) PA_8 (D7) --> MOT1Dir (DigitalOut) PC_7 (D9) --> MOT1Step (PWM) PB_6 (D10)--> ssel (DigitalOut) Motor 2 PA_10(D2) --> flag_irq (DigitalOut) PA_9 (D8) --> Standby (DigitalOut) PB_5 (D4) --> MOT2Dir (DigitalOut) PB_3 (D3) --> MOT2Step (PWM) PB_6 (D10)--> ssel (DigitalOut) Motor 3 PA_10(D2) --> flag_irq (DigitalOut) PA_9 (D8) --> Standby (DigitalOut) PB_4 (D5) --> MOT3Dir (DigitalOut) PB_10(D6) --> MOT3Step (PWM) PB_6 (D10)--> ssel (DigitalOut) */ /////////////// // Libraries // /////////////// #include "mbed.h" #include "BufferedSerial.h" // solves issues of loosing data. alternative doing it yourself #include "l6474_class.h" // stepper library /////////////// // #defines // /////////////// #define DEBUG_MSG #define Motor_Control_Interval 10000 // 100Hz #define TimeoutCommand 2000 // 2 second (ms units) #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping // control loop #define PROPORTIONAL_GAIN 0.0027777f // 1*1/360; (// gain for rotations/sec ///////////// // Objects // ///////////// // X-NUCLEO-IHM01A1 DigitalOut MOT1Dir(D7); DigitalOut MOT1Step(D9); DigitalOut MOT2Dir(D4); DigitalOut MOT2Step(D3); DigitalOut MOT3Dir(D5); DigitalOut MOT3Step(D6); /* Motor Control Component. */ L6474 *motor1; L6474 *motor2; L6474 *motor3; // Led DigitalOut led(LED1); // serial BufferedSerial pc(USBTX, USBRX); // Define Ticker for Motor Motion Control Ticker Ticker Platform_Control_Ticker; // Define Ticker for Steps control Ticker Motor1_Step_Ticker; Ticker Motor2_Step_Ticker; Ticker Motor3_Step_Ticker; // Timer for clock counter Timer time_timer; /////////////// // variables // /////////////// // Send pos update bool Pos_Update_Flag=0; // Driver Flag status, enabled / disabled bool EN_Stepper_Flag=0; // flag to indicate control mode; 1 - SPD , 0 - Position volatile bool SpdPos_Flag=0; // Motor Speed control volatile float CMD_Motor_SPD[3]= {0}; // rotations / sec volatile float Motor_SPD[3]= {0}; // rotations / sec // Motor Position control volatile float CMD_Motor_Pos[3]= {0}; // command motor angle in degrees volatile float Motor_Pos[3]= {0}; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f // variable to store motor position volatile int Motor_Steps[3] = {0}; // motor steps performed // timeout command time stamp volatile int CMDTimeStamp=0; /////////////// // Functions // /////////////// // Incoming Message parser void Parse_Message(char inbyte); // Init Platform void Platform_Init(); // Platform Motion Set void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3); // Platform Motion Control void Platform_Motion_Control(); // Motor1 Step Control void Motor1_Step_Control(); // Motor2 Step Control void Motor2_Step_Control(); // Motor3 Step Control void Motor3_Step_Control(); //////////////////////// // Main Code Setup : // //////////////////////// int main() { //Initializing SPI bus. DevSPI dev_spi(D11, D12, D13); //Initializing Motor Control Components. motor1 = new L6474(D2, D8, D10, dev_spi); motor2 = new L6474(D2, D8, D10, dev_spi); motor3 = new L6474(D2, D8, D10, dev_spi); // Setup serial pc.baud(256000); // Init shika shuka Platform_Init(); // initil time timer: time_timer.start(); /////////////////////// // Main Code Loop : // /////////////////////// while(1) { // loop time stamp int Timer_TimeStamp_ms=time_timer.read_ms(); // receive Motor Command while (pc.readable()) { char InChar=pc.getc(); pc.printf("%c" ,InChar); // debug check/ Parse_Message(InChar); }//end serial // set time out on commad and stop motion if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand) { #ifdef DEBUG_MSG // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp); #endif /* DEBUG_MSG */ CMDTimeStamp=Timer_TimeStamp_ms; motor1->Disable(); motor2->Disable(); motor3->Disable(); CMD_Motor_SPD[0]=0; CMD_Motor_SPD[1]=0; CMD_Motor_SPD[2]=0; Motor_SPD[0]=0; Motor_SPD[1]=0; Motor_SPD[2]=0; EN_Stepper_Flag=0; // reset motor position Motor_Steps[0]=0; Motor_Steps[1]=0; Motor_Steps[2]=0; CMD_Motor_Pos[0]=0; CMD_Motor_Pos[1]=0; CMD_Motor_Pos[2]=0; } // end timeout // send position message if (Pos_Update_Flag) { Pos_Update_Flag=0; int Temp_TimeStamp_ms=time_timer.read_ms(); static uint32_t MSG_Counter=0; MSG_Counter++; 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 }// end position update }// End Main Loop }// End Main /////////////// // Functions // /////////////// // Incoming Message parser Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n void Parse_Message(char inbyte) { static const uint16_t BufferCMDSize=32; static const uint16_t BufferCMD_ValuesSize=4; static float CMD_Values[BufferCMD_ValuesSize]= {0}; static char BufferCMD[BufferCMDSize]= {0}; static uint16_t BufferIndex=0; static uint16_t Values_Index=0; BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler BufferCMD[BufferIndex]=inbyte; BufferIndex++; // parse incoming message if (inbyte=='$') { // start of message BufferIndex=0; // initialize to start of parser Values_Index=0; // index for values position } else if (inbyte==',') { // seperator char CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values BufferIndex=0; // initialize to start of parser Values_Index++; } else if(inbyte=='\r') { // end of message // parse message // Update last value CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values BufferIndex=0; // initialize to start of parser Values_Index=0; // reset values index // set time stamp on time out commad CMDTimeStamp=time_timer.read_ms(); //0 - Speed Control 1 - Position Control SpdPos_Flag=(bool)CMD_Values[0]; // update command Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]); #ifdef DEBUG_MSG pc.printf("CMD: %d ,%.3f ,%.3f ,%.3f \r\n" ,SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/ //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/ led = !led; #endif /* DEBUG_MSG */ }//end parser }// end parser function // Init shika shuka void Platform_Init() { //Initializing Motor Control Components. if (motor1->Init() != COMPONENT_OK) exit(EXIT_FAILURE); if (motor2->Init() != COMPONENT_OK) exit(EXIT_FAILURE); if (motor3->Init() != COMPONENT_OK) exit(EXIT_FAILURE); /*----- Changing motor setting. -----*/ /* Setting High Impedance State to update L6474's registers. */ motor1->SoftHiZ(); motor2->SoftHiZ(); motor3->SoftHiZ(); // Disabling motor motor1->Disable(); motor2->Disable(); motor3->Disable(); /* Changing step mode. */ motor1->SetStepMode(STEP_MODE_1_16); motor2->SetStepMode(STEP_MODE_1_16); motor3->SetStepMode(STEP_MODE_1_16); /* Increasing the torque regulation current. */ motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A motor3->SetParameter(L6474_TVAL, 300); // Limit 0.28A /* Increasing the max threshold overcurrent. */ motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA); motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA); motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA); // Enabling motor motor1->Enable(); motor2->Enable(); motor3->Enable(); // Initialize Control Pins MOT1Dir.write(0); MOT1Step.write(0); MOT2Dir.write(0); MOT2Step.write(0); MOT3Dir.write(0); MOT3Step.write(0); // Start Moition Control // need implementation Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval); }// End Init shika shuka // ShikaShuka Motion Set void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3) { static const float MaxSPDCMD=5.0f; static const float DeadZoneCMD=0.0001f; static const float MaxAngle=180.0f; // Velocity control if(SpdPos_Flag) { // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD) if (Set_M1 > MaxSPDCMD) Set_M1 = MaxSPDCMD; if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD; if (abs(Set_M1) < DeadZoneCMD) Set_M1 = 0; if (Set_M2 > MaxSPDCMD) Set_M2 = MaxSPDCMD; if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD; if (abs(Set_M2) < DeadZoneCMD) Set_M2 = 0; if (Set_M3 > MaxSPDCMD) Set_M3 = MaxSPDCMD; if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD; if (abs(Set_M3) < DeadZoneCMD) Set_M3 = 0; // enable stepper drivers if (EN_Stepper_Flag==0) { motor1->Enable(); motor2->Enable(); motor3->Enable(); EN_Stepper_Flag=1; } // update motor speed command CMD_Motor_SPD[0]=Set_M1; CMD_Motor_SPD[1]=Set_M2; CMD_Motor_SPD[2]=Set_M3; } else { // end velocity control - Start position control // calculate position based on steps: // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD) if (Set_M1 > MaxAngle) Set_M1 = MaxAngle; if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle; if (Set_M2 > MaxAngle) Set_M2 = MaxAngle; if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle; if (Set_M3 > MaxAngle) Set_M3 = MaxAngle; if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle; // enable stepper drivers if (EN_Stepper_Flag==0) { motor1->Enable(); motor2->Enable(); motor3->Enable(); EN_Stepper_Flag=1; } // update motor speed command CMD_Motor_Pos[0]=Set_M1; CMD_Motor_Pos[1]=Set_M2; CMD_Motor_Pos[2]=Set_M3; }// end position control }// End Platform Motion Set // Platform Motion Control void Platform_Motion_Control() { // variable limits: (-100>SPD_M>100) static const float MaxSPD=0.5f; // rounds per second static const float DeadZone=0.0001f; // update max acceleration calculation !!!!!! static const float MaxACC=0.5f/(1000000/Motor_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz // calculate motor pos: Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f // position control if (SpdPos_Flag == 0) { // implement control loop based on desired position and current position // update velocity commands based on position control loop CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN; CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN; CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN; } // update velocity commands // implement control loop here: (basic control with max acceleration limit) if(1) { if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) { CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC; } else { Motor_SPD[0]=CMD_Motor_SPD[0]; } if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) { CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC; } else { Motor_SPD[1]=CMD_Motor_SPD[1]; } if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) { CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC; } else { Motor_SPD[2]=CMD_Motor_SPD[2]; } } // update driver frequency if (1) { // Start Moition Control // motor 1 // update driver direction if (Motor_SPD[0]>0) { MOT1Dir.write(1); } else { MOT1Dir.write(0); } // check if SPD is higher than minimum value if (abs(Motor_SPD[0])<DeadZone) { // disable pulsing, Set speed to zero Motor1_Step_Ticker.detach(); SetMotorSPD[0]=0; } else { // Set Pulse rate based on pulses per second SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION); if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0]))); } // motor 2 // update driver direction if (Motor_SPD[1]>0) { MOT2Dir.write(1); } else { MOT2Dir.write(0); } // check if SPD is higher than minimum value if (abs(Motor_SPD[1])<DeadZone) { // disable pulsing, Set speed to zero Motor2_Step_Ticker.detach(); SetMotorSPD[1]=0; } else { // Set Pulse rate based on pulses per second SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION); if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1]))); } // Motor 3 // update driver direction if (Motor_SPD[2]>0) { MOT3Dir.write(1); } else { MOT3Dir.write(0); } // check if SPD is higher than minimum value if (abs(Motor_SPD[2])<DeadZone) { // disable pulsing, Set speed to zero Motor3_Step_Ticker.detach(); SetMotorSPD[2]=0; } else { // Set Pulse rate based on pulses per second SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION); if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2]))); } } #ifdef DEBUG_MSG //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/ led = !led; #endif /* DEBUG_MSG */ // update position Pos_Update_Flag=1; }// End Platform Motion Control // Motor 1 Ticker step control void Motor1_Step_Control() { MOT1Step=!MOT1Step; if (MOT1Step) { MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--; } }// end motor 1 Step control // Motor 2 Ticker step control void Motor2_Step_Control() { MOT2Step=!MOT2Step; if (MOT2Step) { MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--; } }// end motor 2 Step control // Motor 3 Ticker step control void Motor3_Step_Control() { MOT3Step=!MOT3Step; if (MOT3Step) { MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--; } }// end motor 3 Step control