![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
StateMachinePTR.cpp
- Committer:
- rubenlucas
- Date:
- 2018-11-02
- Revision:
- 14:14f42a87cbfb
- Parent:
- 13:3493825752ac
File content as of revision 14:14f42a87cbfb:
#include "mbed.h" #include "math.h" #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" #include "MovingAverage.h" #define NSAMPLE 200 //states enum States {Waiting, Homing, HomingDemo, 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; int CountTime=0; //Communication MODSERIAL pc(USBTX,USBRX); QEI Encoder1(D10,D11,NC,32); // Encoder motor q1 (arm) QEI Encoder2(D12,D13,NC,32); // Encoder motor q2 (end-effector) //EMG settings AnalogIn emg0(A0); //Biceps Left AnalogIn emg1(A1); //Biceps Right 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) BiQuadChain bqc1; //chain voor High Pass en Notch BiQuad bq1(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //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.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //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 PwmOut PwmPin1(D5); DigitalOut DirectionPin1(D4); //Global pin variables Motors 2 PwmOut PwmPin2(D6); DigitalOut DirectionPin2(D7); //Output LED DigitalOut LedRed (LED_RED); DigitalOut LedGreen (LED_GREEN); DigitalOut LedBlue (LED_BLUE); // Buttons DigitalIn BUTSW2(SW2); DigitalIn BUTSW3(SW3); DigitalIn BUT1(D8); DigitalIn BUT2(D9); //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.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 bool NextMotorFlag = false; //EMG bool EMGBoolLeft; // Boolean EMG 1 (left) bool EMGBoolRight; // Boolean EMG 2 (right) 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; float q2Ref = 0; //Motors float q1 = 0; // Current angle of motor 1 (arm) float q2 = 0; // Current angle of motor 2 (end-effector) float qArm = 0; //Current angle of arm float qEndEff = 0; //Current angle of end-effector float MotorValue1; // Inputvalue to set motor 1 float MotorValue2; // Inputvalue to set motor 2 //Inverse kinematics 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 int xDir; int yDir; int Direction; float RatioGears = 134.0/30.0; float RatioPulleys = 87.4/27.5; //Print to screen int PrintFlag = false; 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() { //Filter Left Biceps 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; // Normalized value //Filter Right Biceps 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; // Normalized value if (NormLeft > ThresholdLeft) { EMGBoolLeft = true; } else { EMGBoolLeft = false; } if (NormRight > ThresholdRight) { EMGBoolRight = true; } else { EMGBoolRight = false; } } void MeasureAll() { //Processing and reading EMG ProcessingEMG(); //Measure current motor angles 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) qArm = -1*q1/RatioGears; //Adjust for opposite direction due to gears qEndEff = q2/RatioPulleys; } // Function to set desired cartesian velocities of end-effector void Vdesired() { if(BUTSW3 == false) { Direction = -1; //negative Direction } else { Direction = 1; //Positive Direction } xDir = 1-BUT1.read(); yDir = 1-BUT2.read(); float Vconst = 0.05; // m/s (3 cm per second) VdesX = Direction*EMGBoolLeft*Vconst; // Left biceps is X-direction VdesY = Direction*EMGBoolRight*Vconst; // Right biceps is Y-direction } // Inverse Kinematics void InverseKinematics() { // matrix inverse Jacobian 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))); 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 } //State machine void StateMachine() { switch (CurrentState) { 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 if (BUT2 == false) { CurrentState = EMGCal; TimerLoop.reset(); } break; case Homing: LedRed = 0; LedGreen = 0; LedBlue = 0; //White 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 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) { MoveHome = false; CurrentState = Homing; SetMotorsOff(); } } break; case Emergency: LedRed = 0; LedGreen = 1; LedBlue = 1; //Red break; case EMGCal: LedRed = 0; LedGreen = 1; LedBlue = 0; //Pink if(LeftBicepsOut > MaxLeft) { MaxLeft = LeftBicepsOut; ThresholdLeft = abs(0.6000); // 60% of maximum value TimerLoop.reset(); } if(RightBicepsOut > MaxRight) { MaxRight = RightBicepsOut; ThresholdRight = abs(0.6000); // 60% of maximum value TimerLoop.reset(); } if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 5.0)) { TimerLoop.reset(); CurrentState = MotorCal; } 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.52*(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.733*(6.2830)) { q2Ref +=0.0005*(6.2830); } else { CurrentState = Homing; Encoder1.reset(); Encoder2.reset(); q1Ref = 0; q2Ref = 0; Error1 = 0; Error2 = 0; q1 = 0; q2 = 0; TimerLoop.reset(); } } // end of if (BUTSW2) statement }// end of else if statement break; case Operation: LedRed = 1; LedGreen = 0; LedBlue = 1; //Green if (BUTSW2 == false) { CurrentState = Homing; CurrentOperationState = OPSet; MoveHome = true; TimerLoop.reset(); } switch (CurrentOperationState) { case OPWait: if (EMGBoolLeft == true) { //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 TimerLoop.reset(); Vdesired(); InverseKinematics(); if (BUTSW2 == false) { CurrentState = HomingDemo; MoveHome = true; TimerLoop.reset(); } break; } //End of switch } // End of stateMachine function //------------------------------------------------------------------------------ void SetErrors() { if ((CurrentState == Demo) || (CurrentState == HomingDemo)) { Error1 = -1*RatioGears*(q1Ref - qArm); Error2 = RatioPulleys*(q2Ref - qEndEff); } else { Error1 = q1Ref - q1; Error2 = q2Ref - q2; } } //------------------------------------------------------------------------------ // controller motor 1 void PID_Controller1(float 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: float u_k = Kp * Err1; //Integral part: ErrorIntegral = ErrorIntegral + Err1*Ts; float u_i = Ki * ErrorIntegral; //Derivative part: 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(float 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: float u_k = Kp * Err2; //Integral part: ErrorIntegral = ErrorIntegral + Err2*Ts; float u_i = Ki * ErrorIntegral; //Derivative part: 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 } // Main function for motorcontrol void MotorControllers() { PID_Controller1(Error1); PID_Controller2(Error2); } //------------------------------------------------------------------------------ //Ticker function set motorvalues void SetMotors() { // Motor 1 if (MotorValue1 >=0) { // set direction DirectionPin1 = 1; } else { DirectionPin1 = 0; } if (fabs(MotorValue1)>1) { // set duty cycle PwmPin1 = 1; } else { PwmPin1 = fabs(MotorValue1); } // Motor 2 if (MotorValue2 >=0) { // set direction DirectionPin2 = 1; } else { DirectionPin2 = 0; } if (fabs(MotorValue2)>1) { // set duty cycle PwmPin2 = 1; } else { PwmPin2 = fabs(MotorValue2); } } //----------------------------------------------------------------------------- // Execution function void LoopFunction() { MeasureAll(); StateMachine(); SetErrors(); MotorControllers(); SetMotors(); PrintToScreen(); } 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 CurrentState = Waiting; // set initial state as Waiting CurrentOperationState = OPSet; //set initial state Operation mode 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) { 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); } } }