Acrob robot prejde bludisko, a program ďalej vizualizuje mapu bludiska na obrazovke (Michal Borčin & Barbora Zaťková)

Úvod

• Našou úlohou bolo skonštruovať robota Acrob, ktorý prejde celé bludisko, a počas toho zaznamenáva údaje tak, aby sa bludisko dalo vykresliť na obrazovku.

Bludisko a zapojenie robota

• Naše bludisko bolo reprezentované čiernou páskou na bielom papieri, kde páska značila cestu, po ktorej robot prechádzal a biele okolie značili steny.









• Použili sme štyri analógové Line senzory, ktoré sme zapojili podľa nasledujúcej schémy http://ap.urpi.fei.stuba.sk/sensorwiki/index.php/Acrob008. Pomocou týchto senzorov sme naprogramovali sledovanie čiary.









• Pripojili sme aj jeden pushbutton, pomocou ktorého sme vypisovali údaje. Zapojili sme ho podľa tejto schémy http://ap.urpi.fei.stuba.sk/sensorwiki/index.php/Acrob004.

Algoritmus sledovania čiary

if ((sen == 10) || (sen == 2)) //0101 a 0100
{
//rovno
LeftServo.write(95);
RightServo.write(85);
// go_straight();
}
else if ((sen == 3)||(sen == 1)) //1100 a 1000
{
//vlavo
RightServo.write(84);
LeftServo.write(88);
delay(250);
}
else if (sen==0) { //0000 -
// otoc
LeftServo.write(95);
RightServo.write(95);
}
else
{
//vpravo
LeftServo.write(95);
RightServo.write(92);
}

Ako sa náš robot pohyboval po bludisku:

Vykreslenie bludiska

• Ďalej bolo našou úlohou vykresliť prejdené bludisko na obrazovku. My sme zvolili metódu offline pomocou kábla.

Naša idea zapamätania bludiska

• pri spustení robot začína v čase 0 a ide rovno
• my si chceme zapamätať iba zákruty, na ktorých sa točí dlhšie ako 0,2 sekundy, a teda krátke otočenia, ktorými sa robot iba vyrovnáva nás nezaujímajú
• informáciu o tom, že robot ide rovno si nemusíme pamätať, keďže vieme, že išiel rovno medzi dvoma zatáčkami
• keď robot rozpozná zákrutu dlhšiu ako 0,2 sekundy, zapíše do poľa znak P, L (smer otočenia) alebo O(značí otočenie), a tiež zapíše čas, v ktorom sa daná operácia uskutočnila
• Príklad poľa:

0 0
p 4
o 4
p 8
o 8
l 13
o 13
p 18
o 18

• vidíme teda, že robot išiel rovno, potom sa v čase 4 otočil doprava, išiel rovno a znova sa otočil doprava v čase 8...

Program v Arduino

#include <Servo.h>

#define SENSOR_1     2 //lavy
#define SENSOR_2     1 //stredny
#define SENSOR_3     0 //pravy
#define SENSOR_P     3 //predny
#define BUTT0N      2 //gombik


Servo LeftServo;
Servo RightServo;



int i=0;
int j=0;
char smer[100];
byte cas[100];
boolean toci = false;
boolean otoc = false;
boolean rovno= false;
long time_lava=0;
long time_prava=0;
int click = 0;
byte zatocR = 0;
byte zatocO = 0;
byte zatocL = 0;
 
void setup() 
{ 
  pinMode(BUTT0N, INPUT);
  pinMode(SENSOR_2, INPUT);
  // nastavenie pinov ako vstupnych
  pinMode(SENSOR_1, INPUT);
  pinMode(SENSOR_3, INPUT);
  pinMode(SENSOR_P, INPUT);
  LeftServo.attach(9);    // attaches the servo on pin 9 to the servo object 
  RightServo.attach(10);  // attaches the servo on pin 10 to the servo object 
  
  Serial.begin(9600);
  Serial.println("START");

 
} 
 

