![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Revision 14:14f42a87cbfb, committed 2018-11-02
- Comitter:
- rubenlucas
- Date:
- Fri Nov 02 12:11:17 2018 +0000
- Parent:
- 13:3493825752ac
- Commit message:
- Final Version
Changed in this revision
StateMachinePTR.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 3493825752ac -r 14f42a87cbfb StateMachinePTR.cpp --- a/StateMachinePTR.cpp Thu Nov 01 15:23:34 2018 +0000 +++ b/StateMachinePTR.cpp Fri Nov 02 12:11:17 2018 +0000 @@ -32,12 +32,12 @@ // filters //Make Biquad filters Left(a0, a1, a2, b1, b2) BiQuadChain bqc1; //chain voor High Pass en Notch -BiQuad bq1(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013);//0.9142135372357942,-1.8284270744715885,0.9142135372357942,-1.821189693211669,0.8356644557315082); //High Pass Filter +BiQuad bq1(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //High Pass Filter BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter BiQuad bq5(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter //Make Biquad filters Right BiQuadChain bqc2; //chain voor High Pass en Notch -BiQuad bq3(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013);//0.9142135372357942,-1.8284270744715885,0.9142135372357942,-1.821189693211669,0.8356644557315082); //High Pass Filter +BiQuad bq3(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //High Pass Filter BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter BiQuad bq6(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter @@ -144,14 +144,14 @@ 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; + NormLeft = LeftBicepsOut/MaxLeft; // Normalized value //Filter Right Biceps 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; + NormRight = RightBicepsOut/MaxRight; // Normalized value if (NormLeft > ThresholdLeft) { EMGBoolLeft = true; @@ -190,17 +190,17 @@ { if(BUTSW3 == false) { - Direction = -1; + Direction = -1; //negative Direction } else { - Direction = 1; + Direction = 1; //Positive Direction } xDir = 1-BUT1.read(); yDir = 1-BUT2.read(); float Vconst = 0.05; // m/s (3 cm per second) - VdesX = Direction*xDir*Vconst; //EMGBoolLeft*Vconst; // Left biceps is X-direction - VdesY = Direction*yDir*Vconst;//-1*EMGBoolRight*Vconst; // Right biceps is minus Y-direction + VdesX = Direction*EMGBoolLeft*Vconst; // Left biceps is X-direction + VdesY = Direction*EMGBoolRight*Vconst; // Right biceps is Y-direction } // Inverse Kinematics @@ -313,7 +313,7 @@ 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)) + if (TimerLoop.read()>=5.0) { MoveHome = false; CurrentState = Homing; SetMotorsOff(); @@ -336,20 +336,17 @@ if(LeftBicepsOut > MaxLeft) { MaxLeft = LeftBicepsOut; - ThresholdLeft = abs(0.6000); + ThresholdLeft = abs(0.6000); // 60% of maximum value TimerLoop.reset(); } if(RightBicepsOut > MaxRight) { MaxRight = RightBicepsOut; - ThresholdRight = abs(0.6000); + ThresholdRight = abs(0.6000); // 60% of maximum value TimerLoop.reset(); } - if (BUT1 == false) { - //ThresholdLeft = abs(0.15000*MaxLeft); - //ThresholdRight = abs(0.15000*MaxRight); - } + if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 5.0)) { TimerLoop.reset(); CurrentState = MotorCal;