StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
StateMachinePTR.cpp
- Committer:
- rubenlucas
- Date:
- 2018-10-30
- Revision:
- 5:cbcff21e74a0
- Parent:
- 4:4728763bbb44
- Child:
- 6:d7ae5f8fd460
File content as of revision 5:cbcff21e74a0:
#include "mbed.h" #include "math.h" #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" #include "MovingAverage.h" #define NSAMPLE 200 //states enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo}; // Global variables States CurrentState; Ticker TickerLoop; Timer TimerLoop; //Communication MODSERIAL pc(USBTX,USBRX); QEI Encoder1(D10,D11,NC,32); // Encoder motor q1 (arm) QEI Encoder2(D12,D13,NC,32); // Encoder motor q2 (end-effector) //EMG settings AnalogIn emg0(A0); //Biceps Left AnalogIn emg1(A1); //Biceps Right MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above MovingAverage <double>Movag_RB(NSAMPLE,0.0); // filters //Make Biquad filters Left(a0, a1, a2, b1, b2) BiQuadChain bqc1; //chain voor High Pass en Notch BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter //Make Biquad filters Right BiQuadChain bqc2; //chain voor High Pass en Notch BiQuad bq3(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter //Global pin variables Motors 1 PwmOut PwmPin1(D5); DigitalOut DirectionPin1(D4); //Global pin variables Motors 2 PwmOut PwmPin2(D6); DigitalOut DirectionPin2(D7); //Output LED DigitalOut LedRed (LED_RED); DigitalOut LedGreen (LED_GREEN); DigitalOut LedBlue (LED_BLUE); // Buttons DigitalIn ButtonHoming(SW2); // Button to go to Homing state DigitalIn BUTSW3(SW3); DigitalIn BUT1(D8); DigitalIn BUT2(D9); //Constant variables const float L0 = 0.1; // Distance between origin frame and joint 1 [m[ const float L1 = 0.326; // Length link 1 (shaft to shaft) [m] const float L2 = 0.209; // Length link 2 (end-effector length) [m] const double Ts = 0.002; //Sampling time 500 Hz // Volatile variables //EMG volatile bool EMGBoolLeft; // Boolean EMG 1 (left) volatile bool EMGBoolRight; // Boolean EMG 2 (right) volatile double LeftBicepsOut; // Final value left of processed EMG volatile double RightBicepsOut; // Final value right of processed EMG volatile double ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1) volatile double ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1) //Motors volatile float q1; // Current angle of motor 1 (arm) volatile float q2; // Current angle of motor 2 (end-effector) volatile float MotorValue1; // Inputvalue to set motor 1 volatile float MotorValue2; // Inputvalue to set motor 2 //Inverse kinematics volatile float VdesX; // Desired velocity in x direction volatile float VdesY; // Desired velocity in y direction volatile float Error1; // Difference in reference angle and current angle motor 1 volatile float Error2; // Difference in reference angle and current angle motor 2 //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ // Function for processing EMG void ProcessingEMG() { //Filter Left Biceps double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal Movag_LB.Insert(LB_Rectify); //Moving Average LeftBicepsOut = Movag_LB.GetAverage(); //Get final value //Filter Right Biceps double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal Movag_RB.Insert(RB_Rectify); //Moving Average RightBicepsOut = Movag_RB.GetAverage(); //Get final value if (LeftBicepsOut > ThresholdLeft) { EMGBoolLeft = true; } else { EMGBoolLeft = false; } if (RightBicepsOut > ThresholdRight) { EMGBoolRight = true; } else { EMGBoolRight = false; } } void MeasureAll() { //Processing and reading EMG ProcessingEMG(); //Measure current motor angles q1 = Encoder1.getPulses()/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2) q2 = Encoder2.getPulses()/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2) } //State machine void StateMachine() { switch (CurrentState) { case Waiting: LedRed = 0; LedGreen = 0; LedBlue = 1; //Yellow if (BUT2 == false) { CurrentState = EMGCal; } break; case Homing: LedRed = 0; LedGreen = 0; LedBlue = 0; //White break; case Emergency: LedRed = 0; LedGreen = 1; LedBlue = 1; //Red break; case EMGCal: LedRed = 0; LedGreen = 1; LedBlue = 0; //Pink static double MaxLeft = 0; static double MaxRight = 0; if(LeftBicepsOut > MaxLeft) { MaxLeft = LeftBicepsOut; } if(RightBicepsOut > MaxRight) { MaxRight = RightBicepsOut; } if (BUT1 == false) { ThresholdLeft = abs(0.15000*MaxLeft); ThresholdRight = abs(0.15000*MaxRight); TimerLoop.reset(); } if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 2)) { CurrentState = MotorCal; TimerLoop.reset(); } break; case MotorCal: LedRed = 1; LedGreen = 0; LedBlue = 0; //Turqoise break; case Operation: LedRed = 1; LedGreen = 0; LedBlue = 1; //Green break; case Demo: LedRed = 1; LedGreen = 1; LedBlue = 0; //Blue break; } } //------------------------------------------------------------------------------ // Function to set desired cartesian velocities of end-effector void Vdesired() { double Vconst = 0.1; // m/s (10 cm per second) VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction VdesY = EMGBoolRight*Vconst; // Right biceps is Y-direction } // Inverse Kinematics void InverseKinematics() { // matrix inverse Jacobian double InvJac11 = (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))); double InvJac12 = -(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))); double InvJac21 = -(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))); double InvJac22 = (L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))); //Gear Ratio's double RatioGears = 134.0/30.0; //Gear Ratio for motor 1 double RatioPulleys = 87.4/27.5; // Gear Ratio for motor 2 double q1DotRef = (InvJac11*VdesX + InvJac12*VdesY)*RatioGears; //reference angular velocity motor 1 double q2DotRef = (InvJac21*VdesX + InvJac22*VdesY)*RatioPulleys; //reference angular velocity motor 2 Error1 = q1DotRef*Ts; // difference between qReference and current q1 Error2 = q2DotRef*Ts; // difference between qReference and current q2 } //------------------------------------------------------------------------------ // controller motor 1 void PID_Controller1() { double Kp = 19.8; // proportional gain double Ki = 40.9;//integral gain double Kd = 3; //derivative gain static double ErrorIntegral = 0; static double ErrorPrevious = Error1; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: double u_k = Kp * Error1; //Integral part: ErrorIntegral = ErrorIntegral + Error1*Ts; double u_i = Ki * ErrorIntegral; //Derivative part: double ErrorDerivative = (Error1 - ErrorPrevious)/Ts; double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); double u_d = Kd * FilteredErrorDerivative; ErrorPrevious = Error1; MotorValue1 = u_k + u_i + u_d; //This will become the MotorValue to set motor 1 } // controller motor 2 void PID_Controller2() { double Kp = 11.1; // proportional gain double Ki = 22.81;//integral gain double Kd = 1.7; //derivative gain static double ErrorIntegral = 0; static double ErrorPrevious = Error2; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: double u_k = Kp * Error2; //Integral part: ErrorIntegral = ErrorIntegral + Error2*Ts; double u_i = Ki * ErrorIntegral; //Derivative part: double ErrorDerivative = (Error2 - ErrorPrevious)/Ts; double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); double u_d = Kd * FilteredErrorDerivative; ErrorPrevious = Error2; MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2 } // Main function for motorcontrol void MotorControllers() { PID_Controller1(); PID_Controller2(); } //------------------------------------------------------------------------------ //Ticker function set motorvalues void SetMotors() { // Motor 1 if (MotorValue1 >=0) // set direction { DirectionPin1 = 1; } else { DirectionPin1 = 0; } if (fabs(MotorValue1)>1) // set duty cycle { PwmPin1 = 1; } else { PwmPin1 = fabs(MotorValue1); } // Motor 2 if (MotorValue2 >=0) // set direction { DirectionPin2 = 1; } else { DirectionPin2 = 0; } if (fabs(MotorValue2)>1) // set duty cycle { PwmPin2 = 1; } else { PwmPin2 = fabs(MotorValue2); } } void SetMotorsOff() { // Turn motors off PwmPin1 = 0; PwmPin2 = 0; } //----------------------------------------------------------------------------- // Execution function void LoopFunction() { // buttons // if (Button1.read() == false) // if pressed // {CurrentState = Operation; TimerLoop.reset();} // if (Button2.read() == false) // if pressed // {CurrentState = Demo; TimerLoop.reset();} // if (ButtonHome.read() == false) // if pressed // {CurrentState = Homing; TimerLoop.reset();} //functions // StateMachine(); //determine states // if (CurrentState >= 4) // {MeasureAndControl(); PrintToScreen();} // else // {SetMotorOff();} } int main() { pc.baud(115200); pc.printf("Hi I'm PTR, you can call me Peter!\r\n"); TimerLoop.start(); // start Timer CurrentState = Waiting; // set initial state as Waiting bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain EMG left bqc2.add( &bq3 ).add( &bq4 ); //make BiQuadChain EMG right TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz while (true) { } }