Operation Mode State Machine plus homing for StateMachinePTR
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of Controller by
OperationMode.cpp
- Committer:
- Roooos
- Date:
- 2018-10-31
- Revision:
- 12:cae29e41ac2e
- Parent:
- 11:dcc416dbe3ea
- Child:
- 13:9125d359619c
File content as of revision 12:cae29e41ac2e:
#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 PositionRef=0; double PositionRef2=0; //Define substates Operation Mode enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming}; States CurrentOperationState; void OperationStateMachine() { switch (CurrentOperationState) { case OPWait: LedRed = 0; //red LedGreen = 1; LedBlue = 1; if (BUT1 == false) { CurrentOperationState = OPState1; TimerLoop.reset(); } if (button_sw3 == false) { CurrentOperationState = OPSet; TimreLoop.reset(); } break; case OPSet: LedRed = 1; //turqoise LedGreen = 0; LedBlue = 0; if (PositionRef2 > -0.853*(6.380)) { PositionRef2 -= 0.0005*(6.380); } if (PositionRef2 < -0.853*(6.380) && TimerLoop > 2) { CurrentOperationState = OPState1; TimerLoop.reset(); } break; case OPState1: LedRed = 0; //yellow LedGreen = 0; LedBlue = 1; if (PositionRef < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4 PositionRef += 0.0005*(6.380); } if ((PositionRef > 0.48*(6.380)) && (TimerLoop >= 4.0)) { CurrentOperationState = OPState2; TimerLoop.reset(); } break; case OPState2: LedRed = 0; //white LedGreen = 0; LedBlue = 0; if (PositionRef2 > -0.40*(6.380)) { PositionRef2 -= 0.0005*(6.380); } if ((PositionRef2 < -0.40*(6.380)) && (TimerLoop >= 4.0)) { CurrentOperationState = OPState3; TimerLoop.reset(); } break; case OPState3: LedRed = 0; //pink LedGreen = 1; LedBlue = 0; if (PositionRef > -0.15*(6.380)) { PositionRef -= 0.0005*(6.380); } if (PositionRef2 > -0.75*(6.380)) { PositionRef2 -= 0.0002*(6.380); } if ((PositionRef < -0.15*(6.380)) && (PositionRef2 < -0.75*(6.380)) && (TimerLoop >= 4.0)) { CurrentOperationState = OPState4; TimerLoop.reset(); } break; case OPState4: LedRed = 1; //blue LedGreen = 1; LedBlue = 0; if (PositionRef2 > -1.4*(6.380)) { PositionRef2 -= 0.005*(6.380); } if ((PositionRef2 < -1.4*(6.380)) && (TimerLoop > 4)) { TimerLoop.reset(); CurrentOperationState = OPHoming; } break; case OPHoming: LedGreen = 0; //Green LedBlue = 1; LedRed = 1; if (PositionRef < 0) { PositionRef += 0.003*(6.380); } if (PositionRef2 < 0) { PositionRef2 += 0.001*(6.380); } if ((PositionRef > 0) && (PositionRef2 > 0) && (TimerLoop > 4)) { CurrentOperationState = OPWait; } break; } } //----------------------------------------------------------------------------- //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 PositionRef = GetReferencePosition(); //double PositionRef2 = GetReferencePosition2(); OperationStateMachine(); double PositionMotor = GetActualPosition(); double PositionMotor2 = GetActualPosition2(); double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error double MotorValue2 = PID_Controller2(PositionRef2 - 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) { } }