#include <DCMotorBot.h>

DCMotorBot bot;

#define left 2
#define right 3

#define wartoscmin 0
#define wartoscmax 700
struct Pomiary
{
  int lewy;
  int prawy;
} odczyt;
void zmierz(struct Pomiary &odczyt);
void steruj(struct Pomiary odczyt);
void drukuj(struct Pomiary odczyt);

void zmierz(struct Pomiary &odczyt)
{
  odczyt.lewy = map(analogRead(left), 0, 1023, wartoscmin, wartoscmax);
  odczyt.prawy = map(analogRead(right), 0, 1023, wartoscmin, wartoscmax);
}

void steruj(struct Pomiary odczyt)
{
  // jesli fototranzystor jest wpięty między analog pin i masę
  if(odczyt.lewy == odczyt.prawy)
  {
    bot.moveForward();
  }
  if(odczyt.lewy < odczyt.prawy)
  {
    //wiecej swiatla po lewej
    bot.turnLeft();
  }
  if(odczyt.lewy > odczyt.prawy)
  {
    //wiecej swiatla po prawej
    bot.turnRight();
  }
}

void drukuj(struct Pomiary odczyt)
{
  Serial.print("lewy czujnik: ");
  Serial.println(odczyt.lewy);
  Serial.print("prawy czujnik: ");
  Serial.println(odczyt.prawy);
  Serial.println();
}

void setup()
{
  bot.setEnablePins(10, 11);
  bot.setControlPins(9, 8, 12, 13);
  Serial.begin(57600);
  bot.moveForward();
  bot.stop();
}

void loop()
{
  zmierz(odczyt);
  steruj(odczyt);
  //drukuj(odczyt);
  delay(100);
}