Sensor URM + Servo II

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;
}
}
}
}