#include <Servo.h>
int pos = 0;
int i = 0;
int var1 = 0;
float coef = 0.7;
float coef2 = 0.7;
int ii = 2000;
int cs1 = 50;
int cs2 = 50;
Servo myservo;


void motor(int power)
{
  if (power < 0)
  {
    analogWrite(5, power * -1);
    analogWrite(10, power * -1);
    analogWrite(6, 0);
    analogWrite(3, 0);
  }
  else
  {
    analogWrite(5, 0);
    analogWrite(10, 0);
    analogWrite(6, power);
    analogWrite(3, power);
  }
}

void dvig(int dest) {
  if (dest > 135) {
    dest = 135;
  }
  if (dest < 75) {
    dest = 75;
  }
  if (pos < dest) {
    for (i = pos; i <= dest; i += 1) {

      myservo.write(i);
      //delay();
    }
  }
  else
  { for (i = pos; i >= dest; i -= 1)
    {
      myservo.write(i);
      delay(1 );
    }
  }
  pos = dest;
}

void prop2() {
  int cs1 = map(analogRead(A4), 570, 680, 0, 100);
  int cs2 = map(analogRead(A5), 570 , 670, 0, 100);
  int error = cs1 - cs2;
  Serial.print(cs1);
  Serial.print("   ");
  Serial.println(cs2);
  int regsig = error * coef2;
  dvig(90 - regsig);
}

void propL() {
  int cs1 = map(analogRead(A4), 530, 660, 0, 100);
  //  Serial.println(cs1);
  int error = cs1 - 40;
  int regsig = error * coef;
  dvig(90 - regsig);

}

void propR() {
  int cs1 = map(analogRead(A5), 520, 640, 0, 100);
  //  Serial.print(cs2);
  int error = 50 - cs2;
  int regsig = error * coef;
  dvig(90 - regsig);
}



void per() {
  while (not (map(analogRead(A4), 530, 660, 0, 100) < 20) and (map(analogRead(A5), 520, 640, 0, 100)) < 20)
  {
    prop2();
  }
  dvig(90);
  delay(250);
}

void lev() {
  for (int x = 0; x < 1500; x++) {
    propL();
  }
}
void prav() {
  for (int x = 0; x < 500; x++) {
    propR();
  }
}


void setup() {
  myservo.attach(11);
  pinMode(12, INPUT);
  pinMode(13, INPUT);
  pinMode(5, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(3, OUTPUT);
  //  delay(3000);
  Serial.begin(9600);
  //  Serial.println("Hello,World");

  motor(100);
  per();
  motor(0);
  // Калибровка датчиков отраженного света
  //  motor(0);
  //  while (true) {
  //    int cs1 = map(analogRead(A4), 570, 680, 0, 100);
  //    int cs2 = map(analogRead(A5), 570, 670, 0, 100);
  //    Serial.print(cs1);
  //    Serial.print("    ");
  //    Serial.print(cs2);
  //    Serial.print("    ");
  //    Serial.print(analogRead(A4));
  //    Serial.print("    ");
  //    Serial.println(analogRead(A5));
  //    prop2();
  //
  //  }

}

void loop() {
  delay(2000);
  Serial.println("loop");
  prop_T();
  motor(0);
}