Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Código para robô seguidor de linha com desvio de obstáculo , utilizando Arduino.
/*
* Este código faz parte do projeto Robô industrial. Acesse http://roboticaufopa.blogspot.com.br/ e saiba mais sobre o mesmo.
* Considera que os dois LDRs do lado de fora da linha.
* Considera o sensor IR no meio da linha preta
* Versão completa, comcontrole com controle via Bluetooth e a sincronizçaão entre o modo autônomo e o seguidor de linha.
*/
//BIBLIOTECA(S)
#include <Servo.h> // Incluindo biblioteca de controle do servo motor.
//SENSORES DE LUMINOSIDADE
const int LdrEsquerda = A1;
const int LdrDireita = A0;
int SensorIR = 10;
//DEMAIS COMPONENTES EM PORTAS DIGITAIS
#define TrigPin 12 //Envia o pulso - Sensor Ultrassônico
#define EchoPin 13 // Lê o pulso- Sensor Ultrassônico
const int ServoMotorPin = 3; //Setando o servo motor da porta 3 (PWM)
const int LedEstado = 8; // Led que indica se o bluetooth está ou não ligado
//MOTORES EM PORTAS PWM
int VelMotorEsq = 5; //Velocidade motor esquerda
int VelMotorDir = 6; //Velocidade motor direita
int DirecaoMotorEsq = 4; //Diraçao motor esquerda
int DirecaoMotorDir = 7; //Diraçao motor esquerda
//VARIÁVEIS GLOBAIS
float _ValorLDREsquerda = 0;
float _ValorLDRDireta = 0;
int _ValorIR = 0;
int _Contador = 0;
int _VetorSensorIR[100];
int _VetorLdrEsq[100];
int _VetorLdrDir[100];
int _VoltaParaEsquerda; //Utilizado na função VoltaParaOcaminho
int _VoltaParaDireita; //Utilizado na função VoltaParaOCaminho
int _DistanciaObstaculo; //Armazena a distância do obstáculo.
int _Frente; //Armazeza a distância do obstáculo à frente
int _Direita; //Armazena a distância do obstáculo à direita.
int _Esquerda; //Armazena a distância do obstáculo à esquerda.
int _EstadoLinha = 0; //Armazena o estado da linha. Ou seja, se há linha ou não.
Servo MeuServoMotor; //Criando um objeto da classe Servo
int EstadoBotao1 = 0; //LED que acende quando o modo autônomo está ativado.
char _Val; //Armazena os caracteres enviados pela portal Serial
int ContadorUltrassonico = 0; //Contador utilizado no , para evitar que o sensor ultrassônico seja lido muitas vezes seguida
int dly = 700; //Tamanho de delay
void setup() {
Serial.begin(9600); //Inicia a comunicação com a portal serial
MeuServoMotor.attach(ServoMotorPin);
pinMode(VelMotorEsq, OUTPUT);
pinMode(DirecaoMotorDir, OUTPUT);
pinMode(VelMotorDir, OUTPUT);
pinMode(VelMotorEsq, OUTPUT);
pinMode(LedEstado, OUTPUT); //Seta pino de saída (LED)
pinMode(LdrDireita, INPUT);
pinMode(LdrEsquerda, INPUT);
pinMode(SensorIR, INPUT);
pinMode(TrigPin, OUTPUT);
pinMode(EchoPin, INPUT);
}
void loop() {
//MODO BLUETOOTH
if(EstadoBotao1 == 0)
{
digitalWrite(LedEstado, LOW); //Mantém o bluetooth apagado.
// Verifica se chegou algum caractere pela porta serial.
if(Serial.available())
{
_Val = Serial.read(); //Se houve a entrada de dado (pela porta serial) este será armazenado na variável _Val
switch(_Val)
{
case 'v':
EstadoBotao1 = 0;
stop();
break;
case 'V':
EstadoBotao1 = 1;
break;
case 'F': //Anda para Frente
AndaFrente(255,255);
Serial.println("Anda Frente");
break;
case 'B': //Marcha Ré
MarchaRe(250, 250);
break;
case 'L': //Vira a Esquerda
Esquerda(250, 250);
break;
case 'G': //Vira a Esquerda
Esquerda(250, 250);
break;
case 'H': //Vira a Esquerda
Esquerda(250, 250);
break;
case 'R': //Vira a direita
Direita(250, 250);
break;
case 'I': //Vira a direita
Direita(250, 250);
break;
case 'J': //Vira a direita
Direita(250, 250);
break;
case 'S':
stop();
break;
}//Fim do Switch
}//Fim do IF
} //Fim do IF do Estado do Botão
//MODO AUTÔNOMO
else if(EstadoBotao1 == 1)
{
//ENTRADA DE DADOS VIA PORTA SERIAL
if(Serial.available())
{
_Val = Serial.read();
switch(_Val)
{
case 'v':
EstadoBotao1 = 0;
stop();
break;
case 'V':
EstadoBotao1 = 1;
break;
}
}
if(EstadoBotao1 == 1)
{
digitalWrite(LedEstado, HIGH); //Acende o LED quando o robô entrar no modo autônomo.
Anda();
} //Fim do if do EstadoDoBotao
} //Fim do IfAvaliable
}
//Calcula a distância frontal e dos lados direito e esquerdo.
void RetornaDistancias() {
DistanciaFrente();
delay(120);
DistanciaDireita();
delay(120);
DistanciaEsquerda();
delay(120);
ServoMotorFrente();
delay(50);
}
void Anda(){
SensorUltrassonico(); //Faz a leitura do sensor ultrassônico
/* Se não houver obstáculo à menos de 45 cm de distância, o robô irá seguir a linha */
if (_DistanciaObstaculo < 45) {
_EstadoLinha = 0;
Obstaculo = 1;
_Contador = 0;
DesvioObstaculo(); //Realiza o desvio de obstáculo
}
else {
while(ContadorUltrassonico < 20){
SegueLinha();
ContadorUltrassonico++;
}
ContadorUltrassonico=0;
}
}
void DesvioObstaculo(){
stop();
delay(1000);
Direita(255, 255);
delay(dly);
AndaFrente(255, 255);
delay(1000);
Esquerda(255, 255);
delay(dly);
AndaFrente(255, 255);
delay(1000);
Esquerda(255, 255);
delay(dly);
while(_EstadoLinha != 1){
AndaFrente(100, 100);
RetornaEstadoLinha();
}
AndaFrente(100, 100);
delay(300);
Direita(255, 255);
delay(dly/2);
while(_EstadoLinha != 1){
AndaFrente(100, 200);
RetornaEstadoLinha();
}
SegueLinha();
}
int SensorUltrassonico() //Faz o disparo de pulsos e retorna o tempo em microsegundos
{
digitalWrite(TrigPin, LOW);
delay(2);
digitalWrite(TrigPin, HIGH);
delay(10);
digitalWrite(TrigPin, LOW);
unsigned long Duracao = pulseIn(EchoPin, HIGH);
delay(4);
_DistanciaObstaculo = Duracao / 58; //Acha a distancia em centímetros.
if (_DistanciaObstaculo > 3500) { //Corrige um erro do sensor ultrassônico, pois este, em alguns momentos, estava retornando erroneamente um valor acima de 3 mil.
SensorUltrassonico();
}
return _DistanciaObstaculo;
delay(10);
}
//Analisa a distancia dos três lados.
int DistanciaFrente() {
ServoMotorFrente();
delay(100);
_Frente = SensorUltrassonico();
Serial.print("O valor de Frente e: ");
Serial.println(_Frente);
return _Frente;
delay(100);
}
int DistanciaDireita() {
ServoMotorDireita();
delay(200);
_Direita = SensorUltrassonico();
Serial.print("O valor de DIREITA e: ");
Serial.println(_Direita);
return _Direita;
delay(100);
}
int DistanciaEsquerda() {
ServoMotorEsquerda();
delay(200);
_Esquerda = SensorUltrassonico();
Serial.print("O valor de ESQUERDA e: ");
Serial.println(_Esquerda);
return _Esquerda;
delay(100);
}
//CONTROLANDO SERVO MOTOR
void ServoMotorFrente() //Volta o Servo Motor para frente
{
MeuServoMotor.write(90); //Servo voltado para a frente
Serial.println("Servo Motor apontando para a Frente");
delay(150);
}
void ServoMotorDireita() //Volta o Servo Motor para Direita
{
MeuServoMotor.write(4); //Servo voltado para a Direita
Serial.println("Servo Motor apontando para a Direita");
delay(150);
}
void ServoMotorEsquerda() //Volta o Servo Motor para Esquerda
{
MeuServoMotor.write(185); //Servo voltado para a Esquerda
Serial.println("Servo Motor apontando para a Esquerda");
delay(150);
}
void stop(void)//stop
{
digitalWrite(VelMotorEsq,LOW);
digitalWrite(VelMotorDir,LOW);
}
void AndaFrente(char a,char b) //Anda Para Frente
{
analogWrite (VelMotorEsq,a); //PWM controle de velocidade
digitalWrite(DirecaoMotorEsq,HIGH);
analogWrite (VelMotorDir,b);
digitalWrite(DirecaoMotorDir,HIGH);
}
void MarchaRe (char a,char b) //Move para trás
{
analogWrite (VelMotorEsq,a);
digitalWrite(DirecaoMotorEsq,LOW);
analogWrite (VelMotorDir,b);
digitalWrite(DirecaoMotorDir,LOW);
}
void Direita (char a,char b) //Vira a esquerda
{
analogWrite (VelMotorEsq,a);
digitalWrite(DirecaoMotorEsq,LOW);
analogWrite (VelMotorDir,b);
digitalWrite(DirecaoMotorDir,HIGH);
}
void Esquerda (char a,char b){ //Vira a direita
analogWrite (VelMotorEsq,a);
digitalWrite(DirecaoMotorEsq,HIGH);
analogWrite(VelMotorDir,b);
digitalWrite(DirecaoMotorDir,LOW);
}
void LeiturasSensoresLum(){
_ValorIR = _VetorSensorIR[_Contador] = digitalRead(SensorIR);
_ValorLDREsquerda = _VetorLdrEsq[_Contador] = analogRead(LdrEsquerda);
_ValorLDRDireta = _VetorLdrDir[_Contador] = analogRead(LdrDireita);
}
void SegueLinha(){
LeiturasSensoresLum();
if((_ValorLDREsquerda>=1)&&(_ValorIR==1)&&(_ValorLDRDireta>=1)) //100% na linha
{
AndaFrente(255,255);
Serial.println("Anda Frente");
}
//LDR Esquerdo tocando na linha
else if ((_ValorLDREsquerda>=1)&&(_ValorIR==1)&&(_ValorLDRDireta<1)) //Saindo da linha pela esquerda
{
Direita(100,200);
Serial.println("Direita");
}
//LDR Direito tocando na linha
else if ((_ValorLDREsquerda<1)&&(_ValorIR==1)&&(_ValorLDRDireta>=1)) //Saindo pela direita
{
Esquerda(200,100);
Serial.println("Esquerda");
}
else if ((_ValorLDREsquerda>=1)&&(_ValorIR==0)&&(_ValorLDRDireta<1)) //Saindo MUITO pela esquerda
{
Direita(255,255);
Serial.println("Direita acentuada");
}
else if ((_ValorLDREsquerda<1)&&(_ValorIR==0)&&(_ValorLDRDireta>=1)) //Saindo muito pela direita
{
Esquerda(255,255);
Serial.println("Esquerda acentuada");
}
else if((_ValorLDREsquerda>0)&&(_ValorIR==0)&&(_ValorLDRDireta>0))
{
_Contador--; //Evita que o _Contador incremente, pois ele retorna para o _Contador anterior. Quando ele incrementar, ele voltara ao que era antes. e assim por diante.
VoltaParaAlinha(); //Chama o metodo capaz de fazer o robo voltar para linha
if(_VoltaParaEsquerda > _VoltaParaDireita){
Esquerda (255,255);
Serial.println("Volta ao caminho - virando a esquerda");
}
else if((_VoltaParaDireita > _VoltaParaEsquerda)){
Direita (255,255);
Serial.println("Volta ao caminho - virando a direita");
}
else if((_VoltaParaDireita == 0) && (_VoltaParaEsquerda == 0)) {
stop();
Serial.println("Parado");
//Futuramente aqui ser a chamada do modo autonomo
}
}
_Contador++; //Incrementa _Contador
Obstaculo = 0;
if(_Contador == 100){
int x, y;
y = 95;
for(x = 0; x<5; x++){
_VetorLdrEsq[x] = _VetorLdrEsq[y];
_VetorLdrDir[x] = _VetorLdrDir[y];
_VetorSensorIR[x] = _VetorSensorIR[y];
y++;
}
_Contador = 5;
}
}
int VoltaParaAlinha(){
int k;
_VoltaParaEsquerda = 0;
_VoltaParaDireita = 0;
if(Obstaculo == 0){
for(k = 0; k <= _Contador; k++){
if(_VetorLdrEsq[k] == 0){ //Se o vetor for igual a zero, incrementa o _Contador.
_VoltaParaEsquerda++;
}
if(_VetorLdrDir[k] == 0){
_VoltaParaDireita++;
}
}
}
return(_VoltaParaEsquerda,_VoltaParaDireita);
}
//Analisa a existência de linha.
void RetornaEstadoLinha() {
// _ValorIR = digitalRead(SensorIR);
LeiturasSensoresLum();
if ((_ValorLDREsquerda >= 1) && (_ValorIR == 1) && (_ValorLDRDireta >= 1)) {
// if(_ValorIR == 1) {
_EstadoLinha = 1;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.