Script of MBR Group 20. Control of robot by EMG and/or potmeters
Dependencies: Encoder HIDScope MODSERIAL biquadFilter mbed
Fork of Script_Group_20 by
Diff: main.cpp
- Revision:
- 28:19cccdd68b5b
- Parent:
- 27:2d9f172c66ad
- Child:
- 29:69cc48b3feaa
--- a/main.cpp Mon Nov 06 14:57:15 2017 +0000 +++ b/main.cpp Mon Nov 06 15:52:46 2017 +0000 @@ -5,29 +5,43 @@ #include "encoder.h" #include "MODSERIAL.h" +// Variables //State Machine and calibration enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration}; States State; -int Counter; bool Position_controller_on; -double looptime = 0.002f; DigitalIn button (D1); -//Encoder/motor +//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; - -//global variables Motor Ticker Treecko; //We make an awesome ticker for our control system -Ticker printer; 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); @@ -35,40 +49,14 @@ MODSERIAL pc(USBTX,USBRX); -double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) +//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; -double tijdstap = 0.002; -volatile double LBF; -volatile double RBF; -volatile double LTF; -volatile double RTF; - -//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; - -// Filters +// 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; @@ -129,28 +117,30 @@ double emgAbsHPRT; double emgLPRT; -double f = 500; // frequency -double dt = 1/f; // sample frequency - -AnalogIn emgLB(A0); // EMG lezen +AnalogIn emgLB(A0); // read EMG AnalogIn emgRB(A1); AnalogIn emgLT(A2); AnalogIn emgRT(A3); -// variables RKI +volatile double LBF; +volatile double RBF; +volatile double LTF; +volatile double RTF; + +// RKI double pi = 3.14159265359; -double q1 = (pi/2); //Reference position hoek 1 in radiance -double q2 = -(pi/2); //Reference position hoek 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 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 @@ -164,41 +154,36 @@ double w1= 0; double w2= 0; - +// Functions void Filteren() { - emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter + 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. + 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()); // Notch filter - emgHPRB = HPFRB.step(emgNotchRB); // High-pass filter: also normalises around 0. - emgAbsHPRB = abs(emgHPRB); // Take absolute value - emgLPRB = LPFRB.step(emgAbsHPRB); // Low-pass filter: creates envelope + emgNotchRB = NFRB.step(emgRB.read()); + emgHPRB = HPFRB.step(emgNotchRB); + emgAbsHPRB = abs(emgHPRB); + emgLPRB = LPFRB.step(emgAbsHPRB); emgMEANSUBLB = emgLPLB - RESTMEANLB; - RBF = emgLPRB/MVCRB; // Scale to maximum signal: useful for motor + RBF = emgLPRB/MVCRB; - emgNotchLT = NFLT.step(emgLT.read() ); // Notch filter - emgHPLT = HPFLT.step(emgNotchLT); // High-pass filter: also normalises around 0. - emgAbsHPLT = abs(emgHPLT); // Take absolute value - emgLPLT = LPFLT.step(emgAbsHPLT); // Low-pass filter: creates envelope - emgMEANSUBLT = emgLPLT - RESTMEANLT; //substract the restmean value - LTF = emgLPLT/MVCLT; // Scale to maximum signal: useful for motor + 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() ); // Notch filter - emgHPRT = HPFRT.step(emgNotchRT); // High-pass filter: also normalises around 0. - emgAbsHPRT = abs(emgHPRT); // Take absolute value - emgLPRT = LPFRT.step(emgAbsHPRT); // Low-pass filter: creates envelope - emgMEANSUBRT = emgLPRT - RESTMEANRT; //substract the restmean value - RTF = emgLPRT/MVCRT; // Scale to maximum signal: useful for motor - - //if (emgFiltered >1) - //{ - // emgFiltered=1.00; - //} + emgNotchRT = NFRT.step(emgRT.read() ); + emgHPRT = HPFRT.step(emgNotchRT); + emgAbsHPRT = abs(emgHPRT); + emgLPRT = LPFRT.step(emgAbsHPRT); + emgMEANSUBRT = emgLPRT - RESTMEANRT; + RTF = emgLPRT/MVCRT; } void CalibrationEMG() @@ -245,7 +230,7 @@ } if(Timescalibration>2000 && Timescalibration<3000) { - pc.printf("maximum linker biceps \r\n"); + pc.printf("maximum left biceps \r\n"); led = 1; emgNotchLB = NFLB.step(emgLB.read() ); emgHPLB = HPFLB.step(emgNotchLB); @@ -260,7 +245,7 @@ if(Timescalibration>3000 && Timescalibration<4000) { - pc.printf(" maximum rechter biceps \r\n"); + pc.printf(" maximum right biceps \r\n"); led = 0; emgNotchRB = NFRB.step(emgRB.read()); emgHPRB = HPFRB.step(emgNotchRB); @@ -275,7 +260,7 @@ if(Timescalibration>4000 && Timescalibration<5000) { - pc.printf("maximum linker triceps \r\n"); + pc.printf("maximum left triceps \r\n"); led = 1; emgNotchLT = NFLT.step(emgLT.read() ); emgHPLT = HPFLT.step(emgNotchLT); @@ -290,7 +275,7 @@ if(Timescalibration>5000 && Timescalibration<6000) { - pc.printf("maximum rechter triceps"); + pc.printf("maximum right triceps"); emgNotchRT = NFRT.step(emgRT.read() ); emgHPRT = HPFRT.step(emgNotchRT); emgAbsHPRT = abs(emgHPRT); @@ -304,10 +289,9 @@ if(Timescalibration>6000) { - pc.printf("calibratie afgelopen"); + pc.printf("calibration finished"); State = SelectDevice; } - } void RKI() @@ -327,7 +311,7 @@ int maxwaarde = 4096; // = 64x64 refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde; - refP2 = (( q1 + q2)/(2*pi))*maxwaarde; //Get reference positions was eerst 0.5*pi + refP2 = (( q1 + q2)/(2*pi))*maxwaarde; //Get reference positions } void SetpointRobot() @@ -336,39 +320,23 @@ double Potmeterwaarde1 = potMeter1.read(); if (Potmeterwaarde2>0.6) { - Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat + Rsx += 0.001; //increases 1 mm when potmetervalue above 0.6 } else if (Potmeterwaarde2<0.4) { - Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat + Rsx -= 0.001; //decreases 1 mm when potmetervalue below 0.4 } - else { //de x-waarde van de setpoint verandert niet + else { //x value of setpoint doesn't change } - if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat + if (Potmeterwaarde1>0.6) { //increases 1 mm when potmetervalue above 0.6 Rsy += 0.001; } - else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 + else if (Potmeterwaarde1<0.4) { //decreases 1 mm when potmetervalue below 0.4 Rsy -= 0.001; } - else { //de y-waarde van de setpoint verandert niet + else { //y value of setpoint doesn't change } } - -double GetReferencePosition() -{ - double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden - int maxwaarde = 4096; // = 64x64 - double refP = Potmeterwaarde*maxwaarde; - return refP; // value between 0 and 4096 -} - -double GetReferencePosition2() -{ - double potmeterwaarde2 = potMeter1.read(); - int maxwaarde2 = 4096; // = 64x64 - double refP2 = potmeterwaarde2*maxwaarde2; - return refP2; // value between 0 and 4096 -} 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) { @@ -415,10 +383,10 @@ M1D = 1; } if (fabs(motorValue) > 1) { - M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) + M1E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1) } else { - M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 + M1E = fabs(motorValue); //absolute velocity determined, motor is "off" at value of 0 } } @@ -431,26 +399,22 @@ M2D = 0; } if (fabs(motorValue2) > 1) { - M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) + M2E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1) } else { - M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 + M2E = fabs(motorValue2); //absolute velocity determined, motor is "off" at value of 0 } } void MeasureAndControl(void) { - //SetpointRobot(); - // RKI aanroepen RKI(); // control of 1st motor - //double refP = GetReferencePosition(); //KOMT UIT RKI 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 refP2 = GetReferencePosition2(); double Huidigepositie2 = motor2.getPosition(); double error2 = (refP2 - Huidigepositie2);// make an error double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); @@ -460,23 +424,22 @@ void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!! { - if (RBF>0.6) { + if (RBF>0.5) { Rsx +=0.001; // hoe veel verder gaat hij? 1 cm? 10 cm? } else {} - if (RTF>0.6) { + if (RTF>0.5) { Rsx -=0.001; } else {} - if (LBF>0.6) { + if (LBF>0.5) { Rsy +=0.001; } else {} - if (LTF>0.6) { + if (LTF>0.5) { Rsy -=0.001; } else {} - pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly); } void Loop_funtion() @@ -502,7 +465,9 @@ MeasureAndControl(); break; case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions. - case Demonstration: // COntrol with potmeters + case Demonstration: // Control with potmeters + SetpointRobot(); + MeasureAndControl(); break; } } @@ -537,7 +502,7 @@ //motor M1E.period(PwmPeriod); //set PWMposition at 5000hz //Ticker - //Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende + //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);