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-05-24
- Revision:
- 25:716a21ab5fd3
- Parent:
- 23:1bcf834fb859
- Child:
- 26:09a541810137
File content as of revision 25:716a21ab5fd3:
//////////////////////////////////////// // Rotating Platform // // Arkadiraf@gmail.com - 24/05/2017 // //////////////////////////////////////// /* Parts: Nucleo STM32F401RE X-NUCLEO-IHM01A1 - 3 Stepper motor controller */ /* 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 ///////////// // 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 // /////////////// // Driver Flag status, enabled / disabled bool EN_Stepper_Flag=0; // Motor Speed control volatile float CMD_Motor_SPD[3]= {0}; volatile float Motor_SPD[3]= {0}; // Motor Position control volatile float CMD_Motor_Pos[3]= {0}; volatile float Motor_Pos[3]= {0}; // variable to store motor position volatile int Motor_Steps[3] = {0}; // 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(bool SpdPos_Flag,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, D7, D9, D10, dev_spi); // motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi); // motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi); 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(115200); // 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; } // end timeout }// 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 bool SpdPos_Flag=(bool)CMD_Values[0]; // update command Platform_Motion_Set(SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]); // motor test while control is developed if (0){ Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/CMD_Values[1])); Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/CMD_Values[2])); Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/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(bool SpdPos_Flag,float Set_M1, float Set_M2, float Set_M3) { static const float MaxSPDCMD=5.0f; static const float DeadZoneCMD=0.001f; // 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 // position control // calculate position based on steps: // implement control loop based on desired position and current position update velocity commands }// 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.001f; // 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 // 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/ //pc.printf("POS: %d ,%d, %d\r\n" ,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // debug check/ led = !led; #endif /* DEBUG_MSG */ }// 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