P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
Revision 18:b42a884bca02, committed 2017-11-03
- Comitter:
- Annelotte
- Date:
- Fri Nov 03 03:13:41 2017 +0000
- Parent:
- 17:1246d6b0c5d0
- Commit message:
- Met demo modus
; hoop ik
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 1246d6b0c5d0 -r b42a884bca02 main.cpp --- a/main.cpp Thu Nov 02 23:36:43 2017 +0000 +++ b/main.cpp Fri Nov 03 03:13:41 2017 +0000 @@ -38,6 +38,7 @@ double Tor2 = 0; double w1= 0; double w2= 0; +bool automode = false; Encoder motor1(D13,D12,true); MODSERIAL pc(USBTX,USBRX); @@ -83,27 +84,107 @@ } void SetpointRobot() -{ - double Potmeterwaarde2 = potMeter2.read(); - double Potmeterwaarde1 = potMeter1.read(); +{ + if automode == 0 + { + for (int n = 0; n < 100; n++) + { + Rsy -= 0.002; + RKI(); + // hier the control of the control system + float Huidigepositie = Encoder(); + float error = (refP - Huidigepositie);// make an error + float motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + float Huidigepositie2 = Encoder2(); + float error2 = (refP2 - Huidigepositie2);// make an error + float 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.002; + RKI(); + // hier the control of the control system + float Huidigepositie = Encoder(); + float error = (refP - Huidigepositie);// make an error + float motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + float Huidigepositie2 = Encoder2(); + float error2 = (refP2 - Huidigepositie2);// make an error + float 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.002; + RKI(); + // hier the control of the control system + float Huidigepositie = Encoder(); + float error = (refP - Huidigepositie);// make an error + float motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + float Huidigepositie2 = Encoder2(); + float error2 = (refP2 - Huidigepositie2);// make an error + float 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); + } - if (Potmeterwaarde2>0.6) { - Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat + for (int n = 0; n < 100; n++) + { + Rsx -= 0.002; + RKI() + // hier the control of the control system + float Huidigepositie = Encoder(); + float error = (refP - Huidigepositie);// make an error + float motorValue = FeedBackControl(error, e_prev, e_int); + SetMotor1(motorValue); + + float Huidigepositie2 = Encoder2(); + float error2 = (refP2 - Huidigepositie2);// make an error + float 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); + } + + automode = 1; } - 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 + + else automode = 1 + { + 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; + 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 + } } - 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 - } } float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)