Diff: StateMachinePTR.cpp

Revision:

13:3493825752ac

Parent:

12:d19588a50fc7

Child:

14:14f42a87cbfb

--- a/StateMachinePTR.cpp	Thu Nov 01 13:16:39 2018 +0000
+++ b/StateMachinePTR.cpp	Thu Nov 01 15:23:34 2018 +0000
@@ -7,7 +7,7 @@
 #define NSAMPLE 200
 
 //states
-enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo};
+enum States {Waiting, Homing, HomingDemo, Emergency, EMGCal, MotorCal, Operation, Demo};
 enum OPStates {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
 
 // Global variables
@@ -16,6 +16,7 @@
 Ticker TickerLoop;
 Timer TimerLoop;
 Timer TimerCheck;
+int CountTime=0;
 
 //Communication
 MODSERIAL pc(USBTX,USBRX);
@@ -31,12 +32,14 @@
 // 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 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 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.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
+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 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
 
 
 //Global pin variables Motors 1
@@ -104,8 +107,9 @@
 float Error2; // Difference in reference angle and current angle motor 2
 int xDir;
 int yDir;
+int Direction;
 float RatioGears = 134.0/30.0;
-float RatioPulleys = 2*87.4/27.5;
+float RatioPulleys = 87.4/27.5;
 //Print to screen
 int PrintFlag = false;
 int CounterPrint = 0;
@@ -184,11 +188,19 @@
 // Function to set desired cartesian velocities of end-effector
 void Vdesired()
 {
+    if(BUTSW3 == false)
+    {
+    Direction = -1;
+    }
+    else
+    {
+    Direction = 1;
+    }
     xDir = 1-BUT1.read();
     yDir = 1-BUT2.read();
-    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
+    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
 }
 
 // Inverse Kinematics
@@ -286,6 +298,30 @@
 
             break;
 
+        case HomingDemo:
+            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;
+                    CurrentState = Homing;
+                    SetMotorsOff();
+
+                }
+            }
+        break;
+
         case Emergency:
             LedRed = 0;
             LedGreen = 1;
@@ -300,13 +336,13 @@
 
             if(LeftBicepsOut > MaxLeft) {
                 MaxLeft = LeftBicepsOut;
-                ThresholdLeft = abs(0.2000);
+                ThresholdLeft = abs(0.6000);
                 TimerLoop.reset();
             }
 
             if(RightBicepsOut > MaxRight) {
                 MaxRight = RightBicepsOut;
-                ThresholdRight = abs(0.2000);
+                ThresholdRight = abs(0.6000);
                 TimerLoop.reset();
             }
 
@@ -391,7 +427,7 @@
 
             switch (CurrentOperationState) {
                 case OPWait:
-                    if (BUT1 == false) {    //When Left Biceps are contracted, initiate chain to flip the page
+                    if (EMGBoolLeft == true) {    //When Left Biceps are contracted, initiate chain to flip the page
                         CurrentOperationState = OPState1;
                         TimerLoop.reset();
                     }
@@ -477,7 +513,7 @@
             InverseKinematics();
 
             if (BUTSW2 == false) {
-                CurrentState = Homing;
+                CurrentState = HomingDemo;
                 MoveHome = true;
                 TimerLoop.reset();
             }
@@ -493,7 +529,7 @@
 
 void SetErrors()
 {
-    if (CurrentState == Demo) {
+    if ((CurrentState == Demo) || (CurrentState == HomingDemo)) {
         Error1 = -1*RatioGears*(q1Ref - qArm);
         Error2 = RatioPulleys*(q2Ref - qEndEff);
     } else {
@@ -609,12 +645,11 @@
     MotorControllers();
     SetMotors();
     PrintToScreen();
-    TimerCheck.stop();
+
 }
 
 int main()
 {
-    TimerCheck.start();
     PwmPin1.period_us(60);
     PwmPin2.period_us(60);
     pc.baud(115200);
@@ -622,14 +657,13 @@
     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
+    bqc1.add( &bq1 ).add( &bq2 ).add( &bq5 ); //make BiQuadChain EMG left
+    bqc2.add( &bq3 ).add( &bq4 ).add( &bq6 ); //make BiQuadChain EMG right
     TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
 
     while (true) {
         if (PrintFlag == true) {
-            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);
+            pc.printf("BoolLeft = %i, BoolRight = %i, NormLeft = %f, MaxLeft = %f, NormRight = %f, MaxRight = %f ThresholdLeft = %f, ThresholdRight = %f\r",EMGBoolLeft,EMGBoolRight,NormLeft,MaxLeft,NormRight,MaxRight,ThresholdLeft,ThresholdRight);
         }
     }