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:
- 22:02a9b5914cc7
- Parent:
- 21:9307dc9be4f7
- Child:
- 23:08255478f6cd
--- a/main.cpp Fri Nov 03 03:14:33 2017 +0000 +++ b/main.cpp Fri Nov 03 04:28:04 2017 +0000 @@ -6,7 +6,7 @@ #include "MODSERIAL.h" //State Machine en calibratie -enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo}; +enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo, STOP}; States State; int Counter; bool Position_controller_on; @@ -14,6 +14,7 @@ double value; double home1; DigitalIn button (D1); +bool automode=false; //Encoder/motor double Huidigepositie1; @@ -385,7 +386,7 @@ if(Timescalibration>6000) { pc.printf("calibratie afgelopen"); - State = SelectDevice; + State = Demo; } // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal); //} @@ -413,69 +414,6 @@ refP2 = (((-pi) + q1 - q2)/(2*pi))*maxwaarde; //Get reference positions was eerst 0.5*pi } -void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!! -{ - if (RBF>0.5) { - //goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm? - Rsx += 0.001; - } - else if (RTF>0.5) { - //goalx--; - Rsx -= 0.001; - } - else {} - if (LBF>0.5) { - //goaly++; - Rsy +=0.001; - } - else if (LTF>0.5) { - //goaly--; - Rsy -= 0.001; - } - else {} - pc.printf("goalx = %f, goaly = %f\r\n",Rsx, Rsy); -} - -void SetpointRobot() -{ - double Potmeterwaarde2 = potMeter2.read(); - 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 - } - else if (Potmeterwaarde2<0.4) { - Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat - } - else { //de x-waarde van de setpoint verandert niet - } - - if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat - Rsy += 0.001; - } - else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 - Rsy -= 0.001; - } - else { //de y-waarde van de setpoint verandert niet - } -} - -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) { double kp = 0.0015; // kind of scaled. @@ -549,6 +487,197 @@ } } +void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!! +{ + if (RBF>0.5) { + //goalx++; // hoe veel verder gaat hij? 1 cm? 10 cm? + Rsx += 0.001; + } + else if (RTF>0.5) { + //goalx--; + Rsx -= 0.001; + } + else {} + if (LBF>0.5) { + //goaly++; + Rsy +=0.001; + } + else if (LTF>0.5) { + //goaly--; + Rsy -= 0.001; + } + else {} + pc.printf("goalx = %f, goaly = %f\r\n",Rsx, Rsy); +} + +void DemoPotmeter () +{ + double Potmeterwaarde2 = potMeter2.read(); + 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 + } + else if (Potmeterwaarde2<0.4) { + Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat + } + else { //de x-waarde van de setpoint verandert niet + } + + if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat + Rsy += 0.001; + } + else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 + Rsy -= 0.001; + } + else { //de y-waarde van de setpoint verandert niet + } + RKI(); + // hier the control of the 1st control system + //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); + // hier the control of the 2nd control system + //double refP2 = GetReferencePosition2(); + 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 SetpointRobot() +{ + if (automode == 0) + { + Treecko.detach(); + for (int n = 0; n < 100; n++) + { + Rsy -= 0.0002; + RKI(); + + // hier the control of the control system + double Huidigepositie = motor1.getPosition(); + double error = (refP - Huidigepositie);// make an error + double motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + double Huidigepositie2 = motor2.getPosition(); + double error2 = (refP2 - Huidigepositie2);// make an error + double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); + SetMotor2(motorValue2); + //pc.printf("encoder 2 = %f\r\n",Huidigepositie2); + pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2); + } + + for (int n = 0; n < 100; n++) + { + Rsy += 0.0002; + RKI(); + // hier the control of the control system + double Huidigepositie = motor1.getPosition(); + double error = (refP - Huidigepositie);// make an error + double motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + double Huidigepositie2 = motor2.getPosition(); + double error2 = (refP2 - Huidigepositie2);// make an error + double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); + SetMotor2(motorValue2); + //pc.printf("encoder 2 = %f\r\n",Huidigepositie2); + pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2); + } + + for (int n = 0; n < 100; n++) + { + Rsx += 0.0002; + RKI(); + // hier the control of the control system + double Huidigepositie = motor1.getPosition(); + double error = (refP - Huidigepositie);// make an error + double motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + double Huidigepositie2 = motor2.getPosition(); + double error2 = (refP2 - Huidigepositie2);// make an error + double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); + SetMotor2(motorValue2); + //pc.printf("encoder 2 = %f\r\n",Huidigepositie2); + pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2); + } + + for (int n = 0; n < 100; n++) + { + Rsx -= 0.0002; + RKI(); + // hier the control of the control system + double Huidigepositie = motor1.getPosition(); + double error = (refP - Huidigepositie);// make an error + double motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + double Huidigepositie2 = motor2.getPosition(); + double error2 = (refP2 - Huidigepositie2);// make an error + double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); + SetMotor2(motorValue2); + //pc.printf("Motor2.getposition() 2 = %f\r\n",Huidigepositie2); + pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2); + } + + automode = 1; + } + + else + { + Treecko.attach(DemoPotmeter,Ts); + } + +} + +/*void SetpointRobot() +{ + double Potmeterwaarde2 = potMeter2.read(); + 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 + } + else if (Potmeterwaarde2<0.4) { + Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat + } + else { //de x-waarde van de setpoint verandert niet + } + + if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat + Rsy += 0.001; + } + else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 + Rsy -= 0.001; + } + else { //de y-waarde van de setpoint verandert niet + } +} */ + +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 +} + + + void MeasureAndControl(void) { //SetpointRobot(); @@ -628,6 +757,8 @@ 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 + wait(2.0); + pc.printf("besturingsmethode kiezen \r\n"); if (button==1) { State=EMG; } @@ -643,6 +774,13 @@ MeasureAndControl(); break; case Demo: // Aansturen met toetsenbord + pc.printf("Automatische DEMO \r\n"); + SetpointRobot(); + State=STOP; + break; + case STOP: // Demo runt zichtzelf + SetMotor1(0); + SetMotor2(0); break; } }