#include <LiquidCrystal.h>
#include <String.h>
#include <stdio.h>
#include <math.h>

//Zapojení pinu pro ovládaní levého motoru
#define ENA  9    // ovládání pwm
#define IN1  8    
#define IN2  7    

//Zapojení pinu pro ovládaní pravého motoru
#define ENB  4   // ovládání pwm 
#define IN3  6    
#define IN4  5    

//Přední ultrazvukový senzory - zapojení na piny Arduina
#define PREDNIECHOPIN 48   
#define PREDNITRIGPIN 50  

//Levý ultrazvukový senzory - zapojení na piny Arduina
#define LEVYECHOPIN 53 
#define LEVYTRIGPIN 52 

//Pravý ultrazvukový senzory - zapojení na piny Arduina
#define PRAVYECHOPIN 12  
#define PRAVYTRIGPIN 13 

//Diplej - zapojení jednoltivých pinů
#define RS 25
#define ENABLE 29
#define D4 41
#define D5 43
#define D6 45
#define D7 47
#define VSS 23
#define RW 27
#define A 49
#define K 51

//Piny přepínače
#define PREPINAC 2
#define GND1 3

//inicializace dipleje
LiquidCrystal lcd (RS, ENABLE, D4 , D5 , D6 , D7); //nastaveni displeje rs,enable,d4,d5,d6,d7

//defaultní rychlost pro jednotlivé motory
int pravekolo = 255;  //rychlost pro pravý motor (pwm 0-255)
int levekolo = 255;   //rychlost pro levý motor  (pwm 0-255)

//pomocné proměnné programu
char znak;  
int speed = 100; // proměnná pro změnu rychlosti, při ovládání mobilem
int vzdalenost = 20;  //definovaná minimální vzdálednost od překážky v cm
long m = 0;
boolean prvnicyklus = true;

void setup()
{
  Serial3.begin(9600);  //bluetooth komunikace probíhá přes seriovou linku 3 ma pinech 14(TX) a 15(RX)
  
  //nastaveni pinů pro jednotlivé ultrazvukové senzory
  pinMode(PRAVYECHOPIN, INPUT); 
  pinMode(PRAVYTRIGPIN, OUTPUT); 
  pinMode(LEVYECHOPIN, INPUT); 
  pinMode(LEVYTRIGPIN, OUTPUT); 
  pinMode(PREDNIECHOPIN, INPUT); 
  pinMode(PREDNITRIGPIN, OUTPUT); 
  
  //nastavení pinů pro motory
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  //piny přepínače
  pinMode(PREPINAC, INPUT);  
  pinMode(GND1, OUTPUT);  
  digitalWrite(GND1,LOW);
  
   //nastevní pinů displeje
   pinMode(VSS,OUTPUT);//vss
   digitalWrite(VSS,LOW);
   pinMode(RW,OUTPUT);//RS
   digitalWrite(RW,LOW);
   pinMode(A,OUTPUT);//A
   digitalWrite(A,HIGH);
   pinMode(K,OUTPUT);//K
   digitalWrite(K,LOW);

  //zapnutí dipleje velikosti 2 řádky, 16 znaků
   lcd.begin(16, 2); 
}


void loop() {

if(digitalRead(PREPINAC)==LOW)
{
  //jestlize je prepnuto do rezimu dalkoveho rizeni, robot se zastavi v prvnim cyklu
      if(prvnicyklus == true)
      { 
        zastav();
        prvnicyklus= false;
      }
  //ovládání mobilem
        if ( Serial3.available()) // test jestli jsou na seriové lince data
      {
       znak = Serial3.read();  //přečte znak ze seriové linky a uloží do char znak
      
        switch (znak){ 
        case '0':          //zastavi          
              zastav();              
         break;
         
        case '1':          //jizda vpred 
              vpred();
        break;
        
        case '2':          //jizda vzad 
              vzad();
        break;
         
        case '3':          //jizda vlevo ,leve kolecko se musi zastavit
              vlevo();
        break;
         
         case '4':          //jizda vpravo ,prave kolecko se musi zastavit
              vpravo();
         break;
         
        case '5':          //zvýšení rychlosti (0 az 100%)
              if(speed<100)
               speed=speed+10;
         break;
         
         case '6':          
              if(speed>0)  //snížení rychlosti (0 az 100%)
              speed=speed-10;
         break;  
         
        default:     
              zastav();  
         break;      
    }
    }
      //každých 100ms se aktualizuje displej
     if (millis() -m > 100)
      {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Ovladani mobilem");
    lcd.setCursor(1,1);
    lcd.print("Rychlost: ");
    lcd.print(speed);
    lcd.print("%");
      m = millis();
      }
      //cim je nizsi nastvena rychlost, umerne s tim ze zvysuje rozdil rychlosti kol (leve pomalejsi)
    pravekolo=155+speed; //nastavení rychlosti pro jednotlivé kola 
    levekolo=180+0.75*speed;     
    
}
else
{
    prvnicyklus= true;
  //pevná, neměnná rychlost pro autonomní režim
    pravekolo = 140;
    levekolo = 180;
    
    //nastavení displeje
    lcd.clear();  //vycisti displej
    lcd.setCursor(0,0);  //nastavi kurzor na zacatek
    lcd.print("Autonomni rezim");
    
    //definování potřebných proměnných
    //left, right, forward - naměřené vzdálenosti jednotlivých senzorů
    int left, right , forward , bezpecnaVzdalenost = 35;

    //měření vzdáleností
    delay(50);
    left = levavzdalenost();  
    delay(50);
    right = pravavzdalenost();      
    delay(50);
    forward = prednivzdalenost();        

    //algoritmus autonomního režimu
    if(forward <bezpecnaVzdalenost)
      if(vlevo>vpravo)
      {
        vpravodozadu();
        delay(200);
      }
      else
      {
        vlevodozadu();
        delay(200);
      }
    else  
      if(right<bezpecnaVzdalenost)
      {
       vpravodozadu();
       delay(200);
      } 
      else
        if(left<bezpecnaVzdalenost)
        {
          vlevodozadu();
          delay(200);
        }
    else
     vpred();
}
}

//funkce vpred,vzad,zastav,vlevo a vpravo ovladají pohyb robota
void vpred()  
    {
      analogWrite(ENA, pravekolo); //nastaví PWM (0-255) pravéhé kola, čím vyšší PWM, tím vyšší energie a rychlost kola
      digitalWrite(IN1, HIGH);    //pro jízdu vpřed musíme nastavit HIGH na piny IN1 a IN3, a LOW na IN2 a IN4
      digitalWrite(IN2, LOW);  
      analogWrite(ENB, levekolo);  //nastaví PWM (0-255) levého kola
      digitalWrite(IN3, HIGH);  
      digitalWrite(IN4, LOW);  
  }
  
void vzad()
    {
      analogWrite(ENA, pravekolo);  
      digitalWrite(IN1,  LOW);  
      digitalWrite(IN2, HIGH);  
      analogWrite(ENB, levekolo);  
      digitalWrite(IN3, LOW);  
      digitalWrite(IN4, HIGH);  
    }
  
void zastav()
    {
      analogWrite(ENA,HIGH);  
      digitalWrite(IN1, HIGH);  
      digitalWrite(IN2, HIGH);  
      analogWrite(ENB, HIGH);  
      digitalWrite(IN3, HIGH);  
      digitalWrite(IN4, HIGH);  
   }

//funkce vlevo a vpravo mají rychlosti nastavené jinak, protože při točením pouze jedním kolem, je potřeba větší výkon
//funkce vlevo a vpravo nevyužíva autonomí režim, tudíž jejich rychlost může být definována tímto rozdílným způsobem
void vpravo()//jedeme doprava, ale tocime levym koleckem
    {
          analogWrite(ENA, 0);  
          digitalWrite(IN1, HIGH); 
          digitalWrite(IN2, LOW);  
          analogWrite(ENB, 205+(speed/2));  
          digitalWrite(IN3, HIGH);  
          digitalWrite(IN4, LOW);  
  }
void vlevo()
    {
          analogWrite(ENA, 205+(speed/2));  
          digitalWrite(IN1, HIGH); 
          digitalWrite(IN2, LOW);  
          analogWrite(ENB, 0);  
          digitalWrite(IN3, HIGH);  
          digitalWrite(IN4, LOW);  
    }
    

 //pohybové funkce vpravodozadu a vlevodozadu využívá pouze autonomní režim, proto zde jsou nastaveny pevné rychlosti
void vpravodozadu()
    {
          analogWrite(ENA, 0);  
          digitalWrite(IN1, LOW);  
          digitalWrite(IN2, HIGH);  
          analogWrite(ENB, 200);  
          digitalWrite(IN3, LOW);  
          digitalWrite(IN4, HIGH);  
    }  
  
void vlevodozadu()
    {
          analogWrite(ENA, 200); 
          digitalWrite(IN1, LOW);  
          digitalWrite(IN2, HIGH);  
          analogWrite(ENB, 0);  
          digitalWrite(IN3, LOW);  
          digitalWrite(IN4, HIGH); 
    }  

//funkce pro měření vzdálenosti z jednotlivých senzorů
float levavzdalenost()  
   {
      digitalWrite(LEVYTRIGPIN, LOW); 
      delayMicroseconds(2); 
      digitalWrite(LEVYTRIGPIN, HIGH); 
      delayMicroseconds(10); 
      digitalWrite(LEVYTRIGPIN, LOW); 
      // Spočítá vzdálenost
       float levydistance = pulseIn(LEVYECHOPIN, HIGH); 
      levydistance= levydistance*0.017315f;      
      return levydistance;
   }
   
float pravavzdalenost()  
   {
      digitalWrite(PRAVYTRIGPIN, LOW); 
      delayMicroseconds(2); 
      digitalWrite(PRAVYTRIGPIN, HIGH); 
      delayMicroseconds(10); 
      digitalWrite(PRAVYTRIGPIN, LOW); 
      // Spočítá vzdálenost
       float pravydistance = pulseIn(PRAVYECHOPIN, HIGH); 
      pravydistance= pravydistance*0.017315f; ;      
      return pravydistance;
   }   
   
float prednivzdalenost()  
   {
      digitalWrite(PREDNITRIGPIN, LOW); 
      delayMicroseconds(2); 
      digitalWrite(PREDNITRIGPIN, HIGH); 
      delayMicroseconds(10); 
      digitalWrite(PREDNITRIGPIN, LOW); 
       float prednidistance = pulseIn(PREDNIECHOPIN, HIGH); 
       prednidistance= prednidistance*0.017315f;
       return prednidistance; 
   } 
  
