StateMachine PTR
Dependencies: MODSERIAL QEI MovingAverage biquadFilter mbed
Diff: StateMachinePTR.cpp
- Revision:
- 3:97cac1d5ba8a
- Parent:
- 2:c2ae5044ec82
- Child:
- 4:4728763bbb44
diff -r c2ae5044ec82 -r 97cac1d5ba8a StateMachinePTR.cpp --- a/StateMachinePTR.cpp Mon Oct 29 14:50:50 2018 +0000 +++ b/StateMachinePTR.cpp Tue Oct 30 13:48:54 2018 +0000 @@ -3,9 +3,19 @@ #include "MODSERIAL.h" #include "QEI.h" #include "BiQuad.h" +#include "MovingAverage.h" +#define NSAMPLE 200 + +// Inverse dingen +//InverseJacobian11 = (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))); +//InverseJacobian12 = -(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))); +//InverseJacobian21 = -(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))); +//InverseJacobian22 = (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))); + + //states -enum States {Waiting, Homing, Emergency,EMGCal,MotorCal,Operation,Demo}; +enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo}; // Global variables States CurrentState; @@ -14,38 +24,125 @@ //Communication MODSERIAL pc(USBTX,USBRX); -QEI Encoder(D10,D11,NC,32); +QEI Encoder1(D10,D11,NC,32); // Encoder motor q1 (arm) +QEI Encoder2(D12,D13,NC,32); // Encoder motor q2 (end-effector) -//Global pin variables Motor 1 -PwmOut PwmPin(D5); -DigitalOut DirectionPin(D4); -AnalogIn Potmeter1(A1); -AnalogIn Potmeter2(A0); +//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); + + // 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 +//Make Biquad filters Right +BiQuadChain bqc2; //chain voor High Pass en Notch +BiQuad bq3(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter +BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch 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 ButtonHoming(SW2); // Button to go to Homing state +DigitalIn BUTSW3(SW3); +DigitalIn BUT1(D8); +DigitalIn BUT2(D9); -// Buttons -DigitalIn Button1(SW2); -DigitalIn Button2(SW3); -DigitalIn ButtonHome(D8); +//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] + + +// Volatile variables +//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) + + +//Motors +volatile float q1; // Current angle of motor 1 (arm) +volatile float q2; // Current angle of motor 2 (end-effector) +volatile float MotorValue1; // Inputvalue to set motor 1 +volatile 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 + -//Global variables for printing on screen -volatile bool PrintFlag = false; -volatile float PosRefPrint; // for printing value on screen -volatile float PosMotorPrint; // for printing value on screen -volatile float ErrorPrint; -volatile int PrintCount = 0; -volatile float MotorValuePrint; - +//------------------------------------------------------------------------------ //------------------------------------------------------------------------------ -//Functions -void StateMachine () +// Function for processing EMG + 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 + 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 + RightBicepsOut = Movag_RB.GetAverage(); //Get final value + + if (LeftBicepsOut > ThresholdLeft) + { + EMGBoolLeft = true; + } + else + { + EMGBoolLeft = false; + } + if (RightBicepsOut > ThresholdRight) + { + EMGBoolRight = true; + } + else + { + EMGBoolRight = false; + } + + } + +void MeasureAll() +{ + //Processing and reading EMG + 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) + +} + +//State machine +void StateMachine() { switch (CurrentState) { @@ -59,9 +156,6 @@ LedRed = 0; LedGreen = 0; LedBlue = 0; //White - if (TimerLoop.read() >= 5.0f) - {CurrentState = Waiting;} - break; case Emergency: @@ -98,154 +192,164 @@ } } -//----------------------------------------------------------------------------- -//The double-functions - -//Get reference position -double GetReferencePosition() +//------------------------------------------------------------------------------ +// Inverse Kinematics +void InverKinematics() { -// This function set the reference position to determine the position of the signal. -// a positive position is defined as a counterclockwise (CCW) rotation - - - double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1) - - - double PositionRef = 3*6.2830*ValuePot - 3*3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations - - return PositionRef; //rad -} - -// actual position of the motor -double GetActualPosition() -{ - //This function determines the actual position of the motor - //The count:radians relation is 8400:2pi - double EncoderCounts = Encoder.getPulses(); //number of counts - double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding - - return PositionMotor; } - -///The controller -double PID_Controller(double Error) -{ - - double Ts = 0.005; //Sampling time 100 Hz +//------------------------------------------------------------------------------ +// controller motor 1 + void PID_Controller1() + { + double Ts = 0.002; //Sampling time 500 Hz + double Kp = 5.34; // proportional gain + double Ki = 2.0;//integral gain + double Kd = 6.9; //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 + } + +// controller motor 2 + void PID_Controller2() + { + double Ts = 0.002; //Sampling time 500 Hz double Kp = 5.34; // proportional gain double Ki = 2.0;//integral gain double Kd = 6.9; //derivative gain static double ErrorIntegral = 0; - static double ErrorPrevious = Error; + static double ErrorPrevious = Error2; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //Proportional part: - double u_k = Kp * Error; + double u_k = Kp * Error2; //Integral part: - ErrorIntegral = ErrorIntegral + Error*Ts; + ErrorIntegral = ErrorIntegral + Error2*Ts; double u_i = Ki * ErrorIntegral; //Derivative part: - double ErrorDerivative = (Error - ErrorPrevious)/Ts; + double ErrorDerivative = (Error2 - ErrorPrevious)/Ts; double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); double u_d = Kd * FilteredErrorDerivative; - ErrorPrevious = Error; + ErrorPrevious = Error2; - return u_k + u_i + u_d; //This will become the MotorValue + 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(); + } +//------------------------------------------------------------------------------ //Ticker function set motorvalues -void SetMotor(double MotorValue) +void SetMotors() { - if (MotorValue >=0) +// Motor 1 + if (MotorValue1 >=0) // set direction { - DirectionPin = 1; + DirectionPin1 = 1; } else { - DirectionPin = 0; + DirectionPin1 = 0; } - if (fabs(MotorValue)>3) // if error more than 1 radian, full duty cycle + if (fabs(MotorValue1)>1) // set duty cycle + { + PwmPin1 = 1; + } + else { - PwmPin = 1; + PwmPin1 = fabs(MotorValue1); + } + +// Motor 2 + if (MotorValue2 >=0) // set direction + { + DirectionPin2 = 1; } else { - PwmPin = 0; //fabs(MotorValue); + DirectionPin2 = 0; } + + if (fabs(MotorValue2)>1) // set duty cycle + { + PwmPin2 = 1; + } + else + { + PwmPin2 = fabs(MotorValue2); + } + } -void SetMotorOff() +void SetMotorsOff() { - PwmPin = 0; // Turn motor off + // Turn motors off + PwmPin1 = 0; + PwmPin2 = 0; } //----------------------------------------------------------------------------- -void MeasureAndControl(void) -{ - double PositionRef = GetReferencePosition(); - double PositionMotor = GetActualPosition(); - double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error - SetMotor(MotorValue); - - //for printing on screen - PosRefPrint = PositionRef; - PosMotorPrint = PositionMotor; - ErrorPrint = PositionRef - PositionMotor; - MotorValuePrint = MotorValue; -} - - - -void PrintToScreen() -{ - PrintCount++; - if (PrintCount == 100) // printing with 2 Hz - {PrintFlag = true; PrintCount = 0;} -} // Execution function void LoopFunction() { // buttons - if (Button1.read() == false) // if pressed - {CurrentState = Operation; TimerLoop.reset();} + // if (Button1.read() == false) // if pressed + // {CurrentState = Operation; TimerLoop.reset();} - if (Button2.read() == false) // if pressed - {CurrentState = Demo; TimerLoop.reset();} + // if (Button2.read() == false) // if pressed + // {CurrentState = Demo; TimerLoop.reset();} - if (ButtonHome.read() == false) // if pressed - {CurrentState = Homing; TimerLoop.reset();} + // if (ButtonHome.read() == false) // if pressed + // {CurrentState = Homing; TimerLoop.reset();} //functions - StateMachine(); //determine states - if (CurrentState >= 4) - {MeasureAndControl(); PrintToScreen();} - else - {SetMotorOff();} + // StateMachine(); //determine states + // if (CurrentState >= 4) + // {MeasureAndControl(); PrintToScreen();} + // else + // {SetMotorOff();} } int main() { pc.baud(115200); pc.printf("Hi I'm PTR, you can call me Peter!\r\n"); - TimerLoop.start(); + TimerLoop.start(); // start Timer CurrentState = Waiting; // set initial state as Waiting + 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) // if-statement for printing every second four times to screen - { - - pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, motorvalue = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,MotorValuePrint); - PrintFlag = false; - } + } } \ No newline at end of file