P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 10:d5369a546201
- Parent:
- 9:2435c48d2032
- Child:
- 11:126ae33919b3
--- a/main.cpp Thu Nov 02 10:48:44 2017 +0000 +++ b/main.cpp Thu Nov 02 11:22:08 2017 +0000 @@ -36,9 +36,13 @@ float potmeterwaarde2; int maxwaarde, maxwaarde2; float refP, refP2; -float kp, Proportional, kd, VelocidyError, Dervative, ki, Integrator, motorValue +float kp, Proportional, kd, VelocityError, Derivative, ki, Integrator, motorValue; float kp2, Proportional2, kd2, VelocityError2, Derivative2, ki2, Integrator2, motorValue2; -float Huidigepositie, error, +float Huidigepositie; +float Aerror; +float Huidigepositie2; +float Aerror2; +float motorVal, motorVal2; float GetReferencePosition() { @@ -66,13 +70,13 @@ Derivative = kd*VelocityError; e_prev = error; - ki = 0.00005; // kind of scaled. + ki = 0.0005; // kind of scaled. e_int = e_int+Ts*error; Integrator = ki*e_int; - motorValue = Proportional + Integrator + Derivative; - return motorValue; + motorVal = Proportional + Integrator + Derivative; + return motorVal; } 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) @@ -85,13 +89,13 @@ Derivative2 = kd2*VelocityError2; e_prev2 = error2; - ki2 = 0.00005; // kind of scaled. + ki2 = 0.0005; // kind of scaled. e_int2 = e_int2+Ts*error2; Integrator2 = ki2*e_int2; - motorValue2 = Proportional2 + Integrator2 + Derivative2; - return motorValue2; + motorVal2 = Proportional2 + Integrator2 + Derivative2; + return motorVal2; } @@ -120,11 +124,11 @@ { if (motorValue2 >= 0) { - M2D = 0; + M2D = 1; } else { - M2D = 1; + M2D = 0; } if (fabs(motorValue2) > 1) @@ -139,34 +143,34 @@ float Encoder () { - float Huidigepositie = motor1.getPosition (); + Huidigepositie = motor1.getPosition (); return Huidigepositie; // huidige positie = current position } float Encoder2 () { - float Huidigepositie2 = motor2.getPosition (); + Huidigepositie2 = motor2.getPosition (); return Huidigepositie2; // huidige positie = current position } void MeasureAndControl(void) { // hier the control of the control system - float refP = GetReferencePosition(); - float Huidigepositie = Encoder(); - float error = (refP - Huidigepositie);// make an error - float motorValue = FeedBackControl(error, e_prev, e_int); + refP = GetReferencePosition(); + Huidigepositie = Encoder(); + Aerror = (refP - Huidigepositie);// make an error + motorValue = FeedBackControl(Aerror, e_prev, e_int); SetMotor1(motorValue); // hier the control of the control system - float refP2 = GetReferencePosition2(); - float Huidigepositie2 = Encoder2(); - float error2 = (refP2 - Huidigepositie2);// make an error - float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); + refP2 = GetReferencePosition2(); + Huidigepositie2 = Encoder2(); + Aerror2 = (refP2 - Huidigepositie2);// make an error + motorValue2 = FeedBackControl2(Aerror2, e_prev2, e_int2); SetMotor2(motorValue2); pc.baud(115200); - 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); + pc.printf("potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, Huidigepositie, Aerror, motorValue, potmeterwaarde2, refP2, Huidigepositie2, Aerror2, motorValue2); }