parcialV2

#include <Servo.h>

/*

   SENSORES
   Entradas analogas A0 a A8

   CONTROL SERVO
   Pin 9

   SALIDAS MOTOR 1
   Adelante - Pin 10
   Atras - Pin 11
   PWM - Pin 6

   SALIDAS MOTOR 2
   Adelante - Pin 12
   Atras - Pin 13
   PWM - Pin 7

*/

// Servo
Servo direccion;
int cabrilla = 9;

// Motor 1 - Izquierdo
int adelante1 = 10;
int atras1 = 11;
int pwm1 = 6;

// Motor 2 - Derecho
int adelante2 = 12;
int atras2 = 13;
int pwm2 = 7;

// Sensores
int s1 = 0;
int s2 = 1;
int s3 = 2;
int s4 = 3;
int s5 = 4;
int s6 = 5;
int s7 = 6;
int s8 = 7;
int s9 = 8;

// Variables
int vel = 255 * 0.45;
int curva = 255 * 0.40;
int limites[18];

int estado;

void setup() {
  Serial.begin(9600);
  // Auto Calibracion:
  for (int i = 0; i < 9; i++) {
    int sensor = analogRead(i);
    limites[i * 2] = sensor - 8;
    limites[i * 2 + 1] = sensor + 8;
    delay(500);
  }

  delay(500);

  // Calibracion Cabrilla:
  direccion.attach(cabrilla);
  direccion.write(110);
  delay(1000);
  direccion.write(70);
  delay(1000);
  direccion.write(90);
  delay(1500);

  // Paso adelante
  direccion.write(90);

  digitalWrite(adelante1, HIGH);
  analogWrite(pwm1, 100);

  digitalWrite(adelante2, HIGH);
  analogWrite(pwm2, 100);

  estado = 0;
}

void parar() {
  digitalWrite(adelante1, LOW);
  digitalWrite(atras1, LOW);
  analogWrite(pwm1, 0);

  digitalWrite(adelante2, LOW);
  digitalWrite(atras2, LOW);
  analogWrite(pwm2, 0);
  delay(100);
}

void adelante() {
  // parar();
  direccion.write(90);

  digitalWrite(adelante1, HIGH);
  analogWrite(pwm1, vel);

  digitalWrite(adelante2, HIGH);
  analogWrite(pwm2, vel);
}

void atras() {
  // parar();
  digitalWrite(atras1, HIGH);
  analogWrite(pwm1, vel);

  digitalWrite(atras2, HIGH);
  analogWrite(pwm2, vel);
}

void girarIzq(int grados) {
  // parar();
  digitalWrite(atras1, HIGH);
  digitalWrite(adelante1, LOW);
  analogWrite(pwm1, curva);
  digitalWrite(adelante2, HIGH);
  digitalWrite(atras2, LOW);
  analogWrite(pwm2, curva);

  direccion.write(grados);
}

void girarDer(int grados) {
  // parar();
  digitalWrite(atras2, HIGH);
  digitalWrite(adelante2, LOW);
  analogWrite(pwm2, curva);
  digitalWrite(adelante1, HIGH);
  digitalWrite(atras1, LOW);
  analogWrite(pwm1, curva);

  direccion.write(grados);
}

void loop() {

  s1 = analogRead(A0);
  s2 = analogRead(A1);
  s3 = analogRead(A2);
  s4 = analogRead(A3);
  s5 = analogRead(A4);
  s6 = analogRead(A5);
  s7 = analogRead(A6);
  s8 = analogRead(A7);
  s9 = analogRead(A8);

  //Adelante
  if (s5 > limites[8] && s2 < limites[9]) {
    if (estado == 0) {
      adelante();
    } else {
      parar();
      adelante();
      estado = 0;
    }
  }

  //Giros Izquierda
  else if (s1 > limites[0] && s1 < limites[1]) {
    if (estado == 1) {
      girarIzq(120);
    } else {
      parar();
      girarIzq(120);
      estado = 1;
    }
  } else if (s2 > limites[2] && s2 < limites[3]) {
    if (estado == 1) {
      girarIzq(110);
    } else {
      parar();
      girarIzq(110);
      estado = 1;
    }
  } else if (s2 > limites[4] && s2 < limites[5]) {
    if (estado == 1) {
      girarIzq(100);
    } else {
      parar();
      girarIzq(100);
      estado = 1;
    }
  } else if (s3 > limites[6] && s3 < limites[7]) {
    if (estado == 1) {
      girarIzq(100);
    } else {
      parar();
      girarIzq(100);
      estado = 1;
    }
  }

  //Giros derecha
  else if (s6 > limites[10] && s6 < limites[11]) {
    if (estado == 2) {
      girarDer(85);
    } else {
      parar();
      girarDer(85);
      estado = 2;
    }
  } else if (s7 > limites[12] && s7 < limites[13]) {
    if (estado == 2) {
      girarDer(80);
    } else {
      parar();
      girarDer(80);
      estado = 2;
    }
  } else if (s8 > limites[14] && s8 < limites[15]) {
    if (estado == 2) {
      girarDer(70);
    } else {
      parar();
      girarDer(70);
      estado = 2;
    }
  } else if (s9 > limites[16] && s9 < limites[17]) {
    if (estado == 2) {
      girarDer(60);
    } else {
      parar();
      girarDer(60);
      estado = 2;
    }
  }

  else {
    parar();
  }
}