Robot lijkt goed te reageren op potmeters van Pauline.
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of PIDZONDERRKI by
Diff: main.cpp
- Revision:
- 10:2435c48d2032
- Parent:
- 9:42d1d02673ae
- Child:
- 11:d5369a546201
--- a/main.cpp Thu Nov 02 10:33:03 2017 +0000 +++ b/main.cpp Thu Nov 02 10:48:44 2017 +0000 @@ -34,58 +34,63 @@ float Potmeterwaarde; float potmeterwaarde2; +int maxwaarde, maxwaarde2; +float refP, refP2; +float kp, Proportional, kd, VelocidyError, Dervative, ki, Integrator, motorValue +float kp2, Proportional2, kd2, VelocityError2, Derivative2, ki2, Integrator2, motorValue2; +float Huidigepositie, error, float GetReferencePosition() { Potmeterwaarde = potMeter2.read(); - int maxwaarde = 4096; // = 64x64 - float refP = Potmeterwaarde*maxwaarde; + maxwaarde = 4096; // = 64x64 + refP = Potmeterwaarde*maxwaarde; return refP; // value between 0 and 4096 } float GetReferencePosition2() { potmeterwaarde2 = potmeter1.read(); - int maxwaarde2 = 4096; // = 64x64 - float refP2 = potmeterwaarde2*maxwaarde2; + maxwaarde2 = 4096; // = 64x64 + refP2 = potmeterwaarde2*maxwaarde2; return refP2; // value between 0 and 4096 } 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) { - float kp = 0.001; // kind of scaled. - float Proportional= kp*error; + kp = 0.001; // kind of scaled. + Proportional= kp*error; - float kd = 0.0004; // kind of scaled. - float VelocityError = (error - e_prev)/Ts; - float Derivative = kd*VelocityError; + kd = 0.0004; // kind of scaled. + VelocityError = (error - e_prev)/Ts; + Derivative = kd*VelocityError; e_prev = error; - float ki = 0.00005; // kind of scaled. + ki = 0.00005; // kind of scaled. e_int = e_int+Ts*error; - float Integrator = ki*e_int; + Integrator = ki*e_int; - float motorValue = Proportional + Integrator + Derivative; + motorValue = Proportional + Integrator + Derivative; return motorValue; } float FeedBackControl2(float error2, float &e_prev2, float &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { - float kp2 = 0.001; // kind of scaled. - float Proportional2= kp2*error2; + kp2 = 0.001; // kind of scaled. + Proportional2= kp2*error2; - float kd2 = 0.0004; // kind of scaled. - float VelocityError2 = (error2 - e_prev2)/Ts; - float Derivative2 = kd2*VelocityError2; + kd2 = 0.0004; // kind of scaled. + VelocityError2 = (error2 - e_prev2)/Ts; + Derivative2 = kd2*VelocityError2; e_prev2 = error2; - float ki2 = 0.00005; // kind of scaled. + ki2 = 0.00005; // kind of scaled. e_int2 = e_int2+Ts*error2; - float Integrator2 = ki2*e_int2; + Integrator2 = ki2*e_int2; - float motorValue2 = Proportional2 + Integrator2 + Derivative2; + motorValue2 = Proportional2 + Integrator2 + Derivative2; return motorValue2; } @@ -161,7 +166,7 @@ SetMotor2(motorValue2); pc.baud(115200); - pc.printf("potmeter = %f, refP = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2); + pc.printf("potmeter = %f, refP = %f, error = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, error2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, error, motorValue, potmeterwaarde2, refP2, error2, motorValue2); }