Hrvatsko društvo za robotiku - Uvod u robotiku - vježbe

Praćenje zida labirinta

Cilj

Programirati robota za praćenje bilo kojeg od 4 zida polja.

Displej

void RobotMaze::wallFollow(Direction wallDirection) {
	// Use mrm-8x8a to display the wall to follow.
	if (wallDirection == Direction::UP)
		mrm_8x8a->bitmapCustomStoredDisplay(LedSign::MAZE_WALL_UP_FOLLOW);
	else if (wallDirection == Direction::DOWN)
		mrm_8x8a->bitmapCustomStoredDisplay(LedSign::MAZE_WALL_DOWN_FOLLOW);
	else if (wallDirection == Direction::LEFT)
		mrm_8x8a->bitmapCustomStoredDisplay(LedSign::MAZE_WALL_LEFT_FOLLOW);
	else
		mrm_8x8a->bitmapCustomStoredDisplay(LedSign::MAZE_WALL_RIGHT_FOLLOW);

	...
}
Kako bismo olakšali testiranje programa, ispisivat ćemo na 8x8 displej koji se zid prati.

"wallDirection" je smjer u kojem je zid koji pratimo i zadan je izvana.

4 "if"a prikazuju odgovarajuću bitmapu na displeju.

Poravnavanje 

void RobotMaze::wallFollow(Direction wallDirection) {
	...

	// Correct alignment to the wall (rotation around vertical axis).
	int16_t differenceCWMinusCCW = distance(wallDirection, false) - distance(wallDirection, 
		true);
	// If differenceCWMinusCCW positive, the robot is rotated too much CW and positive 
		speedR will decrease the error.
	int16_t speedR = differenceCWMinusCCW * WALL_FOLLOW_ROTATION_STRENGTH; 

	...
}
Poravnavajmo robota stalno, tako da bude paralelan sa zidom koji prati.

"differenceCWMinusCCW" je greška, razlika između udaljenosti, na 2 kraja robota, od zida.

"speedR" će biti brzina desnih kotača, proporcionalna grešci.

Greška udaljenosti

void RobotMaze::wallFollow(Direction wallDirection) {
	..

	// Correct distance to the wall. The robot should maintain WALL_FOLLOW_DISTANCE 
		distance.
	int16_t closerToWallThanOptimum = WALL_FOLLOW_DISTANCE - (distance(wallDirection, false) 
		+ distance(wallDirection, true)) / 2; 
	int16_t distanceAdjustmentCW;
	// Correction of alignment error was easier as it did not depend on direction, but 
		this one does, so we have to consider different directions.
	if (directionCurrent == wallDirection || (directionCurrent + 2) % 4 == wallDirection) 
		distanceAdjustmentCW = 0;
	else if ((directionCurrent + 1) % 4 == wallDirection)
		distanceAdjustmentCW = closerToWallThanOptimum * 
			WALL_FOLLOW_DISTANCE_ADJUSTMENT_STRENGTH;
	else // Follow wall to the right
		distanceAdjustmentCW = -closerToWallThanOptimum * 
			WALL_FOLLOW_DISTANCE_ADJUSTMENT_STRENGTH;
	speedR -= distanceAdjustmentCW;

	...
}
"closerToWallThanOptimum" će biti greška u udaljenosti od zida.

Moramo uzeti u obzir razna mogućnosti pozicije zida i na taj način interpretirati grešku u udaljenosti.

Ovo je učinjeno u nekoliko "if"ova, na sažet način, koji nije lako shvatljiv.

Ne razbijajte glavu, bitno je da se greška na kraju oduzme od postojeće brzine desnih kotača.

Na kraju smo u brzini desnih kotača dobili zbroj korekcija 2 greške: u udaljenosti i odstupanju od paralelne (ili okomite) vožnje. Uočite da ta brzina korigira obadvije greške; ako robot ide brže desnim kotačima, ići će i više prema zidu i ispravljati grešku u rotaciji.

Shvatiti ideju je bitnije, nego shvatiti kako rade detalji implementacije. Detalji se zaborave.

Motori

void RobotMaze::wallFollow(Direction wallDirection) {
	...

	int16_t toTheMax = TOP_SPEED - abs(speedR); 
	int16_t speedL = -speedR + toTheMax;
	speedR += toTheMax;

	motorGroup->go(speedL, speedR); // Engage motors.

	...
}
Izračunamo prvo koliko je od brzine desnih motora do maksimalne brzine ("toTheMax").

Pomoću toga odredimo brzinu lijevih motora.

U principu je brzina lijevih motora suprotna od brzine desnih, ali nakon toga i na brzinu lijevih i desnih dodajemo koliko možemo, da nijedna brzina ne ide preko maksimalne.

Pokrećemo motore.

IMU

void RobotMaze::wallFollow(Direction wallDirection) {
	...

	// Correct stored direction. It is constantly adjusted when following a wall. 
		It is mandatory because magnetic field can change much, even in one tile.
	imuLastValid = mrm_imu->heading(); 
}
Na kraju spremimo posljednju poziciju kompasa (ovu sad) kad je robot pratio zid.

Nije potrebno za praćenje zida, ali je jako pogodno kad nestane zid za praćenje pa idemo dalje po kompasu.

Zadatak: parametri

Mijenajte "WALL_FOLLOW_DISTANCE_ADJUSTMENT_STRENGTH" i "WALL_FOLLOW_ROTATION_STRENGTH" kako biste dobili što bolje praćenje zida.

Primjedbe



Projekt "Uvod u robotiku" sufinanciran je iz Europskog socijalnog fonda, poziv "Jačanje kapaciteta organizacija civilnoga društva za popularizaciju STEM-a". Relevantne stranice: Sadržaj vježbe za virtualne radionice isključiva je odgovornost Hrvatskog društva za robotiku.