¿Como funciona? En componentes la lista es facil:
- Chasis y ruedas
- Sensores de ultrasonidos
- Servos trucados
- Placa arduino
- Bateria
Me ha llevado tiempo que los componentes funcionen como deben y se quieran pero la final todo va de manera aceptable. Dos de mis servos corren a diferentes velocidades, por tanto no puedo obtener el maximo rendimiento.
A nivel de software la cosa se complica, ya que yo soy un novato con los robots y es dificil imaginarse los problemas a los que tienen que hacer frente. Aun asi, aqui dejo el codigo de los dos programas que he diseñado, uno para recorrer mi casa y otro para carreras en pasillos. Sentiros libres de usarlos, mejorarlos y hacer con ellos lo que querais.
Os pondre algun video de como funciona cada algoritmo.
Aventura V2:
#include <Ultrasonic.h> // Importamos las librerias del modulo de ultrasonidos
#include <Servo.h> // Importamos las librerias que controlan el servo
Ultrasonic ultraleft(3,4); // Definimos los objetos Ultrasonic
Ultrasonic ultraright(5,6); // Pines del sensor: Trigger pin, Echo pin
Servo servoR; // Definimos los objetos Servo
Servo servoL;
int servoPinR = 10; // Pines del servo
int servoPinL = 9;
int servoStopR = 95; // Angulo del servo para parar
int servoStopL = 79;
long randNumber; // Guardamos memoria para el numero aleatorio
int terrorLevel = 0; // Nivel de terror
unsigned long time; // Inicializamos variable para guardar el tiempo
long fiveSec = 5000; // Intervalo de tiempo para entrar en modo de terror
long terrorAt = 0; // Valor del tiempo en cada momento de panico
void setup() {
Serial.begin(9600); // Recibimos datos del puerto serie
servoR.attach(servoPinR); // Asociamos pines a los Servo
servoL.attach(servoPinL);
randomSeed(analogRead(0)); // Creamos una semilla para numeros aleatorios a partir de la lectura del pin 1
}
// DEFINIMOS LA FUNCION terrorMode, QUE NOS PERMITE DEFINIR EL NIVEL DE TERROR DEPENDIENDO DEL ENTORNO
int terrorMode(int left, int right, int current){
int mode = 0; // Inicializamos el modo, valor que devolveremos
time = millis(); // Guardamos el tiempo
if (current >= 999){ // Comprobamos si la anterior ha sido una situacion de terror maximo
current = 0; // Reiniciamos el contador, pues el robot habra dado media vuelta
}
if (left <=8 || right <=8){ // Situacion de obstaculo tipica, a 8cm del obstaculo
mode = current + (50 / ((long(time) - terrorAt - (50 * (current / 10))))) + 1;
/* Explicare la formula, ya que es el centro del algoritmo. Tomamos el valor actual de terror, al
cual añadimos el tiempo de reaccion multiplicado por diez, lo cual nos da mayor precision (genera
una escala de terror sobre 100 en vez de sobre 10), dividido por la diferencia entre situaciones
de panico, a la cual restamos el retraso producido en (1), el cual se rige por la formula "delay(50 * ((val) / 10));"
Le sumamos 1 para el caso del primer momento de panico, en el cual el resto de operaciones da cero.*/
terrorAt = time; // Tomamos el nuevo instante de panico
}
else{
if (current >= 1){ // No hay ningun obstaculo
if (current - (long(time) - terrorAt) / 2000 >= 0){ // Comprueba si podemos restar sin llegar a num negativos
mode = current - (long(time) - terrorAt) / 2000; // Damos el margen de dos segundos para empezar a restar
}
else{
mode = 0; // Reseteamos si la resta fuera a dar negativo
}
}
else{
return mode;
}
}
if (mode >= 100){ // Entrar en modo de terror absoluto
return 999;
}
else{
return mode;
}
}
// DEFINIMOS LA FUNCION navigate, ENCARGADA DEL MOVIMIENTO DEL ROBOT DEPENDIENDO DEL ENTORNO
void navigate(int left, int right, int val){
switch (val) {
case 0: // Cuando el entorno no tiene ningun obstaculo peligroso
randNumber = random(2); // Generamos numeros aleatorios para humanizar el movimiento del robot,
// permitiendo que se de cuenta de esa pared tan obvia para nosotros.
servoL.write(servoStopL + (19 * right / 50) - randNumber);
servoR.write(servoStopR + (24 * left / 50) - randNumber); // Ese aumento sobre 24 es porque la diferencia entre los dos
break; // provocaba que se fuera hacia la derecha.
case 999: // Situacion de terror absoluto; dara media vuelta
servoL.write(servoStopL + 19);
servoR.write(servoStopR - 24);
delay(1000);
break;
default: // Cuando el entorno tiene algun obstaculo que evitar girara de acuerdo a las distancias medidas
servoL.write(servoStopL - 19 + (38 * right / 25));
servoR.write(servoStopR - 24 + (28 * left / 25));
// (1)
delay(50 * ((val) / 10)); // (1)
}
}
void loop(){
int measureLeft = ultraleft.Ranging(CM); // Medimos la distancia con ultrasonidos
int measureRight = ultraright.Ranging(CM);
terrorLevel = terrorMode(measureLeft, measureRight, terrorLevel); // Devuelve ell nivel de terror
navigate(measureLeft, measureRight, terrorLevel); // Ejecuta la funcion de movimiento
}
Aventura antiguo:
#include <Ultrasonic.h> // Importamos las librerias del modulo de ultrasonidos
#include <Servo.h> // Importamos las librerias que controlan el servo
Ultrasonic ultraleft(3,4); // Trigger pin, echo pin
Ultrasonic ultraright(5,6);
Servo servoR; // Definimos los objetos Servo
Servo servoL;
int servoPinR = 10; // Pines del servo
int servoPinL = 9;
int servoStopR = 95; // Velocidad de parada
int servoStopL = 79;
long randNumber; // Guardamos memoria para el numero aleatorio
int Terror = 0; // Nivel de terror
long tiempo = 5 * 60 * 1000; // Tiempo funcionando, cinco minutos
void setup() {
Serial.begin(9600); // Recibimos datos del puerto serie
servoR.attach(servoPinR); // Asociamos pines a los Servo
servoL.attach(servoPinL);
randomSeed(analogRead(0)); // Creamos una semilla para numeros aleatorios a partir de la lectura del pin 1
}
// DEFINIMOS LA FUNCION terrorMode, QUE NOS PERMITE DEFINIR EL NIVEL DE TERROR DEPENDIENDO DEL ENTORNO
int terrorMode(int left, int right, int current){
int mode = 0; // Inicializamos el modo, valor que devolveremos
if (current == 999){ // Comprobamos si la anterior ha sido una situacion de terror maximo
current = 0;
}
if (left == right && left <= 8){ // Situacion de las esquinas y otros lugares peligrosos para el robot
mode = current + (32 / left);
}
else{
if (left <=10 || right <=10){ // Situacion de obstaculo tipica
mode = current + 2;
}
else{
if ( current >= 1){ // No hay ningun obstaculo
delay (100);
mode = current - 1;
}
}
}
if (mode >= 10 || millis() % tiempo == 0){ // Entrar en modo de terror absoluto
return 999;
}
else{
return mode;
}
}
// DEFINIMOS LA FUNCION navigate, ENCARGADA DEL MOVIMIENTO DEL ROBOT DEPENDIENDO DEL ENTORNO
void navigate(int left, int right, int terror){
switch (terror) {
case 0: // Cuando el entorno no tiene ningun obstaculo peligroso
randNumber = random(1); // Generamos numeros aleatorios para humanizar el movimiento del robot,
// permitiendo que se de cuenta de esa pared tan obvia para nosotros.
servoL.write(servoStopL + ( 19 * right / 50) - randNumber);
servoR.write(servoStopR + ( 24 * left / 50) - randNumber);
break;
case 999: // Situacion de panico
servoL.write(servoStopL + 19);
servoR.write(servoStopR - 24);
delay(1000);
break;
default: // Cuando el entorno tiene algun obstaculo que evitar
servoL.write(servoStopL - 19 + ( 38 * right / 50));
servoR.write(servoStopR - 24 + ( 48 * left / 50));
delay(100 * terror);
}
}
void loop(){
int medirIzquierdo = ultraleft.Ranging(CM); // Medimos la distancia con ultrasonidos
int medirDerecho = ultraright.Ranging(CM);
Serial.println(Terror);
Terror = terrorMode(medirIzquierdo, medirDerecho, Terror);
navigate(medirIzquierdo, medirDerecho, Terror);
}
Carrera:
#include <Ultrasonic.h> // Importamos las librerias del modulo de ultrasonidos
#include <Servo.h> // Importamos las librerias que controlan el servo
Ultrasonic ultraleft(3,4); // Trigger pin, echo pin
Ultrasonic ultraright(5,6);
Servo servoR; // Definimos los objetos Servo
Servo servoL;
int servoPinR = 10; // Pines del servo
int servoPinL = 9;
int servoStopR = 95; // Velocidad de parada
int servoStopL = 79;
void setup() {
servoR.attach(servoPinR); // Asociamos pines a los Servo
servoL.attach(servoPinL);
}
void loop(){
{
int medirIzquierdo = ultraleft.Ranging(CM); // Medimos la distancia con ultrasonidos
int medirDerecho = ultraright.Ranging(CM);
if (medirIzquierdo >=10 && medirDerecho >=10){ // Define el modo de crucero
servoL.write(servoStopL + ( 19 * medirDerecho / 50));
servoR.write(servoStopR + ( 24 * medirIzquierdo / 50));
}
else{ // Define el modo de anti choche
servoL.write(servoStopL - 19 + ( 38 * medirDerecho / 50));
servoR.write(servoStopR - 24 + ( 48 * medirIzquierdo / 50));
delay(200);
}
}
}
No hay comentarios:
Publicar un comentario