Ruben Lucas
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Diff: StateMachinePTR.cpp
- Revision:
- 6:d7ae5f8fd460
- Parent:
- 5:cbcff21e74a0
- Child:
- 7:f4f0939f9df3
diff -r cbcff21e74a0 -r d7ae5f8fd460 StateMachinePTR.cpp
--- 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