V2.3

// ----------- ALTERAR ----------- //
int tamanhoLabirintoH = (2 * 4) + 1; //TAMANHO HORIZONTAL DO LABIRINTO
int tamanhoLabirintoV = (2 * 3) + 1; //TAMANHO VERTICAL DO LABIRINTO
// ----------- ALTERAR ----------- //

// Definir motores
#define motor1T 6
#define motor1R 31
#define motor2T 10
#define motor2R 28
#define motor3T 9
#define motor3R 27
#define motor4T 8
#define motor4R 26

//Definir ultrassons
#define trigPin1 40
#define echoPin1 41
#define trigPin2 34
#define echoPin2 35
#define trigPin3 36
#define echoPin3 37
#define trigPin4 38
#define echoPin4 39
#define toleranciaUltra 65

//Guardar o valor dos ultrassons
int ultraD; // ULTRA DA DIREITA
int ultraE; // ULTRA DA ESQUERDA
int ultra3;
int ultra4;

// --- VETORES - ARRAYS --- //
char labirintoFir[7][9]; //VERTICAL E HORIZONTAL
//PRIMEIRO NÍVEL
/*  Ajustar de acordo com o tamanho Horizontal e Vertical do labirinto
ACIMA.
Cria a matriz vetorial, cada vetor representa um espaço que tenha ou não
parede,
no caso, um labirinto de tamanho 3 irá ter 4 paredes (3 + 1). Essa é a
razão do
"2 * x + 1"
*/
//VARIAVÉIS VARIÁVEIS

//Definir
#define Vertical   0
#define Horizontal 1
#define Direita    0
#define Esquerda   1

#define Frente   0
#define Tras     1
#define Esquerda 3
#define Direita  4

//Definir posição do robô
int posicaoRobo[2] = {
tamanhoLabirintoV-2, tamanhoLabirintoH-2};

void setup(){
// --- Definir motores --- //
//Relays
pinMode(motor1R, OUTPUT);
pinMode(motor2R, OUTPUT);
pinMode(motor3R, OUTPUT);
pinMode(motor4R, OUTPUT);
//Transistors
pinMode(motor1T, OUTPUT);
pinMode(motor2T, OUTPUT);
pinMode(motor3T, OUTPUT);
pinMode(motor4T, OUTPUT);

// --- Definir ultrassons --- //
//Triggers
pinMode (trigPin1, OUTPUT);

pinMode (trigPin2, OUTPUT);
pinMode (trigPin4, OUTPUT);
pinMode (trigPin3, OUTPUT);
//Echoes
pinMode (echoPin1, INPUT);
pinMode (echoPin2, INPUT);
pinMode (echoPin3, INPUT);
pinMode (echoPin4, INPUT);

//Inicializar o robô - colocar o robô no canto inferior direito da
matriz vetorial
labirintoFir[tamanhoLabirintoV-2][tamanhoLabirintoH-2] = 'W'; //
Tamanho-2 é para compensar o fato do vetor começar do 0 (-1), e estar
situado depois de uma parede (-1)
labirintoFir[tamanhoLabirintoV-1][tamanhoLabirintoH-2] = 'P'; // O
quadrado logo abaixo do robô é considerado uma parede, apesar de estar
vazio

Serial.begin(9600);
}

void loop(){
/*ultraD = medirDistanciaCM(trigPin1, echoPin1);
delay(100);
ultraE = medirDistanciaCM(trigPin2, echoPin2); */

alinharRoboFrontal();

/*ultra3 = medirDistanciaCM(trigPin3, echoPin3);
if (ultra3 <= 7){
labirintoFir[posicaoRobo[Vertical]-1][posicaoRobo[Horizontal]] = 'P';
} else {
labirintoFir[posicaoRobo[Vertical]-1][posicaoRobo[Horizontal]] = 'W';
}
if (ultraD <= 7){
labirintoFir[posicaoRobo[Vertical]][posicaoRobo[Horizontal]+1] = 'P';
} else {
labirintoFir[posicaoRobo[Vertical]][posicaoRobo[Horizontal]+1] = 'W';
}
if (ultraE <= 7){
labirintoFir[posicaoRobo[Vertical]][posicaoRobo[Horizontal]-1] = 'P';
} else {
labirintoFir[posicaoRobo[Vertical]][posicaoRobo[Horizontal]-1] = 'W';
}

delay(1000);
Serial.print("\n");
for (int i = 0; i <=6; i++){
for (int x = 0; x <=8; x++){
if (labirintoFir[i][x] == 'W' || labirintoFir[i][x] == 'P'){
Serial.print(labirintoFir[i][x]);
} else {
Serial.print(x);
}
if (x == 8){
Serial.print("\n");
}
}
}*/
}

// --- Medir distância ultrassom em cm --- //
int medirDistanciaCM(int trigPin, int echoPin){
int duration;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin,HIGH);
return duration/29/2;
delay(500);
}

// --- Medir tempo em ms ultrassom --- //
int medirDistancia(int trigPin, int echoPin){
int duration;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin,HIGH);
return duration;
delay(500);
}

void mover(boolean dirA, boolean dirB, int pot){
digitalWrite(motor1R, dirA);
digitalWrite(motor3R, dirA);
digitalWrite(motor2R, dirB);
digitalWrite(motor4R, dirB);

switch (pot) {
case 1: //Normal
analogWrite (motor1T, 228);
analogWrite (motor2T, 250);
analogWrite (motor3T, 190);
analogWrite (motor4T, 250);
break;
case 2: //Rampa
analogWrite (motor1T, 240);
analogWrite (motor2T, 255);
analogWrite (motor3T, 220);
analogWrite (motor4T, 255);
break;
case 3: //Direita
analogWrite (motor1T, 120);
analogWrite (motor2T, 127);
analogWrite (motor3T, 110);
analogWrite (motor4T, 127);
break;
case 4: //Esquerda
analogWrite (motor1T, 200);
analogWrite (motor2T, 255);
analogWrite (motor3T, 200);
analogWrite (motor4T, 255);
case 5: //Girar
analogWrite (motor1T, 114);
analogWrite (motor2T, 125);
analogWrite (motor3T, 95);
analogWrite (motor4T, 125);
break;
default:
analogWrite (motor1T, 0);
analogWrite (motor2T, 0);
analogWrite (motor3T, 0);
analogWrite (motor4T, 0);
}
}

void parar(){
boolean dirA = digitalRead(motor1R);
boolean dirB = digitalRead(motor2R);
digitalWrite(motor1R, !dirA);
digitalWrite(motor3R, !dirA);
digitalWrite(motor2R, !dirB);
digitalWrite(motor4R, !dirB);
delay(25);
analogWrite (motor1T, 0);
analogWrite (motor2T, 0);
analogWrite (motor3T, 0);
analogWrite (motor4T, 0);
}

boolean alinharRoboLateral(){
return true;
ultraD = medirDistancia(trigPin3, echoPin3);
delay(100);
ultraE = medirDistancia(trigPin4, echoPin4);
delay(100);

if (ultraD < ultraE + toleranciaUltra && ultraD > ultraE -
toleranciaUltra){
ultra3 = ultra4;
}

if (ultraD > ultraE){
mover(Frente, Frente, Esquerda);
return false;
}
else if (ultraD < ultraE){
mover(Frente, Frente, Direita);
return false;
}
}

boolean alinharRoboFrontal(){
ultra3 = medirDistancia(trigPin3, echoPin3);
Serial.println(ultra4);
delay(100);
ultra4 = medirDistancia(trigPin4, echoPin4);
Serial.println(ultra4);
delay(100);

if (ultra3 < ultra4 + toleranciaUltra && ultra3 > ultra4 -
toleranciaUltra){
ultra3 = ultra4;
}

if (ultra3 == ultra4){
mover(Frente, Frente, 5564654);
}
else if (ultra3 > ultra4){
mover(Frente, Tras, 5);
}
else if (ultra3 < ultra4){
mover(Tras, Frente, 5);
}
}