Högpresterande rad efterföljare Robot (11 / 12 steg)
Steg 11: PD algoritmen
Så nu har vi nått hjärtat av projektet. Nästan varje god lik efterföljare robot måste denna algoritm för att se till att vi ska följa linjen optimalt. Roboten har två eller fyra motorer. Som säger att vi ska styra riktningen att roboten följer att ge mer makt till en motor eller annat. Vi kan kontrollera hastigheten på motorerna av användningen av puls-bredd-modulering, som är integrerad i robot-koden, och tillåter oss att använda en funktion, MotorsSpeed(a,b) att ställa in hastigheten på motorn A från-1000 till 1000 och hastigheten på motorn B från -1000 till 1000. Vi citerar det men det gör inte meningsfullt att förklara här hur det fungerar. (Men jag lovar att det gör!)
void MotorsSpeed (int A, int B) {
Om läget är lika med alfa sedan Invertera vi motors spänningen
MotorASpeed (läge == ALFA? A: B);
MotorBSpeed (läge == BETA? A: B);
}
void MotorASpeed(int S) {
S = min(S,1000);
S = max(S,-1000);
LINDAS = 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;
}
Så nu ska vi genomföra PD algoritmen vars mål är att bestämma hur mycket react (hur mycket mer makt given på en motor än den andra) när raden är långt ifrån mitten av sensorn matrisen enligt två villkor:
- Proportionella: Denna term är proportionell mot hur långt är stadens sensor array från raden center
- Derivat: Värdet av denna term är enligt hur mycket sensor matrisen center har flyttat från förra gången (ändra hastigheten). Att lägga till denna term till formeln kommer att göra roboten att "förutsäga" var linjen kommer att i nästa iteration som orsakar (om vi använder den korrekt) en enorm minskning av svängningar.
Det finns två konstanter, som KP och KD som säger hur mycket varje termin påverkar robotens beteende. Tester för att få dessa att de bästa värdena är en del av utvecklingen av roboten. Resultatet av formeln kommer att stå i proportion till hur mycket makt ges till en motor än den andra. Om resultatet är positivt, förmodligen motor ett kommer att ha mer makt än B, i det andra fallet, motor B kommer att ha mer makt än A.
Jag har lagt på robot tredje sikt KR, som gör att om varvtal är negativ (vilket betyder att det går i omvänt sätt), då hastigheten är multiplicerad med faktorn. Detta är inte nödvändigt men på grund av egenskaperna hos min motorer hade jag behövde för att lägga till den.
Nu ta en titt på PID algoritmen (som har KR sak, men du kan ignorera det)
void LineFollow() {
dubbel kp, kd, kr, hastighet;
POSICION har värdet av centrera av linjen tidigare beräknats
KP = KP [speedMode];
KD = KD [speedMode];
kr = KR [speedMode];
DER = POSICION - LP; Vi beräknar hur mycket linjen har flyttat från den senaste iterationen
PIDf = (POSICION * kp + DER * kd);
om (PIDf > 0) {
MotorsSpeed(Mr(speed-PIDf,kr), hastighet); Herr gör värdet att multiplicera med KR om det är negativt. Du kan ta bort den
} annat {
MotorsSpeed (hastighet, Mr(speed+PIDf,kr)); Herr gör värdet att multiplicera med KR om det är negativt. Du kan ta bort den
}
LP = POSICION; Vi lagrar den sista rad positionen
}