Ruben Lucas
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);
}
}