Operation Mode State Machine plus homing for StateMachinePTR
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of Controller by
OperationMode.cpp
- Committer:
- Roooos
- Date:
- 2018-11-01
- Revision:
- 16:1c8a8643260f
- Parent:
- 15:97311058e012
File content as of revision 16:1c8a8643260f:
#include "mbed.h" #include "math.h" #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" //Tickers Ticker TickerMeasureAndControl; Timer TimerLoop; //Communication MODSERIAL pc(USBTX,USBRX); QEI Encoder(D10,D11,NC,32); QEI Encoder2(D12,D13,NC,32); //Global pin variables PwmOut PwmPin(D5); PwmOut PwmPin2(D6); DigitalOut DirectionPin(D4); DigitalOut DirectionPin2(D7); DigitalIn BUT1(D8); DigitalIn BUT2(D9); DigitalIn button_sw2(SW2); DigitalIn button_sw3(SW3); DigitalOut LedGreen(LED_GREEN); DigitalOut LedRed(LED_RED); DigitalOut LedBlue(LED_BLUE); double q1Ref=0; double q2Ref =0; //Define substates Operation Mode enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming}; States CurrentOperationState; void SetMotorsOff() { PwmPin = 0; PwmPin2 = 0; } //State Machine for OperationMode void OperationStateMachine() { switch (CurrentOperationState) { case OPWait: if (BUT1 == false) { //When Left Biceps are contracted, initiate chain to flip the page CurrentOperationState = OPState1; TimerLoop.reset(); } if (BUT2 == false) { //When sw3 is pressed, go to homing for Operation Mode CurrentOperationState = OPSet; TimerLoop.reset(); } break; case OPSet: // Set new homing for Operation Mode and go back to waiting mode if (q2Ref > -0.733*(6.380)) { q2Ref -= 0.0005*(6.380); } if (q2Ref < -0.733*(6.380) && TimerLoop > 2) { CurrentOperationState = OPWait; TimerLoop.reset(); } break; case OPState1: // First step of chain to flip the page if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4 q1Ref += 0.0005*(6.380); } if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) { CurrentOperationState = OPState2; TimerLoop.reset(); } break; case OPState2: //Second step of chain to flip the page if (q2Ref > -1.133*(6.380)) { q2Ref -= 0.0005*(6.380); } if ((q2Ref < -1.133*(6.380)) && (TimerLoop >= 2.0)) { CurrentOperationState = OPState3; TimerLoop.reset(); } break; case OPState3: //Third step of chain to flip the page if (q1Ref > -0.15*(6.380)) { q1Ref -= 0.0005*(6.380); } if (q2Ref > -1.483*(6.380)) { q2Ref -= 0.0003*(6.380); } if ((q1Ref < -0.15*(6.380)) && (q2Ref < -1.483*(6.380)) && (TimerLoop >= 3.0)) { CurrentOperationState = OPState4; TimerLoop.reset(); } break; case OPState4: //Fourth step of chain to flip the page if (q2Ref > -2.133*(6.380)) { q2Ref -= 0.005*(6.380); } if ((q2Ref < -2.133*(6.380)) && (TimerLoop > 0.5)) { TimerLoop.reset(); CurrentOperationState = OPHoming; } break; case OPHoming: //Go back to Home for Operation Mode and back to Waiting if (q1Ref < 0) { q1Ref += 0.003*(6.380); } if (q2Ref < -0.733*(6.380)) { q2Ref += 0.001*(6.380); } if ((q1Ref > 0) && (q2Ref > -0.733*(6.380)) && (TimerLoop > 3)) { CurrentOperationState = OPWait; } break; } } //Homing /* if (q1Ref >= 0) { q1Ref -= 0.001*(6.380); } if (q1Ref <= 0) { q1Ref += 0.001*(6.380); } if (q2Ref >= 0) { q2Ref -= 0.001*(6.380); } if (q2Ref <= 0) { q2Ref += 0.001*(6.380); } if ((-0.01*(6.380) < q1Ref < 0.01*(6.380)) && (-0.01*(6.380) < q1Ref < 0.01*(6.380))) { SetMotorsOff(); } */ //----------------------------------------------------------------------------- //The double-functions // actual position of the motor 1 double GetActualPosition() { //This function determines the actual position of the motor //The count:radians relation is 8400:2pi double EncoderCounts = Encoder.getPulses(); //number of counts double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding return PositionMotor; } double GetActualPosition2() //motor 2 { double Encoder2Counts = Encoder2.getPulses(); //number of counts double PositionMotor2 = Encoder2Counts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding return PositionMotor2; } ///The controller double PID_Controller(double Error) { //Arm drive parameters double Ts = 0.002; //Sampling time 100 Hz double Kp = 19.8; // proportional gain double Ki = 3.98; //Intergral gain double Kd = 1.96; //Differential gain. static double ErrorIntegral = 0; static double ErrorPrevious = Error; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: double u_k = Kp * Error; //Integral part: ErrorIntegral = ErrorIntegral + Error*Ts; double u_i = Ki * ErrorIntegral; //Derivative part: double ErrorDerivative = (Error - ErrorPrevious)/Ts; double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); double u_d = Kd * FilteredErrorDerivative; ErrorPrevious = Error; // sum of parts and return it return u_k + u_i + u_d; //This will become the MotorValue } double PID_Controller2(double Error) { //Belt drive parameters double Ts = 0.002; //Sampling time 100 Hz double Kp = 11.1; // proportional gain double Ki = 2.24; //Integral gain double Kd = 1.1; //Differential gain static double ErrorIntegral = 0; static double ErrorPrevious = Error; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: double u_k = Kp * Error; //Integral part: ErrorIntegral = ErrorIntegral + Error*Ts; double u_i = Ki * ErrorIntegral; //Derivative part: double ErrorDerivative = (Error - ErrorPrevious)/Ts; double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); double u_d = Kd * FilteredErrorDerivative; ErrorPrevious = Error; // sum of parts and return it return u_k + u_i + u_d; //This will become the MotorValue } //Ticker function set motorvalues void SetMotor(double MotorValue, double MotorValue2) { if (MotorValue >=0) { DirectionPin = 1; } else { DirectionPin = 0; } if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle PwmPin = 1; } else { PwmPin = fabs(MotorValue); } if (MotorValue2 >=0) { DirectionPin2 = 1; } else { DirectionPin2 = 0; } if (fabs(MotorValue2)>1) { // if error more than 1 radian, full duty cycle PwmPin2 = 1; } else { PwmPin2 = fabs(MotorValue2); } } // ---------------------------------------------------------------------------- //Ticker function void MeasureAndControl(void) { //double q1Ref = GetReferencePosition(); //double q2Ref = GetReferencePosition2(); OperationStateMachine(); double PositionMotor = GetActualPosition(); double PositionMotor2 = GetActualPosition2(); double MotorValue = PID_Controller(q1Ref - PositionMotor); // input is error double MotorValue2 = PID_Controller2(q2Ref - PositionMotor2); SetMotor(MotorValue, MotorValue2); } //----------------------------------------------------------------------------- int main() { LedGreen = 1; LedRed = 1; LedBlue = 1; pc.baud(115200); pc.printf("Hello World\n\r"); TimerLoop.start(); CurrentOperationState = OPWait; PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!) TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz while (true) { } }