State Machine, bezig met mooimaken
Dependencies: Encoder HIDScope MODSERIAL biquadFilter mbed
Fork of vanEMGnaarMOTORPauline_States_nacht by
main.cpp
- Committer:
- Gerber
- Date:
- 2017-11-06
- Revision:
- 28:19cccdd68b5b
- Parent:
- 27:2d9f172c66ad
- Child:
- 29:69cc48b3feaa
File content as of revision 28:19cccdd68b5b:
//libaries #include "mbed.h" #include "BiQuad.h" #include "HIDScope.h" #include "encoder.h" #include "MODSERIAL.h" // Variables //State Machine and calibration enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration}; States State; bool Position_controller_on; DigitalIn button (D1); //Buttons en leds voor calibration DigitalIn button1(PTA4); DigitalOut led(D2); //MVC for calibration double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0; //MEAN for calibration - rest double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0; double emgMEANSUBLB; double emgMEANSUBRB; double emgMEANSUBLT; double emgMEANSUBRT; double emgSUMLB; double emgSUMRB; double emgSUMLT; double emgSUMRT; bool caldone = false; int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample int Timescalibration = 0; int TimescalibrationREST = 0; //Encoder and motor double Huidigepositie1; double Huidigepositie2; double motorValue1; double motorValue2; Ticker Treecko; //We make an awesome ticker for our control system PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed PwmOut M2E(D5); DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control Encoder motor1(D13,D12,true); Encoder motor2(D9,D8,true); DigitalOut M2D(D4); double PwmPeriod = 1.0/5000.0; //Demonstration AnalogIn potMeter2(A1); AnalogIn potMeter1(A2); MODSERIAL pc(USBTX,USBRX); //PID const double Ts = 0.002f; // tickettijd/ sample time double e_prev = 0; double e_int = 0; double e_prev2 = 0; double e_int2 = 0; // EMG and Filters // Biquad filters voor Left Biceps (LB): Notch, High-pass and Low-pass filter BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFLB; BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFLB; BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFLB; // Biquad filters voor Right Biceps (RB): Notch, High-pass and Low-pass filter BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFRB; BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFRB; BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFRB; // Biquad filters voor Left Triceps (LT): Notch, High-pass and Low-pass filter BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFLT; BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFLT; BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFLT; // Biquad filters for Right Triceps (RT): Notch, High-pass and Low-pass filter BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 ); BiQuadChain NFRT; BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 ); BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); BiQuadChain HPFRT; BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 ); BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 ); BiQuadChain LPFRT; double emgNotchLB; double emgHPLB; double emgAbsHPLB; double emgLPLB; double emgNotchRB; double emgHPRB; double emgAbsHPRB; double emgLPRB; double emgNotchLT; double emgHPLT; double emgAbsHPLT; double emgLPLT; double emgNotchRT; double emgHPRT; double emgAbsHPRT; double emgLPRT; AnalogIn emgLB(A0); // read EMG AnalogIn emgRB(A1); AnalogIn emgLT(A2); AnalogIn emgRT(A3); volatile double LBF; volatile double RBF; volatile double LTF; volatile double RTF; // RKI double pi = 3.14159265359; double q1 = (pi/2); //Reference position angle 1 in radiance double q2 = -(pi/2); //Reference position angle 2 in radiance const double L1 = 0.30; //Length arm 1 in mm const double L2 = 0.38; //Length arm 2 in mm double B1 = 1; //Friction constant motor 1 double B2 = 1; //Friction constant motor 2 double K = 1; //Spring constant movement from end-effector position to setpoint position double Tijd = 1; //Timestep value double Rsx = 0.38; //Reference x-component of the setpoint radius double Rsy = 0.30; //Reference y-component of the setpoint radius double refP = 0; //Reference position motor 1 double refP2 = 0; //Reference position motor 2 double Rex = cos(q1)*L1 - sin(q2)*L2; //The x-component of the end-effector radius double Rey = sin(q1)*L1 + cos(q2)*L2; //The y-component of the end-effector radius double R1x = 0; //The x-component of the joint 1 radius double R1y = 0; //The y-component of the joint 1 radius double R2x = cos(q1)*L1; //The x-component of the joint 2 radius double R2y = sin(q1)*L1; //The y-component of the joint 2 radius double Fx = 0; double Fy = 0; double Tor1 = 0; double Tor2 = 0; double w1= 0; double w2= 0; // Functions void Filteren() { emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0. emgAbsHPLB = abs(emgHPLB); // Take absolute value emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope emgMEANSUBLB = emgLPLB - RESTMEANLB; // Substract the restmean value LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1. emgNotchRB = NFRB.step(emgRB.read()); emgHPRB = HPFRB.step(emgNotchRB); emgAbsHPRB = abs(emgHPRB); emgLPRB = LPFRB.step(emgAbsHPRB); emgMEANSUBLB = emgLPLB - RESTMEANLB; RBF = emgLPRB/MVCRB; emgNotchLT = NFLT.step(emgLT.read() ); emgHPLT = HPFLT.step(emgNotchLT); emgAbsHPLT = abs(emgHPLT); emgLPLT = LPFLT.step(emgAbsHPLT); emgMEANSUBLT = emgLPLT - RESTMEANLT; LTF = emgLPLT/MVCLT; emgNotchRT = NFRT.step(emgRT.read() ); emgHPRT = HPFRT.step(emgNotchRT); emgAbsHPRT = abs(emgHPRT); emgLPRT = LPFRT.step(emgAbsHPRT); emgMEANSUBRT = emgLPRT - RESTMEANRT; RTF = emgLPRT/MVCRT; } void CalibrationEMG() { pc.printf("Timescalibration = %i \r\n",Timescalibration); Timescalibration++; if(Timescalibration<2000) { pc.printf("calibration rest EMG \r\n"); led = 1; emgNotchLB = NFLB.step(emgLB.read() ); emgHPLB = HPFLB.step(emgNotchLB); emgAbsHPLB = abs(emgHPLB); emgLPLB = LPFLB.step(emgAbsHPLB); emgSUMLB += emgLPLB; //SUM all rest values LB emgNotchRB = NFRB.step(emgRB.read()); emgHPRB = HPFRB.step(emgNotchRB); emgAbsHPRB = abs(emgHPRB); emgLPRB = LPFRB.step(emgAbsHPRB); emgSUMRB += emgLPRB; //SUM all rest values RB emgNotchLT = NFLT.step(emgLT.read() ); emgHPLT = HPFLT.step(emgNotchLT); emgAbsHPLT = abs(emgHPLT); emgLPLT = LPFLT.step(emgAbsHPLT); emgSUMLT += emgLPLT; //SUM all rest values LT emgNotchRT = NFRT.step(emgRT.read() ); emgHPRT = HPFRT.step(emgNotchRT); emgAbsHPRT = abs(emgHPRT); emgLPRT = LPFRT.step(emgAbsHPRT); emgSUMRT += emgLPRT; //SUM all rest values RT } if(Timescalibration==1999) { led = 0; RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value } if(Timescalibration>2000 && Timescalibration<3000) { pc.printf("maximum left biceps \r\n"); led = 1; emgNotchLB = NFLB.step(emgLB.read() ); emgHPLB = HPFLB.step(emgNotchLB); emgAbsHPLB = abs(emgHPLB); emgLPLB = LPFLB.step(emgAbsHPLB); double emgfinalLB = emgLPLB; if (emgfinalLB > MVCLB) { //determine what the highest reachable emg signal is MVCLB = emgfinalLB; } } if(Timescalibration>3000 && Timescalibration<4000) { pc.printf(" maximum right biceps \r\n"); led = 0; emgNotchRB = NFRB.step(emgRB.read()); emgHPRB = HPFRB.step(emgNotchRB); emgAbsHPRB = abs(emgHPRB); emgLPRB = LPFRB.step(emgAbsHPRB); double emgfinalRB = emgLPRB; if (emgfinalRB > MVCRB) { //determine what the highest reachable emg signal is MVCRB = emgfinalRB; } } if(Timescalibration>4000 && Timescalibration<5000) { pc.printf("maximum left triceps \r\n"); led = 1; emgNotchLT = NFLT.step(emgLT.read() ); emgHPLT = HPFLT.step(emgNotchLT); emgAbsHPLT = abs(emgHPLT); emgLPLT = LPFLT.step(emgAbsHPLT); double emgfinalLT = emgLPLT; if (emgfinalLT > MVCLT) { //determine what the highest reachable emg signal is MVCLT = emgfinalLT; } } if(Timescalibration>5000 && Timescalibration<6000) { pc.printf("maximum right triceps"); emgNotchRT = NFRT.step(emgRT.read() ); emgHPRT = HPFRT.step(emgNotchRT); emgAbsHPRT = abs(emgHPRT); emgLPRT = LPFRT.step(emgAbsHPRT); double emgfinalRT = emgLPRT; if (emgfinalRT > MVCRT) { //determine what the highest reachable emg signal is MVCRT = emgfinalRT; } } if(Timescalibration>6000) { pc.printf("calibration finished"); State = SelectDevice; } } void RKI() { Rex = cos(q1)*L1 - sin(q2)*L2; Rey = sin(q1)*L1 + cos(q2)*L2; R2x = cos(q1)*L1; R2y = sin(q1)*L1; Fx = (Rsx-Rex)*K; Fy = (Rsy-Rey)*K; Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx; Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx; w1 = Tor1/B1; w2 = Tor2/B2; q1 = q1 + w1*Tijd; q2 = q2 + w2*Tijd; int maxwaarde = 4096; // = 64x64 refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde; refP2 = (( q1 + q2)/(2*pi))*maxwaarde; //Get reference positions } void SetpointRobot() { double Potmeterwaarde2 = potMeter2.read(); double Potmeterwaarde1 = potMeter1.read(); if (Potmeterwaarde2>0.6) { Rsx += 0.001; //increases 1 mm when potmetervalue above 0.6 } else if (Potmeterwaarde2<0.4) { Rsx -= 0.001; //decreases 1 mm when potmetervalue below 0.4 } else { //x value of setpoint doesn't change } if (Potmeterwaarde1>0.6) { //increases 1 mm when potmetervalue above 0.6 Rsy += 0.001; } else if (Potmeterwaarde1<0.4) { //decreases 1 mm when potmetervalue below 0.4 Rsy -= 0.001; } else { //y value of setpoint doesn't change } } double FeedBackControl(double error, double &e_prev, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { double kp = 0.0015; // kind of scaled. double Proportional= kp*error; double kd = 0.000008; // kind of scaled. double VelocityError = (error - e_prev)/Ts; double Derivative = kd*VelocityError; e_prev = error; double ki = 0.0001; // kind of scaled. e_int = e_int+Ts*error; double Integrator = ki*e_int; double motorValue = Proportional + Integrator + Derivative; return motorValue; } double FeedBackControl2(double error2, double &e_prev2, double &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { double kp2 = 0.002; // kind of scaled. double Proportional2= kp2*error2; double kd2 = 0.000008; // kind of scaled. double VelocityError2 = (error2 - e_prev2)/Ts; double Derivative2 = kd2*VelocityError2; e_prev2 = error2; double ki2 = 0.00005; // kind of scaled. e_int2 = e_int2+Ts*error2; double Integrator2 = ki2*e_int2; double motorValue2 = Proportional2 + Integrator2 + Derivative2; return motorValue2; } void SetMotor1(double motorValue) { if (motorValue >= 0) { M1D = 0; } else { M1D = 1; } if (fabs(motorValue) > 1) { M1E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1) } else { M1E = fabs(motorValue); //absolute velocity determined, motor is "off" at value of 0 } } void SetMotor2(double motorValue2) { if (motorValue2 >= 0) { M2D = 1; } else { M2D = 0; } if (fabs(motorValue2) > 1) { M2E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1) } else { M2E = fabs(motorValue2); //absolute velocity determined, motor is "off" at value of 0 } } void MeasureAndControl(void) { RKI(); // control of 1st motor double Huidigepositie = motor1.getPosition(); double error = (refP - Huidigepositie);// make an error double motorValue = FeedBackControl(error, e_prev, e_int); SetMotor1(motorValue); // control of 2nd motor double Huidigepositie2 = motor2.getPosition(); double error2 = (refP2 - Huidigepositie2);// make an error double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); SetMotor2(motorValue2); pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2); } void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!! { if (RBF>0.5) { Rsx +=0.001; // hoe veel verder gaat hij? 1 cm? 10 cm? } else {} if (RTF>0.5) { Rsx -=0.001; } else {} if (LBF>0.5) { Rsy +=0.001; } else {} if (LTF>0.5) { Rsy -=0.001; } else {} } void Loop_funtion() { pc.printf("state machine begint \r\n"); switch(State) { case CalEMG: // Calibration EMG CalibrationEMG(); //calculates average EMGFiltered at rest and measures max signal EMGFiltered. break; case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons State = EMG; if (button==1) { State=EMG; } else { // if (button==0) { State=Demonstration; } break; case EMG: //Control by EMG Filteren(); changePosition(); MeasureAndControl(); break; case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions. case Demonstration: // Control with potmeters SetpointRobot(); MeasureAndControl(); break; } } int main()//deze moet ooit nog weg --> pas op voor errors { //voor EMG filteren //Left Bicep NFLB.add( &N1LB ); HPFLB.add( &HP1LB ).add( &HP2LB ); LPFLB.add( &LP1LB ).add( &LP2LB ); //Right Bicep NFRB.add( &N1RB ); HPFRB.add( &HP1RB ).add( &HP2RB ); LPFRB.add( &LP1RB ).add( &LP2RB ); //Left Tricep NFLT.add( &N1LT ); HPFLT.add( &HP1LT ).add( &HP2LT ); LPFLT.add( &LP1LT ).add( &LP2LT ); //Right Tricep NFRT.add( &N1RT ); HPFRT.add( &HP1RT ).add( &HP2RT ); LPFRT.add( &LP1RT ).add( &LP2RT ); //voor serial pc.baud(115200); pc.printf("begint met programma \r\n"); //motor M1E.period(PwmPeriod); //set PWMposition at 5000hz //Ticker //Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. // printer.attach(Tickerfunctie,0.4); //State Machine State = CalEMG; Position_controller_on = false; Treecko.attach(&Loop_funtion, Ts); while(true) { } //is deze wel nodig? }