#define wartosc 500
#define predkosc 255
#define predkosc2 200
#include <DCMotorBot.h>

DCMotorBot bot;


void sprawdz(struct odczyty &odczyt);
void steruj(struct odczyty odczyt);
void drukuj(struct odczyty odczyt);
 
void setup()
{
bot.setEnablePins(10, 11);
bot.setControlPins(9, 8, 12, 13);
Serial.begin(57600);
bot.moveForward();
bot.stop();
}

struct odczyty
{
  int przew;
  int prwew;
  int lewwew;
  int lewzew;
} odczyt;

void sprawdz(struct odczyty &odczyt)
{

  odczyt.przew = analogRead(2);
  odczyt.prwew = analogRead(3);
  odczyt.lewwew = analogRead(4);
  odczyt.lewzew = analogRead(5);

}

void steruj(struct odczyty odczyt)
{
  if (odczyt.lewwew > wartosc || odczyt.prwew > wartosc)
  {
    bot.moveForward();
    //jedzie na wprost
    if (odczyt.lewzew > wartosc && odczyt.przew > wartosc)
   {
   bot.moveForward();
      // jedzie dalej
   }
    }
  if (odczyt.przew > wartosc && odczyt.lewzew < wartosc)
    {
      //prawy zwalnia
    //  bot.stop();
    bot.turnRight();
    }
  if (odczyt.przew < wartosc && odczyt.lewzew > wartosc)
    {
      //lewy zwalnia
    //bot.stop();
    bot.turnLeft();
    }
  
  /* if(odczyt.lewwew < wartosc && odczyt.prwew < wartosc && odczyt.przew < wartosc && odczyt.lewzew < wartosc)
    {
      lewy.setSpeed(0);
      prawy.setSpeed(0);
    }
  */
}

void drukuj(struct odczyty odczyt)
{

  Serial.print("AP 1: ");
  Serial.print(odczyt.przew);
  Serial.print(" AP 2: ");
  Serial.print(odczyt.prwew);
  Serial.print(" AP 4: ");
  Serial.print(odczyt.lewwew);
  Serial.print(" AP 5: ");
  Serial.print(odczyt.lewzew);
  Serial.print("\r\n");

}


void loop()
{
  
  sprawdz(odczyt);
  steruj(odczyt);
  drukuj(odczyt);
 delay(10);
}