int8_t speedX;
int8_t speedY;
/** Custom test.
*/
void RobotSoccer::loop() {
print("Start\n\r");
headingToMaintain = mrm_imu->heading();
speedX = 0;
speedY = 0;
end();
}
/** Generic actions, use them as templates
*/
void RobotSoccer::loop0() { speedY += 5, actionSet(actionLoop4);} // Forward
void RobotSoccer::loop1() { speedY -= 5, actionSet(actionLoop4);} // Backward
void RobotSoccer::loop2() { speedX -= 5, actionSet(actionLoop4);} // Left
void RobotSoccer::loop3() { speedX += 5, actionSet(actionLoop4);} // Right
void RobotSoccer::loop4() {
const int MAX_SPEED = 30;
//Limit speed
if (speedX < -MAX_SPEED)
speedX = -MAX_SPEED;
if (speedX > MAX_SPEED)
speedX = MAX_SPEED;
if (speedY < -MAX_SPEED)
speedY = -MAX_SPEED;
if (speedY > MAX_SPEED)
speedY = MAX_SPEED;
float direction = atan2(speedX, speedY) / PI * 180; // Arcus tangens to get angle.
float speed = sqrt(speedX * speedX + speedY * speedY); // Variable speed.
//print("%i°, x: %i, y: %i, speed: %i\n\r", (int)direction, speedX, speedY, (int)speed);
go(speed, direction, pidRotation->calculate(heading() - headingToMaintain), 100);
}
Na početku je blok od 2 naredbe:
int8_t speedX;
int8_t speedY;
Radi se o "globalnim varijablama", koje su vidljive u cijelom programu. Koristimo ih zato što želimo upotrijebiti standarnde "loopx" funkcije, koje nemaju argumente, a moramo koristiti zajedničke podatke.
Funkcija "loop" pamti početnu orijentaciju robota, koju će robot automatski zadržati. Početne brzine u obje osi su 0.
"loop0()" do "loop3()" su funkcije kojima mijenjamo brzine u oba smjera.
Svaka od ovih funkcija poziva "loop4()", koja obavlja zajednički dio posla.
Ograničava se brzina jer bi se inače robot mogao kretati nekontrolirano brzo.
Računa se "direction" kao hipotenuza pravokutnog trokuta, čije su katete dobivene dvjema promjenjivim brzinama. Kut trokuta dobijemo funkcijom arkus tangens.
Brzina ("speed") se dobije kao duljina hipotenuze trokuta.
Na kraju se funkcijom "go" pokreće robot.