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

Odbijanje od zidova

Cilj

Programirati kretanje nogometaša tako da se odbija od zidova.

Ideja i inicijalizacija

void RobotSoccer::bounce(){
	static int directionCurrent = 45;
	const int VARIATION = 45;
	const int SPEED = 127;
	const bool AVOID_LINE = false;
	if (setup()){
		headingToMaintain = heading();
	}

	...
}
Želimo da se robot kreće unutar arene, nogometnog igrališta, na način da se odbija od zidova. Izlazni kut treba biti okomit na prepreku, sa željenom varijacijom.

Koristit ćemo funkciju "bounce()" u datoteci mrm-robot-soccer.cpp.

Varijabla "directionCurrent" će biti trenutni smjer kretanja robota.

"VARIATION" daje varijaciju. Ako je 0, tad će izlazni kut biti točno okomit na zid.

"SPEED" je brzina gibanja. U našem slučaju, maksimalna.

"AVOID_LINE" - hoćemo li izbjegavati i linije ili ne.

U prvom prolazu funkcije postavimo "headingToMaintain" (kut u odnosu na sjever) na trenutni kut.

Odbijanje od linije

void RobotSoccer::bounce(){
	static int directionCurrent = 45;
	const int VARIATION = 45;
	const int SPEED = 127;
	const bool AVOID_LINE = false;
	if (setup()){
		headingToMaintain = heading();
	}

	if (AVOID_LINE && lineAny()){
		actionSet(actionLineAvoid);
		if (mrm_ref_can->any(false, 0)) // Front
			directionCurrent = headingRandom(-180, VARIATION);
		if (mrm_ref_can->any(false, 1)) // Right
			directionCurrent = headingRandom(-90, VARIATION);
		if (mrm_ref_can->any(false, 2)) // Back
			directionCurrent = headingRandom(0, VARIATION);
		if (mrm_ref_can->any(false, 3)) // Left
			directionCurrent = headingRandom(90, VARIATION);
	}
	else{
		...
	}
}
U slučaju da želimo izbjegavati liniju, robot će se odbijati okomito od nje.

Ako je "lineAny()" istinit, akciju ćemo promijeniti na "actionLineAvoid()".

Ovu granu nećemo proučavati u našem primjeru. Robot nikad neće ući u nju, ako je "AVOID_LINE" "false".

Brzina

void RobotSoccer::bounce(){
	static int directionCurrent = 45;
	const int VARIATION = 45;
	const int SPEED = 127;
	const bool AVOID_LINE = false;
	if (setup()){
		headingToMaintain = heading();
	}

	if (AVOID_LINE && lineAny()){
		actionSet(actionLineAvoid);
		if (mrm_ref_can->any(false, 0)) // Front
			directionCurrent = headingRandom(-180, VARIATION);
		if (mrm_ref_can->any(false, 1)) // Right
			directionCurrent = headingRandom(-90, VARIATION);
		if (mrm_ref_can->any(false, 2)) // Back
			directionCurrent = headingRandom(0, VARIATION);
		if (mrm_ref_can->any(false, 3)) // Left
			directionCurrent = headingRandom(90, VARIATION);
	}
	else{
		uint16_t minDistance = left();
		if (right() < minDistance) 
			minDistance = right();
		if (front() < minDistance) 
			minDistance = front();
		if (back() < minDistance) 
			minDistance = back();
		go(map(minDistance, 0, 700, 40, SPEED), directionCurrent, 
			pidRotation->calculate(heading() - headingToMaintain));
		...
	}
}
Prvo nađemo minimalnu udaljenost u sva 4 smjera i spremimo ju u varijablu "minDistance".

Pokrenemo robota brzinom koja se proporcionalno smanjuje kako se smanjuje udaljenost do najbliže prepreke, kako bi se robot elegantno odbio.

Smjer u kojem pokrećemo robota je izračunat u prethodnom prolazu.

Rotacija oko okomite osi se dobije PID kontroler algoritmom, kojeg nećemo ovdje proučavati.

Izračunavanje kuta

void RobotSoccer::bounce(){
	static int directionCurrent = 45;
	const int VARIATION = 45;
	const int SPEED = 127;
	const bool AVOID_LINE = false;
	if (setup()){
		headingToMaintain = heading();
	}

	if (AVOID_LINE && lineAny()){
		actionSet(actionLineAvoid);
		if (mrm_ref_can->any(false, 0)) // Front
			directionCurrent = headingRandom(-180, VARIATION);
		if (mrm_ref_can->any(false, 1)) // Right
			directionCurrent = headingRandom(-90, VARIATION);
		if (mrm_ref_can->any(false, 2)) // Back
			directionCurrent = headingRandom(0, VARIATION);
		if (mrm_ref_can->any(false, 3)) // Left
			directionCurrent = headingRandom(90, VARIATION);
	}
	else{
		uint16_t minDistance = left();
		if (right() < minDistance) 
			minDistance = right();
		if (front() < minDistance) 
			minDistance = front();
		if (back() < minDistance) 
			minDistance = back();
		go(map(minDistance, 0, 700, 40, SPEED), directionCurrent, 
			pidRotation->calculate(heading() - headingToMaintain));
		if (left() < 200){
			directionCurrent = headingRandom(90, VARIATION);
			// print("Left %i\n\r", directionCurrent);
		}
		if (right() < 200){
			directionCurrent = headingRandom(-90, VARIATION);
			// print("Right %i\n\r", directionCurrent);
		}
		if (front() < 200){
			directionCurrent = headingRandom(180, VARIATION);
			// print("Front %i\n\r", directionCurrent);
		}
		if (back() < 200){
			directionCurrent = headingRandom(0, VARIATION);
			// print("Back %i\n\r", directionCurrent);
		}

		static uint32_t ms = 0;
		if (millis() - ms > 1000){
			print("Target: %i, current: %i\n\r", (int)headingToMaintain, (int)heading());
			ms = millis();
		}
	}
}
U ovom dijelu pristupamo izračunavanju kuta odbijanja.

Nađemo zid bliži od 200 mm i promijenimo smjer na način da idemo okomito od prepreke, uz neku varijaciju.

Varijacija se događa u funkciji "directionCurrent()", koju sad nećemo proučavati.

Video

Odbijanje od zidova.

Zadatak: kut odbijanja.

Neka kud odbijanja bude jednak ulaznom kutu.

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.