void RobotSoccer::loop() {
if (setup())
headingToMaintain = heading();
const int SPEED = 40;
float errorL = 900 - left();
float errorR = right() - 900;
float errorX = left() > right() ? errorL : errorR;
float errorY = 200 - back();
float speed = pidXY->calculate(fabsf(errorX) +
fabsf(errorY));
float direction = atan2(errorX, errorY) / PI * 180;
go(speed > SPEED ? SPEED : speed, direction,
pidRotation->calculate(heading() -
headingToMaintain));
}
Kao i u svim primjerima dosad, prve dvije linije spremaju smjer robota.
"SPEED = 40" definira brzinu kretanja robota. Možete povećati ovaj broj.
"errorL" i "errorR" daju greške od željenje pozicije (900 mm od jednog ili drugog zida). "left()" i "right()" su funkcije koje daju rezultate mjeranje udaljenosti lijevo i desno, koristeći lidare.
"errorX" daje grešku u smjeru u kojem je zid dalje.
"errorY" je greška u poziciji naprijed-natrag. Želimo biti 200 mm ispred gola. Koristimo funkciju "back()", koja mjeri udaljenost iza robota.
"speed" je rezultat djelovanja PID kontrolera na ukupnu grešku i određivat će brzinu kretanja robota.
Slijedi računanje željenog smjera. Za razumijevanje je pogodno poznavati zbrajanje vektora pa nećemo ulaziti u detalje. Rezultat je "direction", varijabla koja daje smjer u kojem se treba kretati robot kako bi poništio greške u x osi (lijevo-desno) i y (naprijed-natrag).
Na kraju funkcija "go()" pokreće robota. Njene smo argumente već proučili u prethodnim vježbama.
Kratki video pokazuje robota koji izvršava gornji program.