Diff: StateMachinePTR.cpp

Revision:

14:14f42a87cbfb

Parent:

13:3493825752ac

--- 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;