int GSD()
{
int s=0;
if(analogRead(SENSOR_1)>600) s+=1;
if(analogRead(SENSOR_2)>600) s+=2;
if(analogRead(SENSOR_3)>600) s+=4;
if(analogRead(SENSOR_P)>600) s+=8;
return s;
}

void loop() 
{ 
  


    click = digitalRead(BUTT0N);
    //Serial.println(click,BIN);
    /*if(micros() > 20000000) {
      Serial.println("stopped!");
    LeftServo.write(90);
      RightServo.write(90);
    }*/
    if(click == HIGH){
      LeftServo.write(90);
      RightServo.write(90);
      delay(10000);
      Serial.println("stopped");
      byte rea1 =0;
      byte rea2 =0;
      char rea3 =0;
      while(rea1!=42 && rea2!=42 && rea3!=42){
      //LeftServo.write(90);
     // RightServo.write(90);
      rea1=Serial.read();      
      rea2=Serial.read();      
      rea3=Serial.read();
      delay(50);      
      }
      for(j=0;j<=i;j++){
        Serial.print(smer[j]);
        Serial.print(' ');
        Serial.println(cas[j]);
      }
      Serial.println("ended -!-");
      delay(50000);
    }
    
    int sen=GSD();
    if ((sen == 10) || (sen == 2)|| (sen == 11)) //0101 a 0100
    {
    //rovno
    if(rovno==false) {
      rovno=true;
      //time_rovno=micros();
    
      if(toci) {
        if((micros() - time_lava) > 800000) {
           if(zatocL >5) {
             /// tocil dolava
            smer[i]='l';
            cas[i]=byte(micros()/1000000);
            Serial.print("l ");
            Serial.println(micros()/1000000);
            i++; 
            zatocL=0;
            //zatocR=0;
            time_lava=0;
            toci= false;    
           }else
           if((micros()-time_prava) > 800000){
           if(zatocR >200) {
             /// tocil doprava
            smer[i]='p';
            cas[i]=byte(micros()/1000000);
            Serial.print("p ");
            Serial.println(micros()/1000000);
            i++;
            zatocO=0;
            //zatocL=0;
            zatocR=0;
            time_prava=0;
            toci= false;            
           }
           }
           
        } 
      }
      if(otoc){
        // uloz ze otacal a cas
        if((micros() - time_prava) > 500000) {
        if(zatocR > 240){
        //smer[i]='o';
        //cas[i]=byte(micros()/1000000);
        Serial.print("o ");
        Serial.println(micros()/1000000);
        i++;
        //time_zmena=0;
        otoc=false;
        zatocR=0;
        }
      }
      }
      
      
    }
      LeftServo.write(92);
      RightServo.write(88);
      /*if((toci || otoc) && (time_rovno>800000)) {
       toci=false;
       otoc=false;
       time_rovno=0;
       time_zmena=0;
      }*/
    }
    else if ((sen == 3)||(sen == 1))  //1100 a 1000
    {
     //vlavo
     rovno=false;
     //time_rovno=0;
     if(toci == false){
     toci=true;
     zatocL++;
     time_lava=micros();
     }     
     if(toci){zatocL++;}
     
     RightServo.write(85);
     LeftServo.write(89);
     delay(250);
    } 
    else if (sen==0) { //0000 - 
     // otoc 
     rovno=false;
     //time_rovno=0;
     if(toci==false || otoc == false){
     otoc=true;
     
     //time_zmena=micros();  
   }
   if(otoc) {zatocO++;}
     if(toci) {zatocR++;}
     LeftServo.write(95);
     RightServo.write(95);
    }    
    else 
    {
     //vpravo
     rovno=false;
     //time_rovno=0;
     if(toci == false){ 
       toci=true;
       time_prava = micros();
     }
     if(toci){zatocR++;}
      LeftServo.write(95);
     RightServo.write(92);
    }  
}