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

Bijeg od linije

Cilj

Vratiti se natrag u igralište nakon što se naiđe na liniju.

Tri stanja

...
enum TriState{Yes, Opposite, Unknown};
...
U logički tip podatka (bool) je moguće spremiti 2 vrijednosti, što za mnoge primjene nije dovoljno, nego je pogodno imati 3: jedno stanje, suprotno stanje i nepoznato.

Zato smo uveli enumeraciju "TriState", u "mrm-robot-soccer.h", koja ima traženi broj stanja i još smo ih nazvali kako je pogodno za naš primjer.

Inicijalizacija

void RobotSoccer::lineAvoid(){
	static TriState lineLeft;
	static TriState lineFront;
	static float escapeDirection;
	const uint16_t WALL_DISTANCE = 550;

	if (setup()){
		print("Line avoid enter\n\r");
		lineLeft = TriState::Unknown;
		lineFront = TriState::Unknown;
	}

	...
}
Nakon što bilo koji senzor uoči liniju, počne se izvršavati funkcija "lineAvoid()".

Na početku su deklaracije 3 varijable: ima li linije lijevo, naprijed i smjer u kojem će robot bježati od linije.

Na kraju je konstanta koja koja daje najveću udaljenost od zida u trenutku kad je robot došao na liniju.

"lineLeft" i "lineFront" se postave u nepoznato stanje.

Detekcija linije

void RobotSoccer::lineAvoid(){
	static TriState lineLeft;
	static TriState lineFront;
	static float escapeDirection;
	const uint16_t WALL_DISTANCE = 550;

	if (setup()){
		print("Line avoid enter\n\r");
		lineLeft = TriState::Unknown;
		lineFront = TriState::Unknown;
	}

	// Line front?
	if (mrm_ref_can->any(false, 0) && lineFront == TriState::Unknown && front() < WALL_DISTANCE)
			lineFront = TriState::Yes, print("Front");

	// Line right?
	if (mrm_ref_can->any(false, 1) && lineLeft == TriState::Unknown && right() < WALL_DISTANCE)
			lineLeft = TriState::Opposite, print("Right");

	// Line back?
	if (mrm_ref_can->any(false, 2) && lineFront == TriState::Unknown && back() < WALL_DISTANCE)
			lineFront = TriState::Opposite, print("Back");

	// Line left?
	if (mrm_ref_can->any(false, 3) && lineLeft == TriState::Unknown && left() < WALL_DISTANCE)
			lineLeft = TriState::Yes, print("Left");

	...
}
Za svaki se od 4 senzora linije se provjeri vidi li liniju i je li udaljenost od zida dovoljno mala.

Ako je zid predaleko, to je lažna linija (krivo očitanje) i rezultat se odbacuje.

Iako ne postoji varijabla "lineRight", nego samo "lineLeft", desnu liniju upisujemo tako da u "lineLeft" stavimo vrijednost "Opposite".

Inicijalizacija

void RobotSoccer::lineAvoid(){
	static TriState lineLeft;
	static TriState lineFront;
	static float escapeDirection;
	const uint16_t WALL_DISTANCE = 550;

	if (setup()){
		print("Line avoid enter\n\r");
		lineLeft = TriState::Unknown;
		lineFront = TriState::Unknown;
	}

	// Line front?
	if (mrm_ref_can->any(false, 0) && lineFront == TriState::Unknown && front() < WALL_DISTANCE)
			lineFront = TriState::Yes, print("Front");

	// Line right?
	if (mrm_ref_can->any(false, 1) && lineLeft == TriState::Unknown && right() < WALL_DISTANCE)
			lineLeft = TriState::Opposite, print("Right");

	// Line back?
	if (mrm_ref_can->any(false, 2) && lineFront == TriState::Unknown && back() < WALL_DISTANCE)
			lineFront = TriState::Opposite, print("Back");

	// Line left?
	if (mrm_ref_can->any(false, 3) && lineLeft == TriState::Unknown && left() < WALL_DISTANCE)
			lineLeft = TriState::Yes, print("Left");

	if (lineLeft != TriState::Unknown || lineFront != TriState::Unknown){ // A line, not a false alarm
		int8_t x = 0;
		int8_t y = 0;
		if (lineFront == TriState::Yes)
			y--;
		else if (lineFront == TriState::Opposite)
			y++;
		if (lineLeft == TriState::Yes)
			x++;
		else if (lineLeft == TriState::Opposite)
			x--;
		escapeDirection = atan2(x, y) / PI * 180; 
		...
	}
	...
}
Smjer izbjegavanja računamo tako da, u koordinatnom sustavu, nađemo točku prema kojoj bismo trebali ići.

Izračunamo arcus tangens i rezultat spremimo u "escapeDirection".

Bijeg

void RobotSoccer::lineAvoid(){
	static TriState lineLeft;
	static TriState lineFront;
	static float escapeDirection;
	const uint16_t WALL_DISTANCE = 550;

	if (setup()){
		print("Line avoid enter\n\r");
		lineLeft = TriState::Unknown;
		lineFront = TriState::Unknown;
	}

	// Line front?
	if (mrm_ref_can->any(false, 0) && lineFront == TriState::Unknown && front() < WALL_DISTANCE)
			lineFront = TriState::Yes, print("Front");

	// Line right?
	if (mrm_ref_can->any(false, 1) && lineLeft == TriState::Unknown && right() < WALL_DISTANCE)
			lineLeft = TriState::Opposite, print("Right");

	// Line back?
	if (mrm_ref_can->any(false, 2) && lineFront == TriState::Unknown && back() < WALL_DISTANCE)
			lineFront = TriState::Opposite, print("Back");

	// Line left?
	if (mrm_ref_can->any(false, 3) && lineLeft == TriState::Unknown && left() < WALL_DISTANCE)
			lineLeft = TriState::Yes, print("Left");

	if (lineLeft != TriState::Unknown || lineFront != TriState::Unknown){ // A line, not a false alarm
		int8_t x = 0;
		int8_t y = 0;
		if (lineFront == TriState::Yes)
			y--;
		else if (lineFront == TriState::Opposite)
			y++;
		if (lineLeft == TriState::Yes)
			x++;
		else if (lineLeft == TriState::Opposite)
			x--;
		escapeDirection = atan2(x, y) / PI * 180;
		print("Line avoid, F:%i R:%i B:%i L:%i Esc dir: %i L:%i F:%i\n\r", mrm_ref_can->any(false, 0), mrm_ref_can->any(false, 1),
			mrm_ref_can->any(false, 2), mrm_ref_can->any(false, 3), (int)escapeDirection, lineLeft, lineFront);

		go(70, escapeDirection, pidRotation->calculate(heading() - headingToMaintain), 100);
	}
	else
		actionSet(actionIdle), print("False line");
}
Ispišemo na ekran što se dogodilo ("print()"), pokrenemo motore ("go()".

Ako je detektirana linija senzorom površine, ali je udaljenost bila prevelika, to ispišemo.

Zadatak: stani.

Popravite program u smislu da robot stane, u slučaju da je u smjeru bijega prepreka.

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.