// Vytvoil: Antonn idla
// Studijn skupina, obor: Informatika a dc technologie - Kombinovan studium
// Soust bakalsk prce na tma: Vukov pomcka pro pedmt mikropotae  mobiln robot
// Datum vytvoen: 05/2013
// Nzev programu: Detekce a objdn pekek

/* 
Popis innosti a funkc: Robot je dky tomuto programu schopn pohybu v neznmm prosted a detekce pekek.
kolem a clem tohoto programu je vyhbn se pekkm a pedchzenm kolozm. Robot nem stanoven pesn cl a pozici svho pemisovn.
zen je provdno pomoc vvojov desky Arduino UNO. Motory jsou ovldny pomoc mstku L298N.
Pohyb je vykonvn dvma motory, snmn vzdlenosti je provdno ultrazvukovm idlem SRF05 a dvma aktivnmi nraznky. Tyto detekuj pekky,
kter robot "pehldl". 
*/

/*
Princip innosti: Robot jede rovnomrn vped. Jakmile se objev pekka detekovan pomoc ultrazvukovho idla SRF05, robot se zastav, rozhldne se vpravo,
sejme vzdlenost a tuto vzdlenost ulo do promnn "prava". Oto ultrazvukov senzor pomoc servomotorku doleva, pem tmto senzorem vzdlenost a namenou
hodnotu ulo do promnn "leva". Nato snma SRF05 do pm polohy. Hodnoty promnnch "prava" a "leva" porovn. Nato se smrem, kde je namen vzdlenost vt.
Pokrauje rovn vped. Pokud se robot dotkne pekky pomoc levho i pravho dorazu, znamen to, e pekka nebyla detekovna pomoc ultrazvukovho idla.
Pi tto akci dojde k oeten tto situace. 
*/


#include <Servo.h>  // knihovna pro prci se servomotorem

// definovn pin
// Prav motor - uren vstup (numerick hodnota zna slo pinu na vvojov desce Arduino UNO)
#define PM1 2
#define PM2 3

// Lev motor - uren vstup (numerick hodnota zna slo pinu na vvojov desce Arduino UNO)
#define LM1 4
#define LM2 5

// Dotekov idla - mechanick spnae
#define Ldoraz 13
#define Pdoraz 12

// Zapojen ultrazvukovho senzoru SRF05
#define TRIGPIN 6
#define ECHOPIN 7

Servo hlava;
int pos = 0;
int leva = 0;
int prava = 0;

// Nastaven modu pin na desce Arduino pro vstupy a vstupy. Nastaven probhne ped zatkem programu
void setup() {
  pinMode(PM1, OUTPUT);  // Prav motor dopedn smr
  pinMode(PM2, OUTPUT);  // Prav motor zptn smr
  pinMode(LM1, OUTPUT);  //Lev motor dopedn smr
  pinMode(LM2, OUTPUT);  // Lev motor zptn smr
  Serial.begin(9600);  // Vstup dat, monitorovn
  pinMode(ECHOPIN, INPUT);  //SRF05 vstup
  pinMode(TRIGPIN, OUTPUT);  //SRF05 vstup
  pinMode(Ldoraz, INPUT);  //Lev doraz
  pinMode(Pdoraz, INPUT);  //Prav doraz
  hlava.attach(8);  // Servomotor
}

void PM_REW(){  // Prav motor vzad    
  digitalWrite(PM1, LOW);
  digitalWrite(PM2, HIGH);
}

void PM_STOP(){  // Prav motor stop
  digitalWrite(PM1, LOW);
  digitalWrite(PM2, LOW);
}

void PM_FWD(){  // Prav motor vped
  digitalWrite(PM1, HIGH);
  digitalWrite(PM2, LOW);
}

void LM_REW() {  //lev motor vzad
  digitalWrite(LM1, LOW);
  digitalWrite(LM2, HIGH);
}

void LM_STOP() {  // Lev motor stop
  digitalWrite(LM1, LOW);
  digitalWrite(LM2, LOW);
}

void LM_FWD() {  // Lev motor vped
  digitalWrite(LM1,HIGH);
  digitalWrite(LM2, LOW);
}

// funkce pro men pomoc senzoru SRF05
int mereni() {
  int distance;
  digitalWrite(TRIGPIN, LOW);                
  delayMicroseconds(2);
  digitalWrite(TRIGPIN, HIGH);                
  delayMicroseconds(10);
  digitalWrite(TRIGPIN, LOW);                  
  distance = pulseIn(ECHOPIN, HIGH);       
  distance =  distance/58;              
  return distance;
}

// program pracuje v nekonen smyce
void loop () {
  hlava.write(90);  // Natoen senzoru smrem vped
  delay(400);  // asov prodleva 0,4 s
  if((mereni()) > 18) {  // Pokud je vzdlenost vt jak 18, je vped 
    PM_FWD();
    LM_FWD(); 
  }
  else if ((mereni()) < 2){  // Oeten chyby men ve velk vzdlenosti
    PM_FWD();
    LM_FWD();
  }
  else {  // Pokud je jin vzdlensot ne pedchoz, rozhldni se
    PM_STOP();
    LM_STOP();
    delay(100);
    hlava.write(0);  // Natoen srevomotoru se ultrazvukovm senzorem ve smru jzdy vped
    delay(2000);
    prava = mereni();  // Piazen men do promnn "prav"
    Serial.println("Prava = ");  // Vpis hodnot pi monitorovn
    Serial.print(prava);
    hlava.write(180);
    delay(2000);    
    leva = mereni();
    Serial.println("Leva = ");
    Serial.print(leva);    
    hlava.write(90);  // Nato senzor zpt dopedu
    
    // Porovnej a prove
    if (leva > prava) {
      Serial.println("Pojedu doleva");
      PM_REW();
      LM_REW();
      delay(300);
      PM_FWD();
      LM_STOP();
      delay(500);
    }
      
    else if (leva < prava) {
      Serial.println("Pojedu doprava");
      PM_REW();
      LM_REW();
      delay(300);
      PM_STOP();
      LM_FWD();
      delay(500);      
    }
  }
 
// Akce pi stisku levho dorazu
if(digitalRead(Ldoraz) == HIGH) {
  PM_STOP();
  LM_STOP();
  delay(500);
  PM_REW();
  LM_REW();
  delay(400);
  LM_STOP();
  delay(750);
  PM_STOP();
  delay(500);
}

// Akce pi stisku levho dorazu
if(digitalRead(Pdoraz) == HIGH) {
  PM_STOP();
  LM_STOP();
  delay(500);
  PM_REW();
  LM_REW();
  delay(400);
  PM_STOP();
  delay(750);
  LM_STOP();
  delay(500);  
}

// Vpis namench hodnot z ultrazvukovho senzoru
Serial.println(mereni());
delay(50);
}



