Stap 11: Het PD-algoritme
We hebben dus nu het hart van het project bereikt. Bijna elke goed als volgeling robot moet dit algoritme om ervoor te zorgen dat we de lijn optimaal zal volgen. De robot heeft twee of vier motoren. Die zeggen dat wij de richting dat de robot volgen meer macht te verlenen aan een motor of de andere zal bepalen. We kunnen controle van de snelheid van de motoren door het gebruik van de pols-breedte-modulatie, die is geïntegreerd in de code van de robot, en laat ons een functie, MotorsSpeed(a,b) om de snelheid van de motor A van-1000 tot 1000 en de snelheid van de motor B -1000 tot 1000 wilt gebruiken. We citeer het, maar het houdt geen steek hier uitleggen hoe het werkt. (Maar ik beloof dat het doet!)
void MotorsSpeed(int A,int B){ ///If mode equals alfa then we invert the motors voltage MotorASpeed(MODE == ALFA ? A : B); MotorBSpeed(MODE == BETA ? A : B); } void MotorASpeed(int S){ S = min(S,1000); S = max(S,-1000); ADIR = S > 0 ? 0 : 1; S = S > 0 ? S : 1000 + S; CCP1CONbits.DC1B1 = S % 4; CCPR1L = S / 4; } void MotorBSpeed(int S){ S = min(S,1000); S = max(S,-1000); BDIR = S > 0 ? 0 : 1; S = S > 0 ? S : 1000 + S; CCP2CONbits.DC2B = S % 4; CCPR2L = S / 4; } Dus nu we de PD-algoritme waarvan de doelstelling zijn zal om te beslissen hoeveel reageren zal uitvoeren (hoe veel meer macht gegeven aan één motor dan de andere) wanneer de lijn is ver van het centrum van de sensor-array op basis van twee voorwaarden:
- Evenredig: Deze term is in verhouding tot hoe ver de sensor array centrum vanuit het midden van de lijn is
- Derivaat: De waarde van deze term is volgens hoeveel de sensor-array centrum is verhuisd van de laatste keer (de snelheid van verandering). Het toevoegen van deze term formule zal het maken van de robot om te "voorspellen" waren de lijn zal worden in de volgende iteratie welke oorzaken (als we het correct gebruiken) een enorme daling van de oscillaties.
Er zijn twee constanten, die zijn KP en KD dat zegt hoeveel elke term van de robot gedrag beïnvloedt. Testen om deze naar de beste waarden is onderdeel van de ontwikkeling van de robot. Het resultaat van de formule zullen evenredig aan hoeveel macht wordt gegeven aan een motor dan de andere. Als het resultaat is positief, waarschijnlijk motor A zal hebben meer macht dan B, in het andere geval, motor B meer macht dan A.
Ik heb toegevoegd in de robot een derde termijn KR, dat dat maakt als de motorsnelheid negatief is (wat betekent dat het gaat in omgekeerde manier), dan is de snelheid wordt vermenigvuldigd met een factor. Dit is niet nodig, maar vanwege de kenmerken van mijn motors ik vergde toe te voegen.
Neem nu een kijkje van de PID-algoritme (dat heeft de KR-ding, maar je kunt negeren)
void LineFollow(){ double kp,kd,kr,speed; ///POSICION has the value of the center of the line previously calculated kp = KP[speedMode]; kd = KD[speedMode]; kr = KR[speedMode]; DER = POSICION - LP; ///We calculate how much the line has moved from the last iteration PIDf = (POSICION* kp + DER * kd); if (PIDf > 0){ MotorsSpeed(Mr(speed-PIDf,kr) , speed); ///Mr makes the value to multiply by KR if it is negative. You can delete it }else{ MotorsSpeed(speed , Mr(speed+PIDf,kr) ); ///Mr makes the value to multiply by KR if it is negative. You can delete it } LP = POSICION; ///We store the last line position }
