Alles in 1
Dependencies: Encoder HIDScope MODSERIAL QEI mbed
Fork of RoboBird3 by
Diff: main.cpp
- Revision:
- 18:0f7f57228901
- Parent:
- 17:c5eea26e171d
- Child:
- 19:b8d959e02e5d
diff -r c5eea26e171d -r 0f7f57228901 main.cpp --- a/main.cpp Wed Oct 07 18:25:02 2015 +0000 +++ b/main.cpp Wed Oct 07 20:06:15 2015 +0000 @@ -7,7 +7,7 @@ PwmOut PowerMotor(D5); //van 0 tot 1 PwmOut PowerServo(D7); DigitalIn Button(PTC6); -//DigitalIn Button2(D2); +DigitalIn Button2(PTC4); AnalogIn PotMeter(A1); QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder Serial pc(USBTX, USBRX); @@ -24,19 +24,24 @@ double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan double Error = 0; -double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides +double Errord = 0; +double Errori = 0; +double Errorp = 0; +const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides +const double Kd = 10; +const double Ki = 0.1; double v = 0; //snelheid van de motor (0-0.1 double upperlimit; //max aantal rotaties bool Excecute = false; bool Excecute2 = false; -double Fs=100; +const double Fs=100; double n = 3; void MotorSet() { - v=Kp*Error; - if (v>=0) { + v=Kp*Error + Kd*Errord + Ki*Errori; + if (Error>=0) { Direction=1; } else { Direction=0; @@ -69,8 +74,15 @@ pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal + Errord = (Error-Errorp)/Fs; + Errorp = Error; + if (fabs(Error) <= 0.5) { + Errori = Errori + Error*1/Fs; + } else { + Errori = 0; + } pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - if (fabs(Error)<=0.15) { + if (fabs(Error)<=0.0015) { timer.start(); } else { timer.stop(); @@ -80,6 +92,8 @@ Excecute=false; Excecute2 = true; Error = 0; + Errori = 0; + Errord = 0; } } while (Excecute2) { @@ -90,7 +104,7 @@ Goal = 0; // Het doel waar hij naar toe moet Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - if (fabs(Error)<=0.15) { + if (fabs(Error)<=0.0015) { timer.start(); } else { timer.stop(); @@ -99,6 +113,8 @@ if (timer.read() >= 5) { Excecute2=false; Error = 0; + Errori = 0; + Errord = 0; } } }