// seguidor de linha
#include <AFMotor.h>
AF_DCMotor motor_esq(1); //Seleciona o motor 1
AF_DCMotor motor_dir(4); //Seleciona o motor 4
int SENSOR1, SENSOR2, SENSOR3;
//deslocamentos de calibracao
int leftOffset = 0, rightOffset = 0, centre = 0;
//pinos para a velocidade e direcao do motor
int speed1 = 3, speed2 = 11, direction1 = 12, direction2 = 13;
//velocidade inicial e deslocamento de rotacao
int startSpeed = 70, rotate = 30;
//limiar do sensor
int threshold = 5;
//velocidades iniciais dos motores esquerdo e direito
int left = startSpeed, right = startSpeed;
//sensor de linha
#define left //= 22
#define center 24
#define right //= 26
//SENSOR DE COR
int s0 = 30;
int s1 = 32;
int s2 = 34;
int s3 = 36;
int out = 38;
//Variaveis cores
int red = 0;
int green = 0;
int blue = 0;
// sensor ultrasónico
//Trig e Echo pin
#define trigPin 7
#define echoPin 6
//Definir variaveis ( sensor ultrasónico)
long duration;
int distance;
const int distanceMin=30; //DISTÂNCIA MINIMA EM cm
//Motor A (ESQUERDA)
int ENA = 8;
int IN1 = 9;
int IN2 = 10;
int IN3 = 4;
int IN4 = 5;
int ENB = 3;
int velocidade = 200;
//seguidor de linha
void calibrate()
{
for (int x=0; x<10; x++) //Executa 10 vezes para obter uma media
{
delay(100);
SENSOR1 = analogRead(0);
SENSOR2 = analogRead(1);
SENSOR3 = analogRead(2);
leftOffset = leftOffset + SENSOR1;
centre = centre + SENSOR2;
rightOffset = rightOffset + SENSOR3;
delay(100);
}
//obtem a media para cada sensor
leftOffset = leftOffset /10;
rightOffset = rightOffset /10;
centre = centre / 10;
//calcula os deslocamentos para os sensores esquerdo e direito
leftOffset = centre - leftOffset;
rightOffset = centre - rightOffset;
}
//DISTÂNCIA LIMITE PARA OS MOTORES PARAR (cm)
void setup() {
//seguidro de linha
calibrate();
delay(3000);
Serial.begin(9600); // COMEÇANDO Serial Terminal
// SENSOR DE COR
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
digitalWrite(s0, HIGH);
digitalWrite(s1, LOW);
//Set motors as outputs
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
// OUTPUT E INPUT DOS SENSOR ULTRASÓNICO
pinMode(trigPin, OUTPUT); // Frontal
pinMode(echoPin, INPUT); // Frontal
//BUTTON AND LED PINS
pinMode(12,OUTPUT); //PINO DO LED
pinMode(13,OUTPUT); //PINO DO LED
}
void loop() {
//seguidor de linha
{
//utiliza a mesma velocidade em ambos os motores
left = startSpeed;
right = startSpeed;
//le os sensores e adiciona os deslocamentos
SENSOR1 = analogRead(0) + leftOffset;
SENSOR2 = analogRead(1);
SENSOR3 = analogRead(2) + rightOffset;
//Se SENSOR1 for maior do que o sensor do centro + limiar,
// vire para a direita
if (SENSOR1 > SENSOR2+threshold)
{
left = startSpeed + rotate;
right = startSpeed - rotate;
}
//Se SENSOR3 for maior do que o sensor do centro + limiar,
// vire para a esquerda
if (SENSOR3 > (SENSOR2+threshold))
{
left = startSpeed - rotate;
right = startSpeed + rotate;
}
//Envia os valores de velocidade para os motores
motor_esq.setSpeed(left);
motor_esq.run(FORWARD);
motor_dir.setSpeed(right);
motor_dir.run(FORWARD);
}
//Deteta a cor (SENSOR DE COR)
Serial.print("Vermelho :");
Serial.print(red, DEC);
Serial.print(" Verde : ");
Serial.print(green, DEC);
Serial.print(" Azul : ");
Serial.print(blue, DEC);
Serial.println();
delay(200);
digitalWrite(s2, LOW);
digitalWrite(s3, LOW);
//Ler a cor vermelha
red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
digitalWrite(s3, HIGH);
//Ler a cor azul
blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
digitalWrite(s2, HIGH);
//Ler a cor verde
green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);
// SET UP SENSOR ULTRASÓNICO
long duracion, distancia;
digitalWrite(trigPin,LOW);
delayMicroseconds(2);
digitalWrite(trigPin,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,LOW);
duracion=pulseIn(echoPin,HIGH);
distancia=duracion/58;
Serial.println(String(distancia)+"cm(front)");
delay(100);
//CONTROLO DO MOTOR - MOTOR A (ESQUERDA) : motorPin1, motorPin2 & MOTOR B (DIREITA) : motorpin3, motorPin4
if (distancia<distanceMin) {
// ESTE CÓDICO VAI PARAR OS MOTORES QUANDO A DISTANCIA FOR MENOR DO QUE A DISTANCIA MINIMA (LIMITE)
//digitalWrite(IN1, LOW);
// digitalWrite(IN2, LOW);
//digitalWrite(IN3, LOW);
// digitalWrite(IN4, LOW);
//Para o motor A
digitalWrite(IN1, HIGH);
digitalWrite(IN2, HIGH);
//Para o motor B
digitalWrite(IN3, HIGH);
digitalWrite(IN4, HIGH);
digitalWrite(13,LOW); //DESLIGA O LED
digitalWrite(12,HIGH); //LIGA O LED
delay(3000);//TEMPO 3 SEGUNDOS
}
// OS MOTORES COMEÇAM QUANDO A DISTÂNCIA FOR MAIOR DO QUE A DISTÂNCIA MINIMA(LITIME)
else {
digitalWrite(12,LOW);//O LED DESLIGA
digitalWrite(13,HIGH);
//ESTE CÓDICO FAZ OS MOTORES TRABALHAREM NO SENTIDO DOS PONTEIROS DO RELÓGIO
//digitalWrite(IN1, LOW);
//digitalWrite(IN2, HIGH);
//digitalWrite(IN3, LOW);
//digitalWrite(IN4, HIGH);
//Gira o Motor A no sentido horario
analogWrite(ENA, velocidade);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
//Gira o Motor B no sentido horario
analogWrite(ENB, velocidade);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
}