P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 8:42d1d02673ae
- Parent:
- 7:05495acc08b0
- Child:
- 9:2435c48d2032
--- a/main.cpp Wed Nov 01 14:59:42 2017 +0000 +++ b/main.cpp Thu Nov 02 10:33:03 2017 +0000 @@ -12,8 +12,8 @@ AnalogIn potMeter2(A1); //Analoge input of potmeter 2 (will be use for te reference position) PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control +Encoder motor1(D13,D12,true); -Encoder motor1(D13,D12,true); MODSERIAL pc(USBTX,USBRX); float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) @@ -32,11 +32,12 @@ float e_prev2 = 0; float e_int2 = 0; - +float Potmeterwaarde; +float potmeterwaarde2; float GetReferencePosition() { - float Potmeterwaarde = potMeter2.read(); + Potmeterwaarde = potMeter2.read(); int maxwaarde = 4096; // = 64x64 float refP = Potmeterwaarde*maxwaarde; return refP; // value between 0 and 4096 @@ -44,7 +45,7 @@ float GetReferencePosition2() { - float potmeterwaarde2 = potmeter1.read(); + potmeterwaarde2 = potmeter1.read(); int maxwaarde2 = 4096; // = 64x64 float refP2 = potmeterwaarde2*maxwaarde2; return refP2; // value between 0 and 4096 @@ -151,31 +152,33 @@ float error = (refP - Huidigepositie);// make an error float motorValue = FeedBackControl(error, e_prev, e_int); 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); + + 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); } int main() { M1E.period(PwmPeriod); + M2E.period(PwmPeriod2); 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); + //DubbelTreecko.attach(MeasureAndControl2, Ts); while(1) { wait(0.2); - pc.baud(115200); + //pc.printf("Potmeter2 = %f, refP = %f, motorValue = %f, \r\nPotmeter1 = %f, refP2 = %f, motorValue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2); + //pc.baud(115200); float B = motor1.getPosition(); float Potmeterwaarde = potMeter2.read(); //float positie = B%4096;