Projeto 4WD - Final

#include <AFMotor.h>
#define ft 1
#define ts 2

int sense1 = 52;
int sense2 = 53;

int valor1 = 0;
int valor2 = 0;

int ult = 0;

int s0_1 = 22;
int s1_1 = 24;
int s2_1 = 26;
int s3_1 = 28;
int out_1 = 30;

int red_1 = 0;
int green_1 = 0;
int blue_1 = 0;

int s0_2 = 31;
int s1_2 = 33;
int s2_2 = 35;
int s3_2 = 37;
int out_2 = 39;

int red_2 = 0;
int green_2 = 0;
int blue_2 = 0;

AF_DCMotor m1(1);
AF_DCMotor m2(2);
AF_DCMotor m3(3);
AF_DCMotor m4(4);

void setup() {
m1.setSpeed(255);
m2.setSpeed(255);
m3.setSpeed(255);
m4.setSpeed(255);
pinMode(sense1, INPUT);
pinMode(sense2, INPUT);
pinMode(s0_1, OUTPUT);
pinMode(s1_1, OUTPUT);
pinMode(s2_1, OUTPUT);
pinMode(s3_1, OUTPUT);
pinMode(out_1, INPUT);
pinMode(s0_2, OUTPUT);
pinMode(s1_2, OUTPUT);
pinMode(s2_2, OUTPUT);
pinMode(s3_2, OUTPUT);
pinMode(out_2, INPUT);
digitalWrite(s0_1, HIGH);
digitalWrite(s1_1, LOW);
digitalWrite(s0_2, HIGH);
digitalWrite(s1_2, LOW);
delay(1000);
Serial.begin(9600);
}

void loop() {

  valor1 = digitalRead(sense1);
  valor2 = digitalRead(sense2);

  if((valor1==HIGH) && (valor2==HIGH)){

     cor1();
      cor2();

    if ((green_1 < red_1) && (blue_1 < green_1) && (blue_1>75)){
      direitaCor();
      delay(700);
    } else {
        if ((red_2 < green_2) && (blue_2 < red_2) && (blue_2>50)){
          esquerdaCor();
          delay(700);
        } else {
          ult = 0;
          frente();

        }
    }
   /* ult = 0;
          frente();*/
  }

  if((valor1==HIGH) && (valor2==LOW)){
    ult = 1;
    direita();
  }

  if((valor1==LOW) && (valor2==HIGH)){
    ult = 2;
    esquerda();
  }

  if((valor1==LOW) && (valor2==LOW)){
    if (ult != 0){
      if (ult == 1)
        direita();
      if (ult == 2)
        esquerda();
  }
    else{
      giro180();
    }
  }
}

void frente() {
  m1.setSpeed(225);
  m2.setSpeed(225);
  m3.setSpeed(225);
  m4.setSpeed(225);
  m1.run(ft);
  m2.run(ft);
  m3.run(ft);
  m4.run(ft);
 }

void giro180() {
  m1.setSpeed(160);
  m2.setSpeed(160);
  m3.setSpeed(160);
  m4.setSpeed(160);
  m1.run(ft);
  m2.run(ft);
  m3.run(ts);
  m4.run(ts);
 }

void esquerda() {
  m1.setSpeed(225);
  m2.setSpeed(225);
  m3.setSpeed(225);
  m4.setSpeed(225);
  m1.run(ft);
  m2.run(ft);
  m3.run(ts);
  m4.run(ts);
 }

  void direita() {
  m1.setSpeed(225);
  m2.setSpeed(225);
  m3.setSpeed(255);
  m4.setSpeed(255);
  m1.run(ts);
  m2.run(ts);
  m3.run(ft);
  m4.run(ft);
 }

 void esquerdaCor() {
  m1.setSpeed(255);
  m2.setSpeed(255);
  m3.setSpeed(100);
  m4.setSpeed(100);
  m1.run(ft);
  m2.run(ft);
  m3.run(ts);
  m4.run(ts);
 }

  void direitaCor() {
  m1.setSpeed(100);
  m2.setSpeed(100);
  m3.setSpeed(255);
  m4.setSpeed(255);
  m1.run(ts);
  m2.run(ts);
  m3.run(ft);
  m4.run(ft);
 }

 void cor1() {
    digitalWrite(s2_1, LOW);
    digitalWrite(s3_1, LOW);
    red_1 = pulseIn(out_1, digitalRead(out_1) == HIGH ? LOW : HIGH);


    digitalWrite(s3_1, HIGH);
    blue_1 = pulseIn(out_1, digitalRead(out_1) == HIGH ? LOW : HIGH);

    digitalWrite(s2_1, HIGH);
    green_1 = pulseIn(out_1, digitalRead(out_1) == HIGH ? LOW : HIGH);
}

void cor2() {
    digitalWrite(s2_2, LOW);
    digitalWrite(s3_2, LOW);
    red_2 = pulseIn(out_2, digitalRead(out_2) == HIGH ? LOW : HIGH);

    digitalWrite(s3_2, HIGH);
    blue_2 = pulseIn(out_2, digitalRead(out_2) == HIGH ? LOW : HIGH);

    digitalWrite(s2_2, HIGH);
    green_2 = pulseIn(out_1, digitalRead(out_2) == HIGH ? LOW : HIGH);
}