Ruben Lucas
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Diff: StateMachinePTR.cpp
- Revision:
- 7:f4f0939f9df3
- Parent:
- 6:d7ae5f8fd460
- Child:
- 8:428ff7b7715d
--- a/StateMachinePTR.cpp Wed Oct 31 09:03:51 2018 +0000
+++ b/StateMachinePTR.cpp Wed Oct 31 13:40:56 2018 +0000
@@ -63,6 +63,9 @@
// Volatile variables
+//Motor calibration
+volatile bool NextMotorFlag = false;
+
//EMG
volatile bool EMGBoolLeft; // Boolean EMG 1 (left)
volatile bool EMGBoolRight; // Boolean EMG 2 (right)
@@ -76,8 +79,8 @@
volatile float q2Ref = 0;
//Motors
-volatile float q1; // Current angle of motor 1 (arm)
-volatile float q2; // Current angle of motor 2 (end-effector)
+volatile float q1 = 0; // Current angle of motor 1 (arm)
+volatile float q2 = 0; // Current angle of motor 2 (end-effector)
volatile float MotorValue1; // Inputvalue to set motor 1
volatile float MotorValue2; // Inputvalue to set motor 2
@@ -111,6 +114,7 @@
// Turn motors off
PwmPin1 = 0;
PwmPin2 = 0;
+
}
// Function for processing EMG
@@ -153,15 +157,19 @@
ProcessingEMG();
//Measure current motor angles
- q1 = Encoder1.getPulses()/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
- q2 = Encoder2.getPulses()/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
+ float Counts1 = Encoder1.getPulses();
+ 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)
+
+
}
// Function to set desired cartesian velocities of end-effector
void Vdesired()
{
- double Vconst = 0.001; // m/s (10 cm per second)
+ double Vconst = 0.05; // m/s (5 cm per second)
VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction
VdesY = -1*EMGBoolRight*Vconst; // Right biceps is minus Y-direction
}
@@ -181,12 +189,9 @@
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;
+
+ q1Ref = q1 + q1DotRef*Ts; // set new reference position motor angle 1
+ q2Ref = q2 + q2DotRef*Ts; // set new reference position motor angle 1
}
//State machine
@@ -196,6 +201,15 @@
{
case Waiting:
SetMotorsOff();
+ Encoder1.reset();
+ q1Ref = 0;
+ q1 = 0;
+ Error1 = 0;
+ Encoder2.reset();
+ q2Ref = 0;
+ q2 = 0;
+ Error2 = 0;
+
LedRed = 0;
LedGreen = 0;
LedBlue = 1; //Yellow
@@ -266,11 +280,70 @@
LedRed = 1;
LedGreen = 0;
LedBlue = 0; //Turqoise
- if (BUTSW2 == false)
+
+ if (NextMotorFlag == false)
+ {
+ if (BUT1==false)
+ {
+ q1Ref += 0.0005*(6.2830);
+ }
+ if (BUTSW3 == false)
+ {
+ q1Ref = 0;
+ Encoder1.reset();
+ }
+ if (BUT2 == false)
+ {
+ q1Ref -=0.0005*(6.2830);
+ }
+ if (BUTSW2 == false)
{
- CurrentState = Homing;
- TimerLoop.reset();
+ if (q1Ref<=0.733*(6.2830))
+ {
+ q1Ref +=0.0005*(6.2830);
+ }
+ else
+ {
+ TimerLoop.reset();
+ NextMotorFlag = true;
+ }
+
+ } //End of if (BUTSW2 == false)
+ } //End of if (NextMotorFlag == false)
+
+ else if ((NextMotorFlag == true) && (TimerLoop.read()>= 2))
+ {
+ if (BUT1==false)
+ {
+ q2Ref += 0.0005*(6.2830);
}
+ if (BUTSW3 == false)
+ {
+ q2Ref = 0;
+ Encoder2.reset();
+ }
+ if (BUT2 == false)
+ {
+ q2Ref -= 0.0005*(6.2830);
+ }
+ if (BUTSW2 == false)
+ {
+ if (q2Ref>=-0.52*(6.2830))
+ {
+ q2Ref -=0.0005*(6.2830);
+ }
+ else
+ {
+ CurrentState = Homing;
+ Encoder1.reset();
+ Encoder2.reset();
+ q1Ref = 0;
+ q2Ref = 0;
+ TimerLoop.reset();
+ }
+ } // end of if (BUTSW2) statement
+ }// end of else if statement
+
break;
case Operation:
@@ -283,44 +356,47 @@
LedRed = 1;
LedGreen = 1;
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();
- }
- }
+
+ //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;
- }
-}
+ } //End of switch
+} // End of stateMachine function
//------------------------------------------------------------------------------
-
-
+void SetErrors()
+{
+ Error1 = q1Ref - q1;
+ Error2 = q2Ref - q2;
+}
//------------------------------------------------------------------------------
// controller motor 1
void PID_Controller1()
{
- double Kp = 19.8; // proportional gain
- double Ki = 40.9;//integral gain
- double Kd = 3; //derivative gain
+ 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 = Error1;
static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
@@ -430,8 +506,10 @@
{
MeasureAll();
StateMachine();
+ SetErrors();
MotorControllers();
SetMotors();
+ PrintToScreen();
}
int main()
@@ -448,7 +526,7 @@
{
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);
+ pc.printf("THleft = %f,THRight = %f,EMG left = %i, EMG right = %i, q1Ref = %f, q1 = %f, q2Ref = %f, q2 = %f,Error1 = %f, Error2 = %f\r",ThresholdLeft,ThresholdRight,EMGBoolLeft,EMGBoolRight,q1Ref,q1,q2Ref,q2,Error1,Error2);
}
}