En el ejemplo anterior ya controlábamos el servomotor para poder orientar el sensor URM. En el siguiente ejemplo recogemos las distancias según la orientación del sensor URM.
Podremos manejar nuestro robot a través de infrarrojos con un mando a distancia. Tiene dos modos, manual (como vimos anteriormente), y automático, que es el modo en el que escaneamos las distancias y tomamos una decisión de movimiento según ese mapa.
Conexiones
-------------
Pines 1, 9 y 16 a +5V
Pin 8 a +9-24V
Pines 2, 7 10 y 15 a la placa Arduino
Pines 4, 5, 12 y 13 a ground (Dos de ellos a la placa y dos a la fuente de alimentación)
Pines 3, 6 11 y 14 conectados a los motores
pin 7 URM --> pin 42 arduino
pin 6 URM --> pin 40 arduino
pin 4 URM --> pin 38 arduino
pin 2 URM --> ground arduino
pin 1 URM --> pin 36 arduino
3V3 al lado positivo del servo
ground al lado negativo del servo
pin 47 al pin de control del servo
Código .pde
#include "WProgram.h"
#include "NECIRrcv.h"
#include "Servo.h"
#define IRPIN 12
NECIRrcv ir(IRPIN);
unsigned long ircode;
Servo servo; //Objeto Servo proporcionado por la librería
int position=0;//Posicion del servo
typedef struct
{
int posicion;
int distancia;
}Angulo;
Angulo angulo[7];
int j=0;
int auxiliar=0;
boolean intercambio=false;
const int pin1=7;
const int pin2=13;
const int pin3=6;
const int pin4=2;
const int pin5=5;
unsigned long a1=2;
unsigned long a2=2;
unsigned long a3=2;
unsigned long a4=2;
unsigned long a5=2;
int duracion1=600;
int duracion2=1000;
boolean aux=true;
int ultraData=38;
int ultraTrigger=40;
int ultraEnable=42;
int ultraPower=36;
int val=0;
int ultraValue=0;
int timecount=0;
bool State=true;
long previousMillis = 0;
long interval = 8000;
const int input1=10;
const int input2=11;
const int input3=9;
const int input4=8;
void adelante()
{
digitalWrite(input1, LOW);
digitalWrite(input2, HIGH);
digitalWrite(input3, LOW);
digitalWrite(input4, HIGH);
digitalWrite(pin1, HIGH);
digitalWrite(pin2, HIGH);
digitalWrite(pin4, LOW);
digitalWrite(pin5, LOW);
}
void izquierda()
{
digitalWrite(input1, HIGH);
digitalWrite(input2, LOW);
digitalWrite(input3, LOW);
digitalWrite(input4, HIGH);
digitalWrite(pin1, HIGH);
digitalWrite(pin2, LOW);
digitalWrite(pin4, LOW);
digitalWrite(pin5, LOW);
}
void derecha()
{
digitalWrite(input1, LOW);
digitalWrite(input2, HIGH);
digitalWrite(input3, HIGH);
digitalWrite(input4, LOW);
digitalWrite(pin1, LOW);
digitalWrite(pin2, HIGH);
digitalWrite(pin4, LOW);
digitalWrite(pin5, LOW);
}
void atras()
{
digitalWrite(input1, HIGH);
digitalWrite(input2, LOW);
digitalWrite(input3, HIGH);
digitalWrite(input4, LOW);
digitalWrite(pin1, LOW);
digitalWrite(pin2, LOW);
digitalWrite(pin3, HIGH);
digitalWrite(pin4, LOW);
digitalWrite(pin5, LOW);
}
void parar()
{
digitalWrite(input1, LOW);
digitalWrite(input2, LOW);
digitalWrite(input3, LOW);
digitalWrite(input4, LOW);
digitalWrite(pin1, LOW);
digitalWrite(pin2, LOW);
digitalWrite(pin3, LOW);
luzPosicion();
}
void luzPosicion()
{
if(aux)
{
digitalWrite(pin4, HIGH);
digitalWrite(pin5, LOW);
aux=false;
}
else
{
digitalWrite(pin4, LOW);
digitalWrite(pin5, HIGH);
aux=true;
}
}
void setup()
{
servo.attach(47);
pinMode(input1, OUTPUT);
pinMode(input2, OUTPUT);
pinMode(input3, OUTPUT);
pinMode(input4, OUTPUT);
pinMode(pin1, OUTPUT);
pinMode(pin2, OUTPUT);
pinMode(pin3, OUTPUT);
pinMode(pin4, OUTPUT);
pinMode(pin5, OUTPUT);
pinMode(IRPIN, INPUT);
ir.begin();
Serial.begin(9600);
pinMode(ultraPower, OUTPUT);
pinMode(ultraEnable, OUTPUT);
digitalWrite(ultraPower, HIGH);
digitalWrite(ultraEnable, HIGH);
delay(200);
}
void loop()
{
while (ir.available())
{
ircode=ir.read();
if(ircode==4194696960)
{
while(true)
{
j=0;
for(position=-30; position<=210; position+=40)//Movemos el servo de 0º a 180º
{
servo.write(position);
delay(500);
ultraValue = 0;
timecount = 0;
val = 0;
pinMode(ultraTrigger, OUTPUT);
digitalWrite(ultraTrigger, HIGH);
delayMicroseconds(500);
digitalWrite(ultraTrigger, LOW);
delayMicroseconds(200);
digitalWrite(ultraTrigger, HIGH);
delayMicroseconds(200);
pinMode(ultraData, INPUT);
do
{
val=digitalRead(ultraData);
}while(val==HIGH);
do
{
val=digitalRead(ultraData);
timecount++;
delayMicroseconds(50);
}while(val==LOW);
ultraValue=timecount;
Serial.println(ultraValue);
servo.write(position);
angulo[j].posicion=j;//Almacenamos los datos en las estructuras
angulo[j].distancia=ultraValue;
j++;
}
do{//Bubble sort (Ordenamos los datos del mapeo)
intercambio=false;
for(int i=0; i<6; i++)
{
if(angulo[i].distancia>angulo[i+1].distancia)
{
auxiliar=angulo[i].distancia;
angulo[i].distancia=angulo[i+1].distancia;
angulo[i+1].distancia=auxiliar;
auxiliar=angulo[i].posicion;
angulo[i].posicion=angulo[i+1].posicion;
angulo[i+1].posicion=auxiliar;
intercambio=true;
}
}
}while(intercambio);
if(angulo[6].posicion==3 || angulo[6].posicion==4)//Hacia delante la mayor distancia con un objeto
{
adelante();
delay(angulo[6].distancia*7);
parar();
}
if(angulo[0].posicion==2 || angulo[0].posicion==1 || angulo[0].posicion==0)//Si a la derecha tenemos un objeto proximo
{
if(angulo[0].distancia<35)
{
izquierda();//Giramos a la izquierda
delay(420);
parar();
}
}
if(angulo[0].posicion==5 || angulo[0].posicion==6)//Si a la izquierda tenemos un objeto próximo
{
if(angulo[0].distancia<35)
{
derecha();//Giramos a la derecha
delay(420);
parar();
}
}
if(angulo[0].posicion==3 || angulo[0].posicion==4)//Si de frente tenemos la distancia mas corta
{
if(angulo[6].posicion==0 || angulo[6].posicion==1 || angulo[6].posicion==2)//Comprobamos el lado derecho
{
derecha();//Giramos a la derecha
delay(420);
parar();
}
if(angulo[6].posicion==5 || angulo[6].posicion==6)//Comprobamos el lado izquierdo
{
izquierda();//Giramos a la izquierda
delay(420);
parar();
}
}
}
}
else
{
if(2707357440==ircode)
{
duracion1=300;
duracion2=1000;
}
if(2724069120==ircode)
{
duracion1=500;
duracion2=3000;
}
if(3776904960==ircode)
{
duracion1=800;
duracion2=5000;
}
if(ircode==3810328320)//HACIA DELANTE
{
adelante();
delay(duracion2);
parar();
ircode=0;
}
if(ircode==3877175040)//HACIA ATRAS
{
atras();
delay(duracion2);
parar();
ircode=0;
}
if(ircode==3893886720)//DERECHA
{
derecha();
delay(duracion1);
parar();
ircode=0;
}
if(ircode==2824339200)//IZQUIERDA
{
izquierda();
delay(duracion1);
parar();
ircode=0;
}
}
}
}
No hay comentarios:
Publicar un comentario