Robot lijkt goed te reageren op potmeters van Pauline.
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of PIDZONDERRKI by
Diff: main.cpp
- Revision:
- 7:05495acc08b0
- Parent:
- 6:083bd713670b
- Child:
- 8:e125b9fa77b9
- Child:
- 9:42d1d02673ae
diff -r 083bd713670b -r 05495acc08b0 main.cpp --- a/main.cpp Tue Oct 31 15:44:56 2017 +0000 +++ b/main.cpp Wed Nov 01 14:59:42 2017 +0000 @@ -21,6 +21,17 @@ float e_prev = 0; float e_int = 0; +//tweede motor +AnalogIn potmeter1(A2); +PwmOut M2E(D5); +DigitalOut M2D(D4); +Encoder motor2(D9,D8,true); +Ticker DubbelTreecko; + +float PwmPeriod2 = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) +float e_prev2 = 0; +float e_int2 = 0; + float GetReferencePosition() @@ -30,6 +41,14 @@ float refP = Potmeterwaarde*maxwaarde; return refP; // value between 0 and 4096 } + +float GetReferencePosition2() +{ + float potmeterwaarde2 = potmeter1.read(); + int maxwaarde2 = 4096; // = 64x64 + float 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) { @@ -50,6 +69,26 @@ 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; + + float kd2 = 0.0004; // kind of scaled. + float VelocityError2 = (error2 - e_prev2)/Ts; + float Derivative2 = kd2*VelocityError2; + e_prev2 = error2; + + float ki2 = 0.00005; // kind of scaled. + e_int2 = e_int2+Ts*error2; + float Integrator2 = ki2*e_int2; + + + float motorValue2 = Proportional2 + Integrator2 + Derivative2; + return motorValue2; +} + + void SetMotor1(float motorValue) { if (motorValue >= 0) @@ -71,12 +110,39 @@ } } +void SetMotor2(float motorValue2) +{ + if (motorValue2 >= 0) + { + M2D = 0; + } + else + { + M2D = 1; + } + + if (fabs(motorValue2) > 1) + { + M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) + } + else + { + M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 + } +} + float Encoder () { float Huidigepositie = motor1.getPosition (); return Huidigepositie; // huidige positie = current position } +float Encoder2 () +{ + float Huidigepositie2 = motor2.getPosition (); + return Huidigepositie2; // huidige positie = current position +} + void MeasureAndControl(void) { // hier the control of the control system @@ -87,13 +153,23 @@ SetMotor1(motorValue); } +void MeasureAndControl2(void) +{ + // 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); + SetMotor2(motorValue2); +} + int main() { M1E.period(PwmPeriod); 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. - + DubbelTreecko.attach(MeasureAndControl2, Ts); while(1)