====== Orienteeruja ======

Lihtne labürindis orienteeruja, mis baseerub Robootika Kodulaboril ja RP05 lintidega robotplatvormil.

{{:projects:rp05:rp05_maze.jpg|}}

===== Versioon 1 - Lihtne lahendus =====

{{:projects:rp05:robot_v1.hex|Kompileeritud HEX}}

{{:et:projects:saaremaa.jpg?500|}}

<code c>
//
// Roboti näidisprogramm v1.
//

#include <stdio.h>
#include <homelab/pin.h>
#include <util/delay.h>
#include <homelab/module/sensors.h>
#include <homelab/module/lcd_gfx.h>
#include <homelab/module/motors.h>
 
//
// Ultraheli anduri ja multiplekseri viigud
//
pin multiplekser = PIN(G,0);
pin pin_trigger = PIN(F, 2);

// Nuppude viigud
pin buttons[3] = { PIN(C, 2), PIN(C, 1), PIN(C, 0) };

//
// Põhiprogramm
//
int main(void)
{ 		 
	unsigned short distance;
	char text[16];
	bool lipp = 0;
	// Multiplekseri seadistamine
	pin_setup_output(multiplekser);
	pin_set(multiplekser);
	// Nuppude seadistamine sisendiks
	for (int i = 0; i < 3; i++) pin_setup_input(buttons[i]);
	// Mootorite seadistamine
	dcmotor_init(0);
	dcmotor_init(1);
	// LCD ekraani algseadistamine
	lcd_gfx_init();
	// Ekraani puhastamine
	lcd_gfx_clear();
	// Taustavalgustuse tööle lülitamine
	lcd_gfx_backlight(true);
	// Programmi nime kuvamine
	lcd_gfx_goto_char_xy(1, 1);
	lcd_gfx_write_string("Robot_v1");
	// Väike paus
	_delay_ms(100);
 	// Lõputu tsükkel
	while (true)
	{
		// Mõõtmine
		distance = ultrasonic_measure_srf05(pin_trigger);
		// Kontrollime nuppu ja salvestame nupuvajutuse
		if(!pin_get_value(buttons[0])) lipp = 0; // Vajutades 0 nuppu läheb lipp madalaks
		if(!pin_get_value(buttons[1])) lipp = 1; // Vajutades 1 nuppu läheb lipp kõrgeks
		// Mõõtmine õnnestus ?
		if (distance > 0)
		{			
			// Kauguse tekstiks teisendamine
			sprintf(text, "%d cm   ", distance);
		}
		// Mõõtmisel tekkis viga ?
		else
		{
			// Vea tekst
			sprintf(text, "Viga    ");
		}			
		// Teksti kuvamine LCD teise rea alguses
		lcd_gfx_goto_char_xy(2, 2);
		lcd_gfx_write_string(text);

		// Kauguse kontorllimine
		// Kui nuppu on vajutatud ja distants on alla teatud väärtuse siis keerame paremale
		if (lipp&&(distance<25))
		{
			dcmotor_drive(0, 1);       // 0 mootor edasi
			dcmotor_drive(1,-1);       // 1 mootor tagasi
			sprintf(text,"paremale "); // kirjutame kontrolliks text massiivi paremale
			_delay_ms(300);            // ootame keeramist, ooteaja pikkus valitakse katseliselt
			distance = ultrasonic_measure_srf05(pin_trigger); // Kordame mõõtmist
			// Kui distants oli endiselt alla teatud väärtuse keerame otsa ringi, ehk algsest asendist vasakule
			if ( distance<25)
			{
				dcmotor_drive(0,-1);       // 0 mootor tagasi
				dcmotor_drive(1, 1);       // 1 mootor edasi
				sprintf(text,"vasakulse"); // kirjutame text massiivi vasakule
				_delay_ms(600);			   // ootame keeramist, ooteaja pikkus valitakse katseliselt
				distance = ultrasonic_measure_srf05(pin_trigger); // Kordame mõõtmist
				// Kui distants on endiselt alla teatud väärtuse jääme seisma
				if(distance<25)
				{
					dcmotor_drive(0, 0);
					dcmotor_drive(1, 0);
					sprintf(text,"sein");
				}
		 	}
		}
		// Kui takistust pole sõidame otse edasi
		else if(lipp)
		{
			dcmotor_drive(0,1);
			dcmotor_drive(1,1);
			sprintf(text, "otse");
		}
		// Kui nupp 0 on alla vajutatud jääme seisma
		else
		{
			sprintf(text,"seisab");
			dcmotor_drive(0,0);
			dcmotor_drive(1,0);
		}
		// Teksti kuvamine LCD teise rea alguses
		lcd_gfx_goto_char_xy(3, 3);
		lcd_gfx_write_string(text);

		sprintf(text, "lipp %d", lipp);
		lcd_gfx_goto_char_xy(2, 4);
		lcd_gfx_write_string(text);
		
		// Väike paus
		_delay_ms(50);
	}
}
</code>

===== Versioon 2 - Targem lahendus =====

{{:projects:rp05:robot_v3.hex|Kompileeritud HEX}}


<code c>
//
// Roboti näidisprogramm v3.
//
#include <stdio.h>
#include <homelab/pin.h>
#include <util/delay.h>
#include <homelab/module/sensors.h>
#include <homelab/module/lcd_gfx.h>
#include <homelab/module/motors.h>
 
//
// Ultraheli anduri ja multiplekseri viigud
//
pin multiplekser = PIN(G,0);
pin pin_trigger = PIN(F, 0);
// Nuppude viigud
pin buttons[3] = { PIN(C, 2), PIN(C, 1), PIN(C, 0) };
//
// Põhiprogramm
//
int main(void)
{ 		 
	unsigned short distance;
	char text[16];
	bool lipp = 0;
	// Loome servo positsioonide massiivi, kus on eelnevalt kindlaks tehtud ääred ja keskasend
	// { parem äär, keskasend, vasak äär, keskasend}
	// NB! asendieid võivad olla nihkes, ning vastavalt servo paigaldusele valesti.
	int positsioon[4] = {-178,-40,110,-40 };
	// Loome suundade mälumassiivi kuhu hiljem hakkame tõeväärtusi kirjutama
	// 0 - sel suunal on takistus , 1 - takistus puudub
	int suund[4] = { 0, 0, 0, 0};
	// Multiplekseri seadistamine
	pin_setup_output(multiplekser);
	pin_set(multiplekser);
	// Nuppude seadistamine sisendiks
	for (int i = 0; i < 3; i++) pin_setup_input(buttons[i]);
	// Mootorite seadistamine
	dcmotor_init(0);
	dcmotor_init(1);
	// Servo seadistamine
	servomotor_init(0);
	// LCD ekraani algseadistamine
	lcd_gfx_init();
	// Ekraani puhastamine
	lcd_gfx_clear();
	// Taustavalgustuse tööle lülitamine
	lcd_gfx_backlight(true);
	// Programmi nime kuvamine
	lcd_gfx_goto_char_xy(1, 1);
	lcd_gfx_write_string("Robot_v3");
	// Väike paus
	_delay_ms(100);
 	// Lõputu tsükkel
	while (true)
	{
		// Mõõtmine
		distance = ultrasonic_measure_srf05(pin_trigger);
		// Kontrollime nuppu ja salvestame nupuvajutuse
		if(!pin_get_value(buttons[0])) lipp = 0; // Vajutades 0 nuppu läheb lipp madalaks
		if(!pin_get_value(buttons[1])) lipp = 1; // Vajutades 1 nuppu läheb lipp kõrgeks
		// Mõõtmine õnnestus ?
		if (distance > 0)
		{			
			// Kauguse tekstiks teisendamine
			sprintf(text, "%d cm   ", distance);
		}
		// Mõõtmisel tekkis viga ?
		else
		{
			// Vea tekst
			sprintf(text, "Viga    ");
		}			
		// Teksti kuvamine LCD teise rea alguses
		lcd_gfx_goto_char_xy(2, 2);
		lcd_gfx_write_string(text);
		// Kontrollime kas sisselülitamise nuppu on vajutatud
		if(lipp)
		{
			// Kordustsüklis hakkame positsiooni ja suuna massiive kontrollima
			for(int i = 0; i<4;i++)
			{
				// Liigutame servomootorit positsiooni massiivist võetud väärtuse võrra
				servomotor_position(0, positsioon[i]);
				// Ootame servo liikumist
				_delay_ms(250);
				// Salvestame ultraheli kauguse näidu
				distance = ultrasonic_measure_srf05(pin_trigger);
				// Tingimusel et antud suund oli vaba kirjutame suuna massiivile vastavasse lahtrisse 1.
				// Distantsi kaugus määrata katseliselt.
				if(distance>25) suund[i] = 1;
				// Kui sel suunal oli takistus kirjutame sinna 0
				else suund[i] = 0;
				// Kontrollime mälust, kas otse olevad suunad on vabad
				if (suund[1] | suund[3])
				{
					dcmotor_drive(0, 1);  // 0 mootor otse
					dcmotor_drive(1, 1);  // 1 mootor otse
										  // Eelnev sõltub sellest kuidas mootorid on ühendatud
					sprintf(text,"otse"); // Kirjutame text massiivi 'otse' hilisemaks kontrolliks.
				}
				// Kontrollime mälust, kas parem suund on takistuseta
				else if(suund[0])
				{
					dcmotor_drive(0,-1);   // 0 mootor tagasi
					dcmotor_drive(1, 1);   // 1 mootor edasi
					sprintf(text,"parem"); // Kirjutame text massiivi parem
					_delay_ms(300);        // Ootame keeramist, pausi pikkus määrata katseliselt
				}
				// Kontrollime mälust, kas vasak suund on takistuseta
				else if(suund[2])
				{
					dcmotor_drive(0, 1);   // 0 mootor edasi
					dcmotor_drive(1,-1);   // 1 mootor tagasi
					sprintf(text,"vasak"); // Kirjutame text massiivi vasak
					_delay_ms(300);        // Ootame keeramist
				}
				// Kui ükski suund pole vaba jäävad mootorid seisma
				else
				{
					dcmotor_drive(0, 0);
					dcmotor_drive(1, 0);
					sprintf(text,"sein");
				}
			}
		}
		// Kui 0 nupp on vajutatud siis jätab mootorid seisma
		else
		{
			sprintf(text,"seisab");
			dcmotor_drive(0,0);
			dcmotor_drive(1,0);
		}
		// Teksti kuvamine LCD-le
		lcd_gfx_goto_char_xy(2, 3);
		lcd_gfx_write_string(text);
		sprintf(text, "suund %d,%d,%d,%d", suund[0],suund[1],suund[2],suund[3]);
		lcd_gfx_goto_char_xy(2, 4);
		lcd_gfx_write_string(text);
		// Väike paus
		_delay_ms(50);
	}
}
</code>