#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 LEFT_MAX_SPEED 125
#define RIGHT_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 turnOff(int del = 0);
void turnRight(int del);
void turnLeft(int del);
void turnForward(int del);
void turnBackward(int del);

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);
  
  turnOff();
//
//  turnForward(3000);
//  turnLeft(700);
//  turnForward(3000);

  turnOff();  
}

  // Serial.println(String(analogRead(LEFT_SENSOR)) + " " + String(analogRead(RIGHT_SENSOR)));
void loop() {

  turnForward(2000);
  turnLeft(850);
  
}


void turnOff(int del = 0) {
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, 0);
  delay(del);
}

void turnRight(int del){
  analogWrite(LEFT_FORWARD, LEFT_MAX_SPEED);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, RIGHT_MAX_SPEED);
  analogWrite(RIGHT_FORWARD, 0);
  delay(del);
  turnOff();
}


void turnLeft(int del){
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, LEFT_MAX_SPEED);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, RIGHT_MAX_SPEED);
  delay(del);
  turnOff();
}


void turnForward(int del){
  analogWrite(LEFT_FORWARD, LEFT_MAX_SPEED);
  analogWrite(LEFT_BACKWARD, 0);
  analogWrite(RIGHT_BACKWARD, 0);
  analogWrite(RIGHT_FORWARD, RIGHT_MAX_SPEED);
  delay(del);
  turnOff();
}


void turnBackward(int del){
  analogWrite(LEFT_FORWARD, 0);
  analogWrite(LEFT_BACKWARD, LEFT_MAX_SPEED);
  analogWrite(RIGHT_BACKWARD, RIGHT_MAX_SPEED);
  analogWrite(RIGHT_FORWARD, 0);
  delay(del);
  turnOff();
}