![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Diff: StateMachinePTR.cpp
- Revision:
- 12:d19588a50fc7
- Parent:
- 11:2ffae0f2110a
- Child:
- 13:3493825752ac
diff -r 2ffae0f2110a -r d19588a50fc7 StateMachinePTR.cpp --- a/StateMachinePTR.cpp Thu Nov 01 09:08:09 2018 +0000 +++ b/StateMachinePTR.cpp Thu Nov 01 13:16:39 2018 +0000 @@ -8,11 +8,14 @@ //states enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo}; +enum OPStates {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming}; // Global variables States CurrentState; +OPStates CurrentOperationState; Ticker TickerLoop; Timer TimerLoop; +Timer TimerCheck; //Communication MODSERIAL pc(USBTX,USBRX); @@ -22,8 +25,8 @@ //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); +MovingAverage <float>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above +MovingAverage <float>Movag_RB(NSAMPLE,0.0); // filters //Make Biquad filters Left(a0, a1, a2, b1, b2) @@ -58,9 +61,13 @@ //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 +const float L2 = 0.185; // Length link 2 (end-effector length) [m] +const float Ts = 0.002; //Sampling time 500 Hz +// Homing boolean +bool MoveHome = false; +bool Switch2Demo = false; +bool Switch2OP = false; // variables //Motor calibration @@ -69,10 +76,14 @@ //EMG bool EMGBoolLeft; // Boolean EMG 1 (left) bool EMGBoolRight; // Boolean EMG 2 (right) -double LeftBicepsOut; // Final value left of processed EMG -double RightBicepsOut; // Final value right of processed EMG -double ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1) -double ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1) +float LeftBicepsOut; // Final value left of processed EMG +float RightBicepsOut; // Final value right of processed EMG +float ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1) +float ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1) +float MaxLeft = 0.00001; +float MaxRight = 0.00001; +float NormLeft; +float NormRight; // Reference positions float q1Ref = 0; @@ -94,7 +105,7 @@ int xDir; int yDir; float RatioGears = 134.0/30.0; -float RatioPulleys = 87.4/27.5; +float RatioPulleys = 2*87.4/27.5; //Print to screen int PrintFlag = false; int CounterPrint = 0; @@ -125,27 +136,27 @@ 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 + float LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch + float LB_Rectify = fabs(LB_Filter_1); //Rectify Signal Movag_LB.Insert(LB_Rectify); //Moving Average LeftBicepsOut = Movag_LB.GetAverage(); //Get final value + NormLeft = LeftBicepsOut/MaxLeft; //Filter Right Biceps - double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch - double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal + float RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch + float RB_Rectify = fabs(RB_Filter_1); //Rectify Signal Movag_RB.Insert(RB_Rectify); //Moving Average RightBicepsOut = Movag_RB.GetAverage(); //Get final value + NormRight = RightBicepsOut/MaxRight; - if (LeftBicepsOut > ThresholdLeft) { + if (NormLeft > ThresholdLeft) { EMGBoolLeft = true; - } - else { + } else { EMGBoolLeft = false; } - if (RightBicepsOut > ThresholdRight) { + if (NormRight > ThresholdRight) { EMGBoolRight = true; - } - else { + } else { EMGBoolRight = false; } @@ -161,10 +172,10 @@ float Counts2 = Encoder2.getPulses(); q1 = Counts1/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2) q2 = Counts2/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2) - - + + qArm = -1*q1/RatioGears; //Adjust for opposite direction due to gears - qEndEff = q2/RatioPulleys; + qEndEff = q2/RatioPulleys; @@ -175,7 +186,7 @@ { xDir = 1-BUT1.read(); yDir = 1-BUT2.read(); - double Vconst = 0.05; // m/s (3 cm per second) + float Vconst = 0.03; // m/s (3 cm per second) VdesX = xDir*Vconst; // Left biceps is X-direction VdesY = -1*yDir*Vconst; // Right biceps is minus Y-direction } @@ -184,15 +195,15 @@ void InverseKinematics() { // matrix inverse Jacobian - double InvJac11 = (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); - double InvJac12 = -(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); - double InvJac21 = -(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); - double InvJac22 = (L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); + float InvJac11 = (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); + float InvJac12 = -(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); + float InvJac21 = -(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); + float InvJac22 = (L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))); - double q1DotRef = (InvJac11*VdesX + InvJac12*VdesY); //reference angular velocity motor 1 - double q2DotRef = (InvJac21*VdesX + InvJac22*VdesY); //reference angular velocity motor 2 + float q1DotRef = (InvJac11*VdesX + InvJac12*VdesY); //reference angular velocity motor 1 + float q2DotRef = (InvJac21*VdesX + InvJac22*VdesY); //reference angular velocity motor 2 q1Ref += q1DotRef*Ts; // set new reference position motor angle 1 q2Ref += q2DotRef*Ts; // set new reference position motor angle 1 @@ -228,30 +239,51 @@ LedRed = 0; LedGreen = 0; LedBlue = 0; //White - static bool Switch2Demo = false; - static bool Switch2OP = false; - if (BUT2 == false) - { - Switch2Demo = true; - TimerLoop.reset(); - } - if ((Switch2Demo == true) && (TimerLoop.read()>=2.0)) - { - CurrentState = Demo; - Switch2Demo = false; - } - if (BUT1 == false) - { - Switch2OP = true; - TimerLoop.reset(); - } - if ((Switch2OP == true) && (TimerLoop.read()>=2.0)) - { - CurrentState = Operation; - Switch2OP = false; - } - - + + + if (MoveHome == true) { + if (q1Ref >= 0) { + q1Ref -= 0.0005*(6.380); + } + if (q1Ref <= 0) { + q1Ref += 0.0005*(6.380); + } + if (q2Ref >= 0) { + q2Ref -= 0.0005*(6.380); + } + if (q2Ref <= 0) { + q2Ref += 0.0005*(6.380); + } + + if (TimerLoop.read()>=5.0) { //(-0.002*(6.380) <= q1Ref <= 0.002*(6.380)) && (-0.002*(6.380) <= q2Ref <= 0.002*(6.380)) + MoveHome = false; + SetMotorsOff(); + + } + + } //End of if(movehome) statement + + + if (MoveHome == false) { + if (BUT2 == false) { + Switch2Demo = true; + TimerLoop.reset(); + } + if ((Switch2Demo == true) && (TimerLoop.read()>=2.0)) { + CurrentState = Demo; + Switch2Demo = false; + } + if (BUT1 == false) { + Switch2OP = true; + TimerLoop.reset(); + } + if ((Switch2OP == true) && (TimerLoop.read()>=2.0)) { + CurrentState = Operation; + Switch2OP = false; + } + + } //End of else statement + break; case Emergency: @@ -264,18 +296,17 @@ LedRed = 0; LedGreen = 1; LedBlue = 0; //Pink - static double MaxLeft = 0; - static double MaxRight = 0; + if(LeftBicepsOut > MaxLeft) { MaxLeft = LeftBicepsOut; - ThresholdLeft = abs(0.2000*MaxLeft); + ThresholdLeft = abs(0.2000); TimerLoop.reset(); } if(RightBicepsOut > MaxRight) { MaxRight = RightBicepsOut; - ThresholdRight = abs(0.2000*MaxRight); + ThresholdRight = abs(0.2000); TimerLoop.reset(); } @@ -350,32 +381,107 @@ LedRed = 1; LedGreen = 0; LedBlue = 1; //Green + + if (BUTSW2 == false) { + CurrentState = Homing; + CurrentOperationState = OPSet; + MoveHome = true; + TimerLoop.reset(); + } + + switch (CurrentOperationState) { + case OPWait: + if (BUT1 == false) { //When Left Biceps are contracted, initiate chain to flip the page + CurrentOperationState = OPState1; + 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; + } + + break; + case Demo: LedRed = 1; LedGreen = 1; LedBlue = 0; //Blue - //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(); - // } + + if (BUTSW2 == false) { + CurrentState = Homing; + MoveHome = true; + TimerLoop.reset(); + } + break; @@ -387,64 +493,62 @@ void SetErrors() { - if (CurrentState == Demo) - { - Error1 = -1*RatioGears*(q1Ref - qArm); - Error2 = RatioPulleys*(q2Ref - qEndEff); - } - else - Error1 = q1Ref - q1; - Error2 = q2Ref - q2; - + if (CurrentState == Demo) { + Error1 = -1*RatioGears*(q1Ref - qArm); + Error2 = RatioPulleys*(q2Ref - qEndEff); + } else { + Error1 = q1Ref - q1; + Error2 = q2Ref - q2; + } } //------------------------------------------------------------------------------ // controller motor 1 -void PID_Controller1(double Err1) +void PID_Controller1(float Err1) { - double Kp = 19.8; // proportional gain - double Ki = 3.98;//integral gain - double Kd = 1.96; //derivative gain - static double ErrorIntegral = 0; - static double ErrorPrevious = Err1; + float Kp = 19.8; // proportional gain + float Ki = 3.98;//integral gain + float Kd = 1.96; //derivative gain + static float ErrorIntegral = 0; + static float ErrorPrevious = Err1; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: - double u_k = Kp * Err1; + float u_k = Kp * Err1; //Integral part: ErrorIntegral = ErrorIntegral + Err1*Ts; - double u_i = Ki * ErrorIntegral; + float u_i = Ki * ErrorIntegral; //Derivative part: - double ErrorDerivative = (Err1 - ErrorPrevious)/Ts; - double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); - double u_d = Kd * FilteredErrorDerivative; + float ErrorDerivative = (Err1 - ErrorPrevious)/Ts; + float FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); + float u_d = Kd * FilteredErrorDerivative; ErrorPrevious = Err1; MotorValue1 = u_k + u_i + u_d; //This will become the MotorValue to set motor 1 } // controller motor 2 -void PID_Controller2(double Err2) +void PID_Controller2(float Err2) { - double Kp = 11.1; // proportional gain - double Ki = 2.24;//integral gain - double Kd = 1.1; //derivative gain - static double ErrorIntegral = 0; - static double ErrorPrevious = Err2; + float Kp = 11.1; // proportional gain + float Ki = 2.24;//integral gain + float Kd = 1.1; //derivative gain + static float ErrorIntegral = 0; + static float ErrorPrevious = Err2; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: - double u_k = Kp * Err2; + float u_k = Kp * Err2; //Integral part: ErrorIntegral = ErrorIntegral + Err2*Ts; - double u_i = Ki * ErrorIntegral; + float u_i = Ki * ErrorIntegral; //Derivative part: - double ErrorDerivative = (Err2 - ErrorPrevious)/Ts; - double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); - double u_d = Kd * FilteredErrorDerivative; + float ErrorDerivative = (Err2 - ErrorPrevious)/Ts; + float FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); + float u_d = Kd * FilteredErrorDerivative; ErrorPrevious = Err2; MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2 @@ -505,23 +609,27 @@ MotorControllers(); SetMotors(); PrintToScreen(); + TimerCheck.stop(); } int main() { + TimerCheck.start(); PwmPin1.period_us(60); PwmPin2.period_us(60); 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 + CurrentOperationState = OPSet; //set initial state Operation mode 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) { if (PrintFlag == true) { - pc.printf("xButton = %i, yButton = %i, q1Ref = %f, q1 = %f, q2Ref = %f, q2= %f Error1 = %f, Error2 = %f\r",xDir,yDir,q1Ref,q1,q2Ref,q2,Error1,Error2); + float Time = TimerCheck.read(); + pc.printf("Time = %f,BoolLeft = %i, BoolRight = %i, NormLeft = %f, MaxLeft = %f, NormRight = %f, MaxRight = %f ThresholdLeft = %f, ThresholdRight = %f\r",Time,EMGBoolLeft,EMGBoolRight,NormLeft,MaxLeft,NormRight,MaxRight,ThresholdLeft,ThresholdRight); } }