(En proceso) Miniatura de un R2D2 autónomo.
Elementos de la estructura
Ruedas y tracción
Como cualquier casa pensé que empezar a construir desde abajo será un buen criterio.
Punto fundamental a plantearse aunque parezca una nimiedad es el tipo de adherencia al suelo del robot. Quizás unos de los puntos que me ha provocado mayores quebraderos de cabeza.
Las dimensiones de las ruedas deben ser reducidas, no superar los 35mm de diámetro. Esto y el poco peso del robot hacen que esta cuestión no deba dejarse de lado.
Decide qué tipo de adherencia te viene mejor y haz algunas pruebas previas. Puedes usar una o dos ruedas por pata si así se requiere (recuerda que el motor posee dos ejes).
Propongo a continuación varias soluciones al problema. Elige la que mejor se adapte a tus necesidades y recuerda que además existen una gran variedad de ruedas en el mercado.
Modelo 1 de tracción:
La goma se puede imprimir en TPU o Nylon que encajará a presión sobre la llanta que puedes imprimir en un material rígido. El diámetro total del conjunto es de 32cm.
Modelo 2 de tracción:
Este modelo es un solo bloque que puedes imprimir enteramente con un filamento flexible.
Modelo 3 de tracción:
Esta es la opción que adopté personalmente pues me parece la más estable y con mejor fricción. Consta de una llanta con un hueco adecuado para insertar una goma toroide de grifería. El agarre es mejor. Tener en cuenta que el diámetro de la llanta es de 27mm, se pueden usar gomas de menor tamaño (20mm es apropiado) pues se estiran y adaptan bastante bien
Las ruedas deben ser la primera pieza a montar en el robot. Pues una vez colocado el motor dentro de las patas, ya será imposible el acceso a estas sin desmontar nada.
Patas
El modelo consta de 3 patas. Una central y dos laterales.
Patas laterales
Las patas laterales están marcadas en su interior por las letras I de izquierda y D de derecha.
Cada pata está compuesta de dos piezas que encajarán una vez colocados los motores.
En ellas deben ubicarse los motores en los que previamente habremos colocado las ruedas.
Para fijar los motores en su interior usaremos tornillos M3 de 25mm de largo (preferentemente de cabeza cónica). Las tuercas deben incrustarse previamente en los orificios adecuados para ellas que nos facilitarán enormemente la fijación con el destornillador.
El cable de los motores debe pasarse en este momento por el interior de la pata que la atravesará y saldrá por el otro extremo con forma cuadrada.
Una vez fijados, es momento de cerrar el conjunto usando la pieza «pie«. Existen igualmente dos de ellas marcadas con la letra correspondiente. Para ello usamos 4 tornillos autorroscantes de 3x10mm.
Ya podemos ir añadiendo partes electrónicas con el driver ln298n que controlarán los motores.
Realizar la misma tarea en las dos patas.
Pata Central
Como apoyo en esta para podemos usar cualquier tipo de pata loca de pequeño tamaño. En mi caso usaré unos rodamientos de bola de acero.
La pata central consta de dos piezas. Una es la pata en sí y la otra es el soporte del rodamiento que usaremos de apoyo que servirá para su fijación. Irán atornilladas con tornillos autorroscantes de 3x10mm.
Nótese que la pata central posee 5 orificios para ser anclada al cuerpo del robot desde su interior.
Cabeza
Cuerpo
Tener en cuenta que la pieza que sujeta el altavoz, la pila y el USB aparecen en apartados siguientes para poder descargar e imprimir.
Engranaje y rodamiento central
En primer lugar usaremos la pieza con forma de H en la que incrustaremos el servomotor atornillado con 2 dornillos de 3x10mm autorroscables.
También añadiremos el rodamiento 608ZZ (22x8x7mm) que atraparemos con la pieza «soporte_rodamiento» asegurada con 3 tornillos 3x10mm autoroscantes. También ajustaremos la pieza del eje del servo que agarrará el engranaje de 18 dientes.
Este es un buen momento para asegurarnos que el servo tiene la posición 90º que es la que usaremos para que el robot mire al centro.
Para ello usaremos un pequeño programa que coloque el servo en esa posición.
#include <Servo.h>
Servo myservo;
int servopin = 6;
void setup() {
myservo.attach(servopin);
myservo.write(0);
delay(500);
myservo.write(90);
delay(500);
}
void loop() {
}Esto lo hacemos para poder colocar con certeza la pieza anillo en la posición correcta y poder garantizar así el giro adecuado de la cabeza del robot.
Roscaremos el engranaje de 18 dientes con los tornillos que incluye el kit del servomotor con la precaución de que los tornillos deben estar hacia arriba para evitar colisiones.
La razón por la que he decidido diseñar los engranajes con distinto número de dientes es garantizar que con 180º de giro del servo, la cabeza gire algunos grados más, es decir, que pueda mirar de una pata a la otra para medir la distancia a los obstáculos laterales.
Es momento ahora de colocar la pieza «anillo» que posee un rodamiento de 16 dientes. La ajustaremos el rodamiento usando un tornillo 25x3mm con su tuerca incrustada en el hueco destinado. Se usarán las arandelas necesarias para garantizar el ajuste perfecto del sistema.
Una vez asegurada la pieza que se ancla al rodamiento, es el momento de añadir la pieza que sostendrá la brújula. La atornillaremos desde abajo preferentemente para evitar colisiones con 3 tornillos 10x3mm autorroscantes.
Ya solo queda añadir el sensor de orientación y asegurarlo con la pieza que lo agarrará al sistema. La razón por la que no uso tornillos metálicos es como se ha comentado antes para evitar en todo lo posible la interferencia electromagnética.
El último paso es conectar adecuadamente los terminales a la placa central y atornillar el bloque de engranajes a la cabeza con 3 tornillos 10x3mm autorroscantes y el cuerpo con 4 de ellos.
El sistema debe quedar sujeto y cerrado.
Electrónica
Esquema electrónico
Elementos electrónicos:
Para el proyecto usaremos las siguientes partes:
- Brújula HCM5883L
- Alimentación: batería 18650, cargador USB, Step-Up para 5Voltios.
- 2Motores TT
- 5 Leds RGB de cátodo común, 3 transistores 2N2222A, 6 resistencias.
- Driver para motores LN298N
- Servo motor MG90S
- Sensor de distancia por ultrasonidos HC-SR04
- Sensor capacitivo de contacto TTP223B
- Reproductor/amplificador de sonidos Open-Smart Mp3
Brújula
Una brújula digital es un sensor que mide el valor del campo magnético en tres ejes. Con esta medición, es posible estimar la orientación del dispositivo respecto al campo magnético de la tierra.
En este caso usaremos la brújula HCM5883L.
La conexión es sencilla, simplemente alimentamos el módulo desde Arduino mediante GND y 5V y conectamos el pin SDA y SCL de Arduino con los pines correspondientes del sensor, dejando el pin DRY sin conectar.
En este ejemplo empleamos la medición del campo magnético para determinar la orientación del sensor.
La orientación se obtiene en grados respecto al norte magnético. Sin embargo, el norte geográfico no está perfectamente alineado al norte magnético. Esto se denomina declinación, y depende del punto del globo donde estéis ubicados.
Por tanto, si queréis obtener la orientación respecto al norte geográfico deberéis introducir la declinación de vuestra posición. Existen varias páginas donde consultar este valor, como www.ign.es para ubicaciones dentro de España, o www.ngdc.noaa.gov para ubicaciones en todo el mundo.
Realmente para este proyecto no es necesario compensar la declinación, pero la información no está de más. En mi localización y en esta fecha la compensación será de 0’06.
//GND - GND
//VCC - VCC
//SDA - Pin A4
//SCL - Pin A5
#include "Wire.h"
#include "I2Cdev.h"
#include "HMC5883L.h"
HMC5883L compass;
int16_t mx, my, mz;
//declinacion en grados en tu posición
const float declinacion = 0.12;
void setup()
{
Serial.begin(9600);
Wire.begin();
compass .initialize();
}
void loop() {
//Obtener componentes del campo magnético
compass .getHeading(&mx, &my, &mz);
//Calcular ángulo el ángulo del eje X respecto al norte
float angulo = atan2(my, mx);
angulo = angulo * RAD_TO_DEG;
angulo = angulo - declinacion;
if(angulo < 0) angulo = angulo + 360;
Serial.print("N:");
Serial.println(angulo,0);
}Para usar el código necesitarás las siguientes librerías: HMC5883L.h y I2Cdev.h que se usarán también en el programa definitivo.
El motivo para incluir una brújula en este proyecto es que el desplazamiento del robot sea ortogonal, es decir que realice giros de 90 grados para corregir su posición.
El código de la función que usaré para la posición será el siguiente:
void brujula() {
mag.getHeading(&mx, &my, &mz);
azimut = atan2(my, mx);
if (azimut < 0) {
azimut += 2 * M_PI;
}
azimut = azimut * 180 / M_PI;
rumbo=azimut;
}Problemas
Durante las pruebas la brújula ha dado valores erróneos y parecen ser debidos a el campo magnético alterado por los tornillos que lo anclan a la estructura y la cercanía de la brújula al imán del altavoz del robot. Por este motivo modificaré la fijación de la brújula para ubicarla lo más lejos posible de estos elementos y evitaremos usar sujeciones metálicas. La pieza de los engranajes de la cabeza parece el mejor candidato.
Alimentación
Por economía de espacio usaremos una batería litio 18650 que proporciona un voltaje de 3.7 voltios junto a su cargador USB. Como es preciso elevar dicho voltaje a 5 Voltios debemos usar un Step-UP para esta tarea. Además debemos incorporar un interruptor de encendido y apagado que interrumpa el paso de corriente desde el positivo del Step-up hasta la placa de conexiones.
Emplearemos un interruptor de palanca de dimensiones como se muestran en la imagen.
El interruptor encaja en el orificio asignado en la parte trasera del cuerpo del robot y es bloqueado por su propia rosca.
Y el cargador de la batería posee un lugar especial en la parte superior de la trasera del cuerpo del robot en la que va anclada con una pieza especial que debemos imprimir con dos tornillos 3x10mm autorroscantes.
Por su parte, la pila debe ir colocada en el hueco cilíndrico del interior del cuerpo del robot bloqueada por una pieza que impide su movimiento que debemos imprimir y atornillar con 2 tornillos 3x10mm autorroscantes.
Motores
El motor TT para Arduino es un motor DC pequeño y económico utilizado en proyectos de electrónica y robótica debido a su tamaño compacto y bajo costo. Este tipo de motor se caracteriza por su fácil control a través de un módulo controlador de motores o puente H.
Pueden ser alimentados de 3 a 8 voltios. En nuestro proyecto usaremos 5 voltios. Disponen de una reductora que genera suficiente tracción para nuestro robot.
En nuestro robot usaremos dos de ellos que serán controlados por el driver LN298N.
Luces leds
Para el efecto luminoso del ojo central usaremos 5 leds RGB de cátodo común en paralelo dispuestos sobre una pieza que se anclará al interior de la cabeza con 3 tornillos 3x10mm autorroscantes.
En las imágenes se puede ver el proceso de soldadura de los leds, que usaran unas resistencias de 150 ohm para el rojo, 150 ohm para el azul y 300 ohm para el verde que parece más luminoso en mi caso.
Para encenderlos emplearemos 3 transistores 2N2222A en cada canal con una resistencia de 1K en cada una de las bases. Estos transistores serán controlados por 3 pines PWM que corresponderán a D9, D10 y D11 para cada uno de los colores.
Ha de tenerse en cuenta que la librería «servo.h» que usaremos en el proyecto para el servomotor afecta al funcionamiento del PWM del pin 10, lo que nos impedirá usar el comando «analogWrite» para encender los leds con grados de intensidad y tendremos que emplear en su lugar «digitalWrite».
Driver de motores
Para controlar los motores vamos a usar el driver LN298N que posee un puente H y puede controlar el giro y la velocidad de dos motores.
Vin admite tensiones entre 3V y 35V, nosotros lo alimentaremos con 5Voltios por lo que dejamos puesto el jumper que alimentará también a la parte lógica con los mismos 5V.
Los pines IN1 e IN2 nos sirven para controlar el sentido de giro del motor A, y los pines IN3 e IN4 el del motor B. Funcionan de forma que si IN1 está a HIGH e IN2 a LOW, el motor A gira en un sentido, y si está IN1 a LOW e IN2 a HIGH lo hace en el otro. Y lo mismo con los pines IN3 e IN4 y el motor B.
ENA y ENB los conectaremos a dos salidas PWM de Arduino de forma que le enviemos un valor entre 0 y 255 que controle la velocidad de giro.
Usaremos la siguiente configuración de pines en el programa definitivo:
// Motor A
int ENA = 5;
int IN1 = A1;
int IN2 = 12;
// Motor B
int ENB = 3;
int IN3 = 4;
int IN4 = 2;Este driver irá atornillado en un lateral del cuerpo del robot con 4 tornillos 3x10mm autorroscantes.
Servomotor
El servo elegido es el MG90S que incorpora un eje metálico y proporciona una fiabilidad algo mejor que los conocidos SG90.
Usaremos la librería incluida en el IDE de Arduino llamada «Servo.h» y conectaremos el cable naranja al pin D6 que debe ser PWM.
La documentación de esta librería nos advierte que los pines D9 y D10 serán afectados en la función PWM.
Sensor de distancia
Para medir la distancia a un obstáculos emplearemos el común y barato sensor de ultrasonidos HC-SR04 que nos permitirá saber si hay paredes en un radio de 150cm aproximadamente.
Su funcionamiento consiste en emitir un sonido ultrasónico por uno de sus transductores, y esperar que el sonido rebote de algún objeto presente, el eco es captador por el segundo transductor. La distancia es proporcional al tiempo que demora en llegar el eco.
El emisor lo denominaremos trigger (TRIG) cuya traducción sería «desencadenante» y al receptor lo llamaremos echo cuya traducción sería «eco».
Conectaremos el sensor en los pines A2 y A3 que a pesar de ser analógicos podemos usarlos como si fuesen digitales simplemente asignándolos en el setup del programa. Debido a la gran demanda de pines de este proyecto debemos usar algunos de los analógicos como si de digitales se tratase.
Este sensor se alimenta también con 5 voltios.
La distancia se puede calcular utilizando la siguiente formula:
Distancia(m) = {(Tiempo del pulso ECO) * (Velocidad del sonido=340m/s)}/2
Lo dividimos entre dos por que el sonido hace el viaje de ida y vuelta.
Lo atornillaremos en la cabeza del robot usando 2 tornillos autorroscantes de 3x10mm. (precaución de no atornillar demasiado pues el orificio es de menor distancia que el tornillo).
Para calcular la distancia hasta el obstáculo emplearemos esta función a la que llamaremos periódicamente desde el loop.
int getDistance() //Measure the distance to an object
{
unsigned long pulseTime; //Create a variable to store the pulse travel time
int distance; //Create a variable to store the calculated distance
digitalWrite(trig, HIGH); //Generate a 10 microsecond pulse
delayMicroseconds(10);
digitalWrite(trig, LOW);
pulseTime = pulseIn(echo, HIGH, timeOut); //Measure the time for the pulse to return
distance = (float)pulseTime * 340 / 2 / 10000; //Calculate the object distance based on the pulse time
return distance;
}Finalmente y a pesar de que el código funciona, he decidido emplear la librería que encontraremos en el repositorio de Ardunio «NewPing.h» pues ofrece mejor estabilidad al programa ya necesitamos de un timeout en casos de no detectar el rebote entre otras cosas.
Sensor touch (táctil).
Este módulo utiliza el circuito integrado TTP223B que es un sensor touch capacitivo. En su estado normal la salida del módulo se encuentra en cero lógico y bajo consumo de corriente, cuando un dedo toca la posición correspondiente la salida del módulo se activa en uno lógico. Si no se toca el modulo en 13 segundos vuelve a modo de bajo consumo.
Será alimentado en VCC con 5 voltios. GND debe conectarse a masa y el pin marcado con I/O es la salida digital que conectaremos directamente al pin A0 que configuraremos como entrada digital a pesar de tratarse de un pin analógico, pero la gran necesidad de pines me obliga a usarlos de esta manera.
El módulo posee dos zonas de puentes de soldadura nombrados como A y B.
Por defecto viene con estos puentes sin soldar (puente abierto).
El selector A configura el estado lógico, es decir, si el puente está sin soldar, el módulo entregará un 0 cuando no es pulsado y un 1 cuando es pulsado. Su comportamiento se invertirá en este aspecto si aplicamos un puente de soldadura en este selector. (Funciona como un inversor).
El selector B hace que cambie el comportamiento del módulo, es decir, si el puente está sin soldar, al ser pulsado se comportará como un interruptor momentaneo (sin bloqueo). Su aplicamos soldadura en este puente conseguiremos que se comporte como un interruptor encendido/apagado (con bloqueo). Esto es útil para evitar oscilaciones al dejar presionado durante un tiempo prolongado el sensor.
El módulo posee además una soldadura para añadir una antena y aumentar así su distancia de acción.
También puede cambiarse su sensibilidad modificando el condensador que trae por defecto.
Es capaz de ser estimulado a través de la capa de filamento de unos milímetros por lo que lo situaremos en la parte superior de la cabeza del robot y dará la sensación que responde al estímulo del contacto.
Para sujetar el módulo capacitivo usaremos una pieza que debemos imprimir que se encuentra en el conjunto STL de la cabeza que se llama soporte_touch y atornillarla usando 2 tornillos 3x10mm autorroscantes.
Reproductor de sonidos.
Vamos a usar un módulo muy económico y versátil, ya que además de reproducir audio en formato mp3 o wav lo amplifica ahorrando espacio pues no es necesario añadir amplificador de audio.
Se trata del Serial MP3 player A de Open – Smart. Funciona a 5 Voltios y se comunica con el microprocesador por puerto serie.
Dispone de 4 pines. Tx que conectaremos al pin D8 ,Rx que conectaremos al pin D7, GND y VDD.
En el código de programación usaremos un puerto serial creado por software en los pines 7 y 8 mediante la librería <SoftwareSerial.h> que encontraremos en el repositorio natural de arduino.
Los audios los almacenaremos en una tarjeta de memoria SD. Contenida en carpetas nombrados con una numeración de 3 dígitos (001.mp3, 002.mp3…).
El kit contiene un altavoz que emplearemos también en el proyecto. Irá anclado en la parte baja del robot. Para ello es necesario haber anclado la pata central antes mediante 5 tornillos 3x10mm autorroscantes como se muestra en la figura.
Seguidamente ajustaremos el parlante y lo bloquearemos con la pieza especial para ello mediante 2 tornillos 3x10mm autorroscantes.
Por último colocar y atornillar con 4 tornillos 3x10mm autorroscantes el módulo MP3 por encima del parlante y conectar.
Para manejar este módulo necesitaremos la librería RedMP3.h demás de la librería <SoftwareSerial.h> mencionada antes.
En el siguiente enlace puedes descargar el fichero con los sonidos que se han usado en este proyecto ordenador por carpetas.
Carpeta 01: Sonidos normales que se emplearán al pulsar el sensor táctil.
Carpeta 02: Sonidos optimistas de felicidad se emplearán al encontrar una salida. (falta el 019.mp3).
Carpeta 03: Sonidos de contradicción se emplearán al detectar un obstáculo.
Carpeta 04: Sonidos de enfado se emplearán en momentos de frustración.
Código y programación
#include <SoftwareSerial.h>
#include "RedMP3.h"
#include <Servo.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "HMC5883L.h"
#include <NewPing.h>
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(A3, A2, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
int stopDist = 35; //Minimum distance from an object to stop in cm
int turnDir = 0;
HMC5883L mag;
int16_t mx, my, mz;
float azimut;
int rumbo;
int frente;
int pos_de;
int pos_iz;
Servo myservo;
int servopin = 6;
int centro = 97; //Centro de la cabeza, grados del servo
int posicion;
#define MP3_RX 7 //RX of Serial MP3 module connect to D7 of Arduino
#define MP3_TX 8 //TX to D8, note that D8 can not be used as RX on Mega2560, you should modify this if you donot use Arduino UNO
MP3 mp3(MP3_RX, MP3_TX);
int8_t volume = 0x19; //0~0x1e (30 adjustable level)
boolean pulsa;
int ledrojo = 9;
int ledverde = 11;
int ledazul = 10;
int pintouch = A0;
int i = 0;
// Motor A
int ENA = 5;
int IN1 = A1;
int IN2 = 12;
// Motor B
int ENB = 3;
int IN3 = 4;
int IN4 = 2;
byte motorSpeed = 255; //Velocidad máxima de los motores
byte minimavel = 150;
int motorOffset = 0; //En caso de desfase de potencia entre los motores
int feliz;
int felicidad = 60;
void setup() {
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ledrojo, OUTPUT);
pinMode(ledverde, OUTPUT);
pinMode(ledazul, OUTPUT);
pinMode(pintouch, INPUT);
Serial.begin(9600);
Serial.println("Saludos_R2D2");
for (int i = 0; i <= 250; i++) {
analogWrite(ledazul, i);
delay(5);
}
digitalWrite(ledazul, HIGH);
Wire.begin();
mag.initialize();
mag.setMode(0); //Brújula en modo continuo
delay(500); //Se necesitan al menos 500ms de espera para inicializar el módulo MP3
mp3.setVolume(volume);
delay(50); //Si no se espera más de 50ms entre dos comandos de mp3, se bloquea y hay que reiniciarlo.
mp3.playWithFileName(01, 0x01);
delay(300); //Espera que acabe el mp3
myservo.attach(servopin);
posicion = centro;
myservo.write(posicion + 10);
delay(300);
myservo.write(posicion - 10);
delay(300);
myservo.write(posicion);
delay(500);
digitalWrite(ledazul, LOW);
myservo.detach();
delay(50);
digitalWrite(ledazul, HIGH);
delay(80);
digitalWrite(ledazul, LOW);
delay(50);
digitalWrite(ledazul, HIGH);
}
void loop() {
pulsa = digitalRead(pintouch);
if (pulsa == HIGH) { tocado(); }
int distance = getDistance();
if (distance == 0 || distance >= stopDist) //If there are no objects within the stopping distance, move forward
{
// Serial.println("avanzo");
adelante();
digitalWrite(ledrojo, LOW);
digitalWrite(ledverde, LOW);
digitalWrite(ledazul, HIGH);
}
while (distance == 0 || distance >= stopDist) //Keep checking the object distance until it is within the minimum stopping distance
{
distance = getDistance();
delay(100);
feliz++;
Serial.println(feliz);
if (feliz >= felicidad) {
int son = random(0x1, 0x15); //001.mp3 hasta 21.mp3
mp3.playWithFileName(0x02, son); //Carpeta de sonidos felices.
feliz = 0;
}
}
para();
medir();
switch (turnDir) //Turn left, turn around or turn right depending on the instruction
{
case 1: //gira a la izquierda
Serial.println("Giro a IZQUIERDA");
giro_iz();
break;
case 2: //gira para volver
Serial.println("Giro atrás");
atras();
break;
case 3: //gira a la derecha
Serial.println("Giro a DERECHA");
giro_de();
break;
}
}
void giro_iz() {
digitalWrite(ledazul, LOW);
digitalWrite(ledrojo, LOW);
digitalWrite(ledverde, HIGH);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, 255); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, 255); //Velocidad motor B
delay(200);
int objetivo;
objetivo = pos_iz;
Serial.print("mi objetivo es: ");
Serial.println(objetivo);
while (rumbo != objetivo && rumbo != (objetivo - 1) && rumbo != (objetivo + 1) && rumbo != (objetivo - 2) && rumbo != (objetivo + 2)) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA, minimavel); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, minimavel);
brujula();
}
}
void giro_de() {
digitalWrite(ledazul, LOW);
digitalWrite(ledrojo, LOW);
digitalWrite(ledverde, HIGH);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, 255); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, 255); //Velocidad motor B
delay(200);
int objetivo;
objetivo = pos_de;
Serial.print("mi objetivo es: ");
Serial.println(objetivo);
Serial.print("RUMBO es:");
Serial.println(rumbo);
while (rumbo != objetivo && rumbo != (objetivo - 1) && rumbo != (objetivo + 1) && rumbo != (objetivo - 2) && rumbo != (objetivo + 2)) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, minimavel); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENB, minimavel);
brujula();
}
}
void para() {
feliz = 0;
digitalWrite(ledazul, LOW);
digitalWrite(ledverde, LOW);
digitalWrite(ledrojo, HIGH);
analogWrite(ENA, 0); //Velocidad motor A
analogWrite(ENB, 0); //Velocidad motor A
int son = random(0x1, 0x11); //001.mp3 hasta 17.mp3
mp3.playWithFileName(0x03, son); //Carpeta de sonidos tristes.
delay(60);
}
void medir() {
int medida[2] = { 0, 0 };
myservo.attach(servopin);
posicion = centro;
myservo.write(posicion);
delay(300);
brujula();
frente = rumbo;
Serial.print("Mirumbo actual es: ");
Serial.println(frente);
myservo.write(10);
delay(500);
Serial.print("Miro total a la izquierda:\t ");
brujula();
pos_iz = rumbo;
medida[0] = getDistance();
if (medida[0] == 0) { medida[0] = MAX_DISTANCE; }
delay(200);
Serial.print(pos_iz);
Serial.print(" distancia: ");
Serial.println(medida[0]);
myservo.write(180);
delay(800);
Serial.print("Miro total a la derecha:\t ");
brujula();
pos_de = rumbo;
medida[1] = getDistance();
if (medida[1] == 0) { medida[1] = MAX_DISTANCE; }
delay(500);
Serial.print(pos_de);
Serial.print(" distancia: ");
Serial.println(medida[1]);
myservo.write(centro);
delay(500);
if (medida[0] >= MAX_DISTANCE && medida[1] >= MAX_DISTANCE) //Los dos lados despejados, girará a la Izquierda
turnDir = 1;
else if (medida[0] <= stopDist && medida[1] <= stopDist) //Los dos lados cerrados, dará la vuelta
turnDir = 2;
else if (medida[0] >= medida[1]) //La izquierda está libre, girará a la izquierda
turnDir = 1;
else if (medida[0] < medida[1]) //La derecha está libre, girará a la derecha
turnDir = 3;
rumbo = frente;
myservo.detach();
return turnDir;
}
int getDistance() //Measure the distance to an object
{
int distance;
distance = sonar.ping_cm();
return distance;
}
void brujula() {
mag.getHeading(&mx, &my, &mz);
azimut = atan2(my, mx);
if (azimut < 0) {
azimut += 2 * M_PI;
}
azimut = azimut * 180 / M_PI;
rumbo = azimut; //Entero
Serial.print("rumbo:\t");
Serial.println(rumbo);
}
void adelante() //Function to accelerate the motors from 0 to full speed
{
for (int i = minimavel; i <= motorSpeed; i = i + 5) //Loop from 0 to full speed
{
//Direccion motor A
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA, i); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENB, i); //Velocidad motor B
delay(10);
}
}
void atras() {
int objetivo;
if (frente >= 180) {
objetivo = frente - 180;
} else {
objetivo = frente + 180;
}
Serial.print("Mi objetivo es: ");
Serial.println(objetivo);
//Direccion motor A
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
analogWrite(ENA, 255); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, 255); //Velocidad motor B
delay(300);
while (rumbo != objetivo && rumbo != (objetivo - 1) && rumbo != (objetivo + 1) && rumbo != (objetivo - 2) && rumbo != (objetivo + 2)) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
analogWrite(ENA, minimavel); //Velocidad motor A
//Direccion motor B
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENB, minimavel);
brujula();
}
}
void tocado() {
digitalWrite(ledazul, LOW);
int son = random(0x1, 0x4D); //001.wav hasta 077.wav
mp3.playWithFileName(0x01, son); //Carpeta de sonidos alegres.
Serial.println(son);
delay(400);
pulsa = 0;
digitalWrite(ledazul, HIGH);
}
