#include <Servo.h>



const int sens1pin = 0;
const int sens2pin = 1;



Servo directserv;
int dif2;
int state;
int dirCount = 9;


void setup() {
  // put your setup code here, to run once:
  directserv.attach(3);
  Serial.begin(9600);

  int sens1 = analogRead(sens1pin);
  int sens2 = analogRead(sens2pin);

  dif2 = difof(sens1, sens2);
  state = state1(sens1, sens2);

}

void loop() {
  // put your main code here, to run repeatedly:
  int sens1 = analogRead(sens1pin);
  int sens2 = analogRead(sens2pin);
  if (state == 1) {
    sens1 = sens1 + dif2;
  }
  if (state == 2) {
    sens2 = sens2 + dif2;
  }
  int servh = 70;
  int servl = 110;
  int dir3 = difof(sens1, sens2);

  dir3 = dir3 / 6;

  if (sens1 < sens2){
    servh = servh + dir3;
    servl = servl + dir3;
  }
  if (sens1 > sens2){
    servh = servh - dir3;
    servl = servl - dir3;
  }

  servh = constrain(serh, 180, 40);
  servl = constrain(serl, 140, 0);
  
  directserv.write(servh);
  delay(250);
  directserv.write(servl);
  delay(250);
}


int difof(int val1, int val2) {
  int dif;

  if (val1 < val2) {
    dif = val2 - val1;
  }
  if (val2 < val1) {
    dif = val1 - val2;
  }
  Serial.println(dif);
  return dif;
}

int state1(int v1,int v2){
  if (v1 < v2){
    state = 1;
  }
  if (v2 < v1){
    state = 2;
  }
}