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:
- 10:93957c339972
- Child:
- 11:dcc416dbe3ea
File content as of revision 10:93957c339972:
#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, OPState1, OPState2, OPState3, OPState4, OPHoming}; States CurrentOperationState; void OperationStateMachine() { switch (CurrentOperationState) { case OPWait: LedRed = 0; //red if (button_sw2 == false) { CurrentOperationState = OPState1; TimerLoop.reset(); } break; case OPState1: LedGreen = 0; //yellow if (PositionRef < 0.24*(6.380)) { //counts/8400 *2 because encoder X2 -> X4 PositionRef += 0.0005*(6.380); } if ((PositionRef > 0.24*(6.380)) && (TimerLoop >= 4.0)) { CurrentOperationState = OPState2; TimerLoop.reset(); } break; case OPState2: LedBlue = 0; //white if (PositionRef2 > -0.23*(6.380)) { PositionRef2 -= 0.0005*(6.380); } if ((PositionRef2 < -0.23*(6.380)) && (TimerLoop >= 4.0)) { CurrentOperationState = OPState3; TimerLoop.reset(); } break; case OPState3: LedGreen = 1; //Turqoise if (PositionRef > -0.005*(6.380)) { PositionRef -= 0.0005*(6.380); } if (PositionRef2 > -0.345*(6.380)) { PositionRef2 -= 0.0005*(6.380); } if ((PositionRef < -0.005*(6.380)) && (PositionRef2 < -0.345*(6.380)) && (TimerLoop >= 4.0)) { CurrentOperationState = OPState4; TimerLoop.reset(); } break; case OPState4: LedGreen = 1; //Blue if (PositionRef2 < 0.595*(6.380)) { PositionRef2 += 0.0005*(6.380); } if (PositionRef2 > 0.595*(6.380)) { CurrentOperationState = OPHoming; } break; case OPHoming: LedGreen = 0; //Green LedBlue = 1; LedRed = 1; 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) { } }