#define LEFT_FORWARD 3    //едет вперёд левое
#define LEFT_BACKWARD 5   //назад левое
#define RIGHT_FORWARD 6   //назад едет правое
#define RIGHT_BACKWARD 11 //вперёд

#define LEFT_SENSOR A0
#define RIGHT_SENSOR A7

#define MAX_SPEED 150
#define NULL_SPEED 0

float Kp, Ki, Kd;
float integral, pError;

float computePID(float setpoint, float input, float dt) {
    float error = setpoint - input;
    float derivative = (error - pError) / dt;
    pError = error;
    integral += error * dt;
    return error * Kp + integral * Ki + derivative * Kd;
}

void setup() {
  Serial.begin (9600);
  pinMode (LEFT_FORWARD, OUTPUT);
  pinMode (LEFT_BACKWARD, OUTPUT);
  pinMode (RIGHT_FORWARD, OUTPUT);
  pinMode (RIGHT_BACKWARD, OUTPUT);

  pinMode (LEFT_SENSOR, INPUT);
  pinMode (RIGHT_SENSOR, INPUT);
  
  analogWrite(RIGHT_FORWARD, 0);
  analogWrite(LEFT_FORWARD, 0);

}

  // Serial.println(String(analogRead(LEFT_SENSOR)) + " " + String(analogRead(RIGHT_SENSOR)));
void loop() {
  /*for (int i = 0; i < 1000; ++i) {
    analogWrite(LEFT_FORWARD, map(i, 0, 999, 0, 200));
    analogWrite(RIGHT_FORWARD, map(i, 0, 999, 0, 100));
    delay(1);
  }*/
  analogWrite(RIGHT_FORWARD, 255);
  analogWrite(LEFT_FORWARD, 255);
  delay(10);
  
  analogWrite(RIGHT_FORWARD, 100);
  analogWrite(LEFT_FORWARD, 150);


  for (int i = 0; i < 500; ++i) {
    analogWrite(LEFT_FORWARD, 200);
    analogWrite(RIGHT_FORWARD, 100);
    delay(9);
    analogWrite(LEFT_FORWARD, 150);
    analogWrite(RIGHT_FORWARD, 110);
    delay(1);
  }
  
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(RIGHT_FORWARD, 0);
  
  delay(2000);
}

void turnRight(){
  analogWrite(LEFT_FORWARD, MAX_SPEED);
  analogWrite(RIGHT_BACKWARD, MAX_SPEED);
}


void turnLeft(){
  analogWrite(LEFT_BACKWARD, MAX_SPEED);
  analogWrite(RIGHT_FORWARD, MAX_SPEED);
}