![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Diff: StateMachinePTR.cpp
- Revision:
- 6:d7ae5f8fd460
- Parent:
- 5:cbcff21e74a0
- Child:
- 7:f4f0939f9df3
--- a/StateMachinePTR.cpp Tue Oct 30 14:57:45 2018 +0000 +++ b/StateMachinePTR.cpp Wed Oct 31 09:03:51 2018 +0000 @@ -50,7 +50,7 @@ DigitalOut LedBlue (LED_BLUE); // Buttons -DigitalIn ButtonHoming(SW2); // Button to go to Homing state +DigitalIn BUTSW2(SW2); DigitalIn BUTSW3(SW3); DigitalIn BUT1(D8); DigitalIn BUT2(D9); @@ -71,6 +71,9 @@ 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) +// Reference positions +volatile float q1Ref = 0; +volatile float q2Ref = 0; //Motors volatile float q1; // Current angle of motor 1 (arm) @@ -84,11 +87,32 @@ volatile float Error1; // Difference in reference angle and current angle motor 1 volatile float Error2; // Difference in reference angle and current angle motor 2 +//Print to screen +volatile int PrintFlag = false; +volatile int CounterPrint = 0; //------------------------------------------------------------------------------ //------------------------------------------------------------------------------ +void PrintToScreen() +{ + CounterPrint++; + if (CounterPrint == 100) + { + PrintFlag = true; + CounterPrint = 0; + } +} + +// Function to set motors off +void SetMotorsOff() +{ + // Turn motors off + PwmPin1 = 0; + PwmPin2 = 0; +} + // Function for processing EMG void ProcessingEMG() { @@ -134,12 +158,44 @@ } +// Function to set desired cartesian velocities of end-effector +void Vdesired() +{ + double Vconst = 0.001; // m/s (10 cm per second) + VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction + VdesY = -1*EMGBoolRight*Vconst; // Right biceps is minus 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 + + q1Ref = q1 + Error1; + q2Ref = q2 + Error2; +} + //State machine void StateMachine() { switch (CurrentState) { case Waiting: + SetMotorsOff(); LedRed = 0; LedGreen = 0; LedBlue = 1; //Yellow @@ -147,6 +203,7 @@ if (BUT2 == false) { CurrentState = EMGCal; + TimerLoop.reset(); } break; @@ -155,6 +212,17 @@ LedRed = 0; LedGreen = 0; LedBlue = 0; //White + if (BUT2 == false) + { + CurrentState = Demo; + TimerLoop.reset(); + } + + if (BUT1 == false) + { + CurrentState = Operation; + TimerLoop.reset(); + } break; case Emergency: @@ -184,9 +252,10 @@ { ThresholdLeft = abs(0.15000*MaxLeft); ThresholdRight = abs(0.15000*MaxRight); + pc.printf("ThresholdLeft = %f and ThresholdRight = %f\r\n",ThresholdLeft,ThresholdRight); TimerLoop.reset(); } - if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 2)) + if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 2.0)) { CurrentState = MotorCal; TimerLoop.reset(); @@ -197,6 +266,11 @@ LedRed = 1; LedGreen = 0; LedBlue = 0; //Turqoise + if (BUTSW2 == false) + { + CurrentState = Homing; + TimerLoop.reset(); + } break; case Operation: @@ -208,7 +282,28 @@ case Demo: LedRed = 1; LedGreen = 1; - LedBlue = 0; //Blue + LedBlue = 0; //Blue + PrintToScreen(); + if (TimerLoop.read() <= 5.0) + { + if((EMGBoolLeft == true) || (EMGBoolRight == true)) + { + TimerLoop.reset(); + Vdesired(); + InverseKinematics(); + } + } + else + { + q1Ref = 0; + q2Ref = 0; + Error1 = q1Ref - q1; + Error2 = q2Ref - q2; + if ((Error1 <= 0.1) && (Error2 <= 0.1)) + { + TimerLoop.reset(); + } + } break; @@ -216,33 +311,7 @@ } //------------------------------------------------------------------------------ -// 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 -} //------------------------------------------------------------------------------ @@ -350,12 +419,8 @@ } -void SetMotorsOff() -{ - // Turn motors off - PwmPin1 = 0; - PwmPin2 = 0; -} + + //----------------------------------------------------------------------------- @@ -363,21 +428,10 @@ // 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();} + MeasureAll(); + StateMachine(); + MotorControllers(); + SetMotors(); } int main() @@ -392,7 +446,10 @@ while (true) { - + if (PrintFlag == true) + { + pc.printf("EMG left = %i, EMG right = %i, q1Ref = %f, q1 = %f, q2Ref = %f, q2 = %f\r",EMGBoolLeft,EMGBoolRight,q1Ref,q1,q2Ref,q2); + } } } \ No newline at end of file