#include <Wire.h>
#include <Servo.h>

int fotorezystorgora = 5;
int fotorezystordol = 4;
int fotorezystorprawo = 3;
int fotorezystorlewo = 2;
float pomiarnapiecia = 1;

int fot1 = 0;
int fot2 = 0;
int fot3 = 0;
int fot4 = 0;
float napiecie = 0;



Servo serwomechanizm1;
Servo serwomechanizm2;

int pozycja = 0; //Aktualna pozycja serwa 0-180

void setup()
{
  pinMode(fotorezystorgora, INPUT);
  pinMode(fotorezystordol, INPUT);
  pinMode(fotorezystorprawo, INPUT);
  pinMode(fotorezystorlewo, INPUT);
  pinMode(pomiarnapiecia, INPUT);
  Serial.begin(9600);


  serwomechanizm1.attach(9);  //Serwomechanizm podłączony do pinu 9
  serwomechanizm2.attach(10);  //Serwomechanizm podłączony do pinu 9
}

void loop (){

  fot1 = analogRead(fotorezystorgora);
  fot1 = map(fot1, 0,1023, 0,180);
  Serial.print("gora: ");
  Serial.print(fot1);
  Serial.print("   ");

  fot2 = analogRead(fotorezystordol);
  fot2 = map(fot2, 0,1023, 0,180);
  Serial.print("dol: ");
  Serial.print(fot2);
  Serial.print("   ");

  fot3 = analogRead(fotorezystorprawo);
  fot3 = map(fot3, 0,1023, 0,180);
  Serial.print("prawo: ");
  Serial.print(fot3);
  Serial.print("   ");

  fot4 = analogRead(fotorezystorlewo);
  fot4 = map(fot4, 0,1023, 0,180);
  Serial.print("lewo: ");
  Serial.print(fot4);
  Serial.print("   ");

  Serial.println("   ");

  if(fot1>fot2){
    gora();
  }

  else if (fot2>fot1){
    dol();
  }
  else{
    srodek();
  }


  if(fot3>fot4){
    prawo();
  }

  else if (fot3<fot4){
    lewo();
  }
  else{
    srodek2();
  }



  delay(1000);
  }



  void gora(){
    serwomechanizm1.write(45);
  }

  void dol() {
    serwomechanizm1.write(135);
  }

  void prawo() {
    serwomechanizm2.write(0);
  }

  void lewo() {
    serwomechanizm2.write(177);
  }

  void srodek() {
   serwomechanizm1.write(90);
  }

  void srodek2() {
   serwomechanizm2.write(90);
  }