Table of Contents
Orienteeruja
Lihtne labürindis orienteeruja, mis baseerub Robootika Kodulaboril ja RP05 lintidega robotplatvormil.
Versioon 1 - Lihtne lahendus
// // 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); } }
Versioon 2 - Targem lahendus
// // 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); } }
