Ruben Lucas
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Diff: StateMachinePTR.cpp
- Revision:
- 8:428ff7b7715d
- Parent:
- 7:f4f0939f9df3
- Child:
- 9:3410f8dd6845
--- a/StateMachinePTR.cpp Wed Oct 31 13:40:56 2018 +0000
+++ b/StateMachinePTR.cpp Wed Oct 31 16:06:57 2018 +0000
@@ -22,11 +22,11 @@
//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_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
MovingAverage <double>Movag_RB(NSAMPLE,0.0);
-
- // filters
-//Make Biquad filters Left(a0, a1, a2, b1, b2)
+
+// 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 bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
@@ -57,42 +57,42 @@
//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 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
-
-// Volatile variables
+
+// variables
//Motor calibration
-volatile bool NextMotorFlag = false;
+bool NextMotorFlag = false;
//EMG
-volatile bool EMGBoolLeft; // Boolean EMG 1 (left)
-volatile bool EMGBoolRight; // Boolean EMG 2 (right)
-volatile double LeftBicepsOut; // Final value left of processed EMG
-volatile double RightBicepsOut; // Final value right of processed EMG
-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)
+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)
// Reference positions
-volatile float q1Ref = 0;
-volatile float q2Ref = 0;
+float q1Ref = 0;
+float q2Ref = 0;
//Motors
-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
+float q1 = 0; // Current angle of motor 1 (arm)
+float q2 = 0; // Current angle of motor 2 (end-effector)
+float MotorValue1; // Inputvalue to set motor 1
+float MotorValue2; // Inputvalue to set motor 2
//Inverse kinematics
-volatile float VdesX; // Desired velocity in x direction
-volatile float VdesY; // Desired velocity in y direction
-volatile float Error1; // Difference in reference angle and current angle motor 1
-volatile float Error2; // Difference in reference angle and current angle motor 2
+float VdesX; // Desired velocity in x direction
+float VdesY; // Desired velocity in y direction
+float Error1; // Difference in reference angle and current angle motor 1
+float Error2; // Difference in reference angle and current angle motor 2
//Print to screen
-volatile int PrintFlag = false;
-volatile int CounterPrint = 0;
+int PrintFlag = false;
+int CounterPrint = 0;
//------------------------------------------------------------------------------
@@ -100,56 +100,51 @@
void PrintToScreen()
{
- CounterPrint++;
- if (CounterPrint == 100)
- {
- PrintFlag = true;
- CounterPrint = 0;
- }
+ CounterPrint++;
+ if (CounterPrint == 100) {
+ PrintFlag = true;
+ CounterPrint = 0;
+ }
}
// Function to set motors off
void SetMotorsOff()
{
// Turn motors off
- PwmPin1 = 0;
+ PwmPin1 = 0;
PwmPin2 = 0;
-
+
}
// Function for processing EMG
- void ProcessingEMG()
- {
+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
+ double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
Movag_LB.Insert(LB_Rectify); //Moving Average
LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
-
+
//Filter Right Biceps
double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
- Movag_RB.Insert(RB_Rectify); //Moving Average
+ Movag_RB.Insert(RB_Rectify); //Moving Average
RightBicepsOut = Movag_RB.GetAverage(); //Get final value
-
- if (LeftBicepsOut > ThresholdLeft)
- {
- EMGBoolLeft = true;
- }
- else
- {
- EMGBoolLeft = false;
- }
- if (RightBicepsOut > ThresholdRight)
- {
- EMGBoolRight = true;
- }
- else
- {
- EMGBoolRight = false;
- }
-
+
+ if (LeftBicepsOut > ThresholdLeft) {
+ EMGBoolLeft = true;
}
+ else {
+ EMGBoolLeft = false;
+ }
+ if (RightBicepsOut > ThresholdRight) {
+ EMGBoolRight = true;
+ }
+ else {
+ EMGBoolRight = false;
+ }
+
+}
void MeasureAll()
{
@@ -161,9 +156,9 @@
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
@@ -179,17 +174,17 @@
{
// 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 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
-
+
q1Ref = q1 + q1DotRef*Ts; // set new reference position motor angle 1
q2Ref = q2 + q2DotRef*Ts; // set new reference position motor angle 1
}
@@ -197,8 +192,7 @@
//State machine
void StateMachine()
{
- switch (CurrentState)
- {
+ switch (CurrentState) {
case Waiting:
SetMotorsOff();
Encoder1.reset();
@@ -209,176 +203,154 @@
q2Ref = 0;
q2 = 0;
Error2 = 0;
-
+
LedRed = 0;
LedGreen = 0;
LedBlue = 1; //Yellow
-
- if (BUT2 == false)
- {
+
+ if (BUT2 == false) {
CurrentState = EMGCal;
TimerLoop.reset();
}
-
- break;
-
+
+ break;
+
case Homing:
LedRed = 0;
LedGreen = 0;
LedBlue = 0; //White
- if (BUT2 == false)
- {
+ if (BUT2 == false) {
CurrentState = Demo;
- TimerLoop.reset();
+ TimerLoop.reset();
}
-
- if (BUT1 == false)
- {
+
+ if (BUT1 == false) {
CurrentState = Operation;
- TimerLoop.reset();
+ TimerLoop.reset();
}
- break;
-
+ break;
+
case Emergency:
LedRed = 0;
LedGreen = 1;
LedBlue = 1; //Red
- break;
-
+ break;
+
case EMGCal:
LedRed = 0;
LedGreen = 1;
LedBlue = 0; //Pink
static double MaxLeft = 0;
static double MaxRight = 0;
-
- if(LeftBicepsOut > MaxLeft)
- {
- MaxLeft = LeftBicepsOut;
- }
-
- if(RightBicepsOut > MaxRight)
- {
- MaxRight = RightBicepsOut;
- }
-
- if (BUT1 == false)
- {
- ThresholdLeft = abs(0.15000*MaxLeft);
- ThresholdRight = abs(0.15000*MaxRight);
- pc.printf("ThresholdLeft = %f and ThresholdRight = %f\r\n",ThresholdLeft,ThresholdRight);
+
+ if(LeftBicepsOut > MaxLeft) {
+ MaxLeft = LeftBicepsOut;
+ ThresholdLeft = abs(0.2000*MaxLeft);
+ TimerLoop.reset();
+ }
+
+ if(RightBicepsOut > MaxRight) {
+ MaxRight = RightBicepsOut;
+ ThresholdRight = abs(0.2000*MaxRight);
TimerLoop.reset();
}
- if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 2.0))
- {
- CurrentState = MotorCal;
+
+ 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;
}
- break;
-
+ break;
+
case MotorCal:
LedRed = 1;
LedGreen = 0;
LedBlue = 0; //Turqoise
-
- 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)
- {
- if (q1Ref<=0.733*(6.2830))
- {
- q1Ref +=0.0005*(6.2830);
+ if (NextMotorFlag == false) {
+ if (BUT1==false) {
+ q1Ref += 0.0005*(6.2830);
}
- else
- {
- TimerLoop.reset();
- NextMotorFlag = true;
+ if (BUTSW3 == false) {
+ q1Ref = 0;
+ Encoder1.reset();
+ }
+ if (BUT2 == false) {
+ q1Ref -=0.0005*(6.2830);
}
-
- } //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);
+ if (BUTSW2 == false) {
+ 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();
}
- else
- {
- CurrentState = Homing;
- Encoder1.reset();
- Encoder2.reset();
- q1Ref = 0;
- q2Ref = 0;
- TimerLoop.reset();
+ if (BUT2 == false) {
+ q2Ref -= 0.0005*(6.2830);
}
- } // end of if (BUTSW2) statement
- }// end of else if statement
-
- break;
-
+ 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:
LedRed = 1;
LedGreen = 0;
LedBlue = 1; //Green
- break;
-
+ break;
+
case Demo:
LedRed = 1;
LedGreen = 1;
- LedBlue = 0; //Blue
-
+ LedBlue = 0; //Blue
+
//if (TimerLoop.read() <= 5.0)
//{
- // if((EMGBoolLeft == true) || (EMGBoolRight == true))
- //{
- TimerLoop.reset();
- Vdesired();
- InverseKinematics();
- //}
+ // 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;
-
+ // 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
@@ -392,63 +364,63 @@
}
//------------------------------------------------------------------------------
// controller motor 1
- void PID_Controller1()
- {
- 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);
-
- //Proportional part:
- double u_k = Kp * Error1;
-
- //Integral part:
- ErrorIntegral = ErrorIntegral + Error1*Ts;
- double u_i = Ki * ErrorIntegral;
-
- //Derivative part:
- double ErrorDerivative = (Error1 - ErrorPrevious)/Ts;
- double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
- double u_d = Kd * FilteredErrorDerivative;
- ErrorPrevious = Error1;
-
- MotorValue1 = u_k + u_i + u_d; //This will become the MotorValue to set motor 1
- }
-
+void PID_Controller1(double Err1)
+{
+ 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 = Err1;
+ static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+ //Proportional part:
+ double u_k = Kp * Err1;
+
+ //Integral part:
+ ErrorIntegral = ErrorIntegral + Err1*Ts;
+ double u_i = Ki * ErrorIntegral;
+
+ //Derivative part:
+ double ErrorDerivative = (Err1 - ErrorPrevious)/Ts;
+ double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
+ double 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 Kp = 11.1; // proportional gain
- double Ki = 22.81;//integral gain
- double Kd = 1.7; //derivative gain
- static double ErrorIntegral = 0;
- static double ErrorPrevious = Error2;
- static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-
- //Proportional part:
- double u_k = Kp * Error2;
-
- //Integral part:
- ErrorIntegral = ErrorIntegral + Error2*Ts;
- double u_i = Ki * ErrorIntegral;
-
- //Derivative part:
- double ErrorDerivative = (Error2 - ErrorPrevious)/Ts;
- double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
- double u_d = Kd * FilteredErrorDerivative;
- ErrorPrevious = Error2;
-
- MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2
- }
+void PID_Controller2(double Err2)
+{
+ double Kp = 11.1; // proportional gain
+ double Ki = 22.81;//integral gain
+ double Kd = 1.7; //derivative gain
+ static double ErrorIntegral = 0;
+ static double ErrorPrevious = Err2;
+ static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+ //Proportional part:
+ double u_k = Kp * Err2;
+
+ //Integral part:
+ ErrorIntegral = ErrorIntegral + Err2*Ts;
+ double u_i = Ki * ErrorIntegral;
+
+ //Derivative part:
+ double ErrorDerivative = (Err2 - ErrorPrevious)/Ts;
+ double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
+ double u_d = Kd * FilteredErrorDerivative;
+ ErrorPrevious = Err2;
+
+ MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2
+}
// Main function for motorcontrol
void MotorControllers()
-{
- PID_Controller1();
- PID_Controller2();
-
+{
+ PID_Controller1(Error1);
+ PID_Controller2(Error2);
+
}
//------------------------------------------------------------------------------
@@ -456,40 +428,28 @@
void SetMotors()
{
// Motor 1
- if (MotorValue1 >=0) // set direction
- {
+ if (MotorValue1 >=0) { // set direction
DirectionPin1 = 1;
- }
- else
- {
+ } else {
DirectionPin1 = 0;
}
-
- if (fabs(MotorValue1)>1) // set duty cycle
- {
- PwmPin1 = 1;
- }
- else
- {
+
+ if (fabs(MotorValue1)>1) { // set duty cycle
+ PwmPin1 = 1;
+ } else {
PwmPin1 = fabs(MotorValue1);
}
-
+
// Motor 2
- if (MotorValue2 >=0) // set direction
- {
+ if (MotorValue2 >=0) { // set direction
DirectionPin2 = 1;
- }
- else
- {
+ } else {
DirectionPin2 = 0;
}
-
- if (fabs(MotorValue2)>1) // set duty cycle
- {
- PwmPin2 = 1;
- }
- else
- {
+
+ if (fabs(MotorValue2)>1) { // set duty cycle
+ PwmPin2 = 1;
+ } else {
PwmPin2 = fabs(MotorValue2);
}
@@ -514,6 +474,8 @@
int main()
{
+ 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
@@ -521,13 +483,11 @@
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("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);
+
+ while (true) {
+ if (PrintFlag == true) {
+ pc.printf("THleft = %f,THRight = %f,EMG left = %i, EMG right = %i, MOVAG Left = %f, MOVAG Right = %f\r",ThresholdLeft,ThresholdRight,EMGBoolLeft,EMGBoolRight,LeftBicepsOut, RightBicepsOut);
}
}
-
+
}
\ No newline at end of file