Seguidor de linha + sonar + garra mkII

Ver o tópico anterior Ver o tópico seguinte Ir em baixo

Seguidor de linha + sonar + garra mkII

Mensagem  Thugar em Qui 12 Nov - 9:27

Bom dia,
estou envolvido em um projeto que pretende executar os seguintes processos:
>Reconhecer uma linha preta no contraste com o branco
>Seguir a linha
>chegar no ponto final
>pegar um objeto, usando o sonar como sensor de presença e a garra mkii
>retornar para a linha
>chegar no outro ponto final
>largar o objeto

mas meu código não esta funcionando direito.


alguém pode me ajudar?

Primeiramente a config dos pinos:
________________________________________________________________________________________
//Guia de montagem

Sensores analogicos:
• Centro esquerdo        A4
• Centro direito A3

Sensores digitais
• Sonar:
o Trig:                11
o Echo:                12
• Garra:
o Servo 180º                   4
• Servo 360º
o Esquerdo:                5
o Direito:         6
________________________________________________________________________________________________________

Segue o código:

#include
#include "Ultrasonic.h"


//DEFINIÇÕES DA GARRA
#define echoPin 12 //Pino 12 recebe o pulso do echo
#define trigPin 11 //Pino 11 envia o pulso para gerar o echo
#define FREQ_LEITURA_DISTANCIA 1000 //Define a frequencia de leitura do sensor de distância o tempo de 1 segundo
#define Garra 4 //Pino 4 onde o servo da garra está conectado
#define ABERTA 140 //Define o valor da garra aberta
#define FECHADA 70 //Define o valor da garra fechada
#define DISTANCIA_MINIMA 6 //Define a distância mínima para a garra pegar o objeto


//DEFINIÇÕES SENSORES DE MOVIMENTO
#define Sensor1 A3 //Define a entrada analógica A3 para o Sensor1
#define Sensor2 A4 //Define a entrada analógica A4 para o Sensor2
#define RodaDireita 5 //Define a entrada digital 5 para o servo da roda do lado direito
#define RodaEsquerda 6 //Define a entrada digital 6 para o servo da roda do lado esquerdo
#define FREQ_LEITURA_SENSOR_LINHA 200 //Define a frequencia de leitura do sensor de linha o tempo de 200 milisegundo

//DEFINIÇÕES DE VARIÁVEIS DE MOVIMENTO
Servo ServoD;
Servo ServoE;
int Valor_Sensor1 = 0;
int Valor_Sensor2 = 0;
long leituraAnteriorSensorLinha;     //Variável de controle do tempo de leitura do sensor de linha


//DEFINIÇÕES DE VARIÁVEIS DA GARRA
Ultrasonic ultrasonic(trigPin,echoPin); //Iniciando a função e passando os pinos
Servo myservo;
int distancia;
long leituraAnteriorSensorDist;     //Variável de controle do tempo de leitura do sensor de distancia


//VARIÁVEIS GLOBAIS
boolean garraFechada;
unsigned long correnteMillis;


void setup(){
Serial.begin(9600); //Inicia a porta serial

//CONFIGURANDO GARRA
pinMode(echoPin, INPUT); //Define o pino echoPin como entrada (recebe)
pinMode(trigPin, OUTPUT); //Define o pino trigPin como saída (envia)
myservo.attach(Garra);
myservo.write(ABERTA); //Garante que a garra estará aberta sempre que o programa iniciar
garraFechada = false;
distancia = 0;
leituraAnteriorSensorDist = 0;

//CONFIGURANDO SERVOS DE MOVIMENTO
ServoD.attach(RodaDireita);
ServoE.attach(RodaEsquerda);
leituraAnteriorSensorLinha = 0;
}

void loop(){
correnteMillis = millis();    //Tempo atual em ms

if ((DistanciaObjeto() < DISTANCIA_MINIMA) && !garraFechada){
Freiar();
FecharGarra();  //Pegar o objeto
}
else {
if (EFimDeLinha() && garraFechada){
Freiar();
AbrirGarra();  //Soltar o objeto
}
else {
Movimentar();  //Andar
}
}

}

int DistanciaObjeto(){
if (correnteMillis - leituraAnteriorSensorDist > FREQ_LEITURA_DISTANCIA) { //So faz a leitura do sensor a cada 1 segundo
leituraAnteriorSensorDist = correnteMillis;    // Salva o tempo atual
digitalWrite(trigPin, LOW);   //seta o pino 12 com um pulso baixo "LOW" ou desligado ou ainda 0
delayMicroseconds(2); //delay de 2 microssegundos
digitalWrite(trigPin, HIGH);   //seta o pino 12 com pulso alto "HIGH" ou ligado ou ainda 1
delayMicroseconds(10);   //delay de 10 microssegundos
digitalWrite(trigPin, LOW);   //seta o pino 12 com pulso baixo novamente

//Função Ranging, faz a conversão do tempo de resposta do echo em centimetros, e armazena na variavel distancia
distancia = (ultrasonic.Ranging(CM));  //Faz a leitura da distância entre a empilhadeira e o objeto
}
return distancia;
}

void FecharGarra(){
myservo.write(FECHADA);
delay(1000);
garraFechada = true;
Serial.print("Distancia em CM: ");
Serial.println(distancia);
Girar180();
}

void AbrirGarra(){
myservo.write(ABERTA);
delay(1000);
garraFechada = false;
Girar180();
}

boolean EFimDeLinha(){
boolean fim = false;
if((Valor_Sensor1 < 450) && (Valor_Sensor2 < 450)){ //caso os dois sensores saim da linha
fim = true;
}
return fim;
}

void Movimentar(){

if (correnteMillis - leituraAnteriorSensorLinha > FREQ_LEITURA_SENSOR_LINHA) { //So faz a leitura do sensor a cada 200 milisegundo
leituraAnteriorSensorLinha = correnteMillis;    // Salva o tempo atual
Valor_Sensor1 = analogRead(Sensor1);
Valor_Sensor2 = analogRead(Sensor2);

if((Valor_Sensor1 > 450) && (Valor_Sensor2 > 450)){
ServoD.write(45);
ServoE.write(135);
}

if((Valor_Sensor1 < 450) && (Valor_Sensor2 > 450)){
ServoD.write(90);
ServoE.write(135);
}

if((Valor_Sensor1 > 450) && (Valor_Sensor2 < 450)){
ServoD.write(45);
ServoE.write(90);
}

if((Valor_Sensor1 < 450) && (Valor_Sensor2 < 450)){ //caso os dois sensores saim da linha
ServoD.write(45);
ServoE.write(90);
}
}
delay(200);
}

void Girar180(){
//Faz o movimento de ré
ServoD.write(135);
ServoE.write(45);
   delay(1000);

Freiar();

//Faz o giro de 180 graus
ServoD.write(135);
ServoE.write(90);
delay(1000)
}

void Freiar() {  //Pára motores
   ServoD.write(90);
   ServoE.write(90);
   delay(1000);

}

Thugar

Mensagens : 1
Data de inscrição : 12/11/2015

Ver perfil do usuário

Voltar ao Topo Ir em baixo

Ver o tópico anterior Ver o tópico seguinte Voltar ao Topo

- Tópicos similares

 
Permissão deste fórum:
Você não pode responder aos tópicos neste fórum