Hoy pongo aquí una pequeña prueba para usar el bluetouth para comandar un robot.
Para facilitar las cosas he utilizado un programa de control ya diseñado para el android.
Entre todos los programas de control de robots que están disponibles en google play, he elegido el Arduino Joystick Controller. Es muy completo y sencillo a la vez. En enste caso nos sobran opciones, pues para empezar solo moveremos el robot. Pero nos queda preparado para futuras ampliaciones. La parte mñas interesante del progrma es el hecho de que pueda ya hayan implementado la visualización de dcatos, y el hecho de que podamos parametrizar los límites y valores de los mandos, de forma que podremos ajustar la respuesta de los motores sin tener que reprogramar el arduino. Simplemente variaremos los límites y la posición cero de los slider.
El robot lleva solo dos ruedas, pero controladas por servocontroles paralax de movimiento continuo de alta velocidad. Pueden verse sus características aquí.
Los servos son más fáciles de controlar que los motores de continua, sobre todo a velocidades bajas. Este servo puede ir de 0 a 60hz. Las señales de control son mucho menos, pero un poco más difíciles de gestionar a nivel de soft. Pero para mí son ventajas.
Otra ventaja del servo es su facil colocación. Aunque en la foto no se vea, no están atornillados. Simplemente están pegados con cinta adesiva doble cara al chasis.
Otra ventaja del servo es su facil colocación. Aunque en la foto no se vea, no están atornillados. Simplemente están pegados con cinta adesiva doble cara al chasis.
Como hemos dicho el hard se simplifica, pues solo necesitamos una salida por servo paracontrolar su sentido y velocidad. La salida funciona en PWM de forma que da un pulso de duracion definida cada 20ms. La duración de ese pulso definirá la velocidad y sentido del servo.
En este punto encontraremos un problema con el arduino. La librería servo del arduino, genera el PWM mediante una interrupción del Timer1. Esta interrupción que se usa para medir tiempos y determinar cuando encender y apagar la salida resulta ser la misma que se utiliza para softserial. Es decir está compartida. Esto es un problema, pues si deseamos leer y enviar muchos datos por el puerto serie y a la vez hacer PWM, es muy posible que en el momento que se deba bajar la señal del PWM el micro esté ocupado gestionando el puerto serie. Eso hará que se alargue el tiempo del pulso PWM y se trasmita una velocidad indebida. Esto se traduce en un tembleque de los servos durante el control.
Para evitarlo solo hay dos maneras. O usamos PWM por hardware, o usamos puerto serie por hardware. El segundo caso no podemos pues ya está ocupado para el usb, y la primera forma no es facil, pues el arduino ya ha implementado la función servo y manejar el PWM a perlo del micro solo nos serviría para 1 servo, pues no tiene mas que una señal de salida PWM por hard. Al menos el uno y el leonardo.
Entonces la mejor solución, y además preparada para ampliaciones del robot, es compar una tarjeta de 16 PWM contolada por I2C.
De esta forma dispondremos de 14 salidas de servo en reserva. El único problema es que debido a la duración el ciclo PWM del servo son 20ms, y en cambio el pusso debe de ser de 1,5ms, la resolución efectiva del PWM para controlar el servo es muy pequeña. En concreto 24 puntos. Pero suficiente, ya que nos da 24 velocidades.
Inicialmente solo le pondré quí un movimiento comandado remoto. Mediante este modo, guiaremos el robot como un coche teledirigido, pudiendo parar, ir marcha atras y adelante y girar. Mas adelante añadiré otras funciones.
Para poder hacer mejoras futuras en mi robot, he construido una clase específica para el robot, en la que podré ir incluyendo las funciones que vaya implementando, o derivarla en futuros proyectos.
La rutina de control de comunicaciones la he dejado dentro del bucle principal del programa, pues es la parte que quiero testear..
Al robot le he implementado un sensor ultrasonico por delante y otro por detrás para futuras funcione. En este caso simplemente pasaré la medida de distancia a través del bluetooth al Joystick Controller.
Como es de enteneder la comunicación entre el robot y el android se hará por bluetooth, conectando este a los pines 10 y 11 del arduino.
Programa principal
/************************************************************* Programa de control de Robot
* Basado en la clas Robot, integra la comunicación y comandos
* Por José Ángel Moneo
* Escrito para Arduino 09-09-15
* Se integra la comuniciación con bluethooth
* Con los comandos compatibles con la aplicacion
* Arduino Joystick Controller
* https://play.google.com/store/apps/details?id=com.andico.control.joystick
* Explicacion comandos en
* https://sites.google.com/site/bluetoothrccar/home/6-Joystick-Control
*
* Estructura del Mensjase mando 4 bytes
* Byte 0 Comando 243=0xF3 parado, 241=0xF1 avance, 242=0xF2 retroceso, 244=0xF4 cabeza 245=0xF5 Apagar todo
* Byte 1 Velocidad
* Byte 2 Dirección
* Byte 3 Banderas
* bit 0 E
* bit 1 D
* bit 2 C
* bit 3 B
* bit 4 A
* bit 5 claxon
* bit 6 Freno
* bit 7 Luces
*
* Estructura recepción sensores 14 bytes
* Byte 0 Inicio datos = 0xee
* Byte 1-4 Dato 1 coma flotante
* Byte 5-8 Dato 2 coma flotante
* Byte 9 Dato 3 byte
* Byte 10 Dato 4 Byte
* Byte 11 Dato 5 Byte
* Byte 12 Dato 6 Byte
* Byte 13 Fin datos = 0xcc
*
***********************************************************/
#include <SoftwareSerial.h>
#include <Ultrasonic.h>
#include <Adafruit-PWM-Servo-Driver-Library-master/Adafruit_PWMServoDriver.h>
#include <Wire.h>
#include "Robot.h"
SoftwareSerial blue = SoftwareSerial(10, 11);
Robot MyRobot=Robot();
#define btE 0
#define btD 1
#define btC 2
#define btB 3
#define btA 4
#define btClaxon 5
#define btFreno 6
#define btLuces 7
//Estructura de unión para los datos de los sensores a enviar.
// bytes a enviar al mando bluetooth
union u_sensor{
byte a[4];
float b;
};
u_sensor Sensor0;
u_sensor Sensor1;
byte Sensor2,Sensor3,Sensor4,Sensor5;
// bytes recibidos del mando bluetooth
int mando=0,velocidad,direccion,botones;
unsigned int id=0; //Bandera de inicio de comandos
unsigned long timer0; //sobrepaso tiempo datos recibidos
unsigned long timer1; //Tiempo para siguiente envío
//Lectura comando
void mando_read()
{ byte n;
//Si pasa demasiado tiempo sin respuesta para el robot y vuelve a esperar primer comando
if((millis() - timer0)>400){
MyRobot.Stop();
id=0;
mando=0;
}
if (blue.available() > 0 ) {
n=blue.read();
if (n==241||n==242||n==243||n==244||n==245) {
while (blue.available() <3); //Espera el resto de los datos
mando=n;
velocidad=blue.read(); //SE enviará
direccion=blue.read();
botones=blue.read();
timer0 = millis();
}
}
}
void mando_write()
{ byte three[14] = {0xee,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xcc};
if((millis() - timer1)>=477){ //Check if it has been 477ms since sensor reading were sent
//Break the sensor0 float into four bytes for transmission
three[1] = Sensor0.a[0];
three[2] = Sensor0.a[1];
three[3] = Sensor0.a[2];
three[4] = Sensor0.a[3];
//Break the sensor1 float into four bytes for transmission
three[5] = Sensor1.a[0];
three[6] = Sensor1.a[1];
three[7] = Sensor1.a[2];
three[8] = Sensor1.a[3];
//Get the remaining reading from the analog inputs
three[9] = MyRobot.DistFront(1); //Muesta la distancia frontal
three[10] = Sensor3;
three[11] = Sensor4;
three[12] = Sensor5;
//Send the six sensor readings
blue.write(three,14);
//Store the time when the sensor readings were sent
timer1 = millis();
}
}
void setup() {
Serial.begin(9600);
blue.begin(9600);
Serial.println("Realizando Setup");
MyRobot.Init();
Serial.println("Fin Setup");
}
void loop() {
int comando,i;
String entrada;
//Lectura de comandos
mando_read();
//ShowComand();
//////////////////Gestión de comandos
switch(mando) {
case 0xF1: //Avanzar
// velocidad adelante
//recodificamos la velocidad para usar signo para el sentido. El programa de android está configurado para enviar de 2 a 24 con centro en 0
MyRobot.SetVel(velocidad);
MyRobot.SetDireccion(direccion);
MyRobot.Move();
break;
case 0xF2: //Retroceder
// velocidad Atras
MyRobot.SetVel(-velocidad);
MyRobot.SetDireccion(direccion);
MyRobot.Move();
break;
case 0xF3: // parado
MyRobot.Stop();
break;
case 0xF5: //detener todo
MyRobot.Stop();
break;
}
mando_write(); //envía los datos al android
}
Robot.cpp
#include "Robot.h"#include <Adafruit-PWM-Servo-Driver-Library-master/Adafruit_PWMServoDriver.h>
#include <Ultrasonic.h>
Robot::Robot(){
ultrasonicI=new Ultrasonic(TRIGGER_PINI, ECHO_PINI);
ultrasonicD=new Ultrasonic(TRIGGER_PIND, ECHO_PIND);
pwm= new Adafruit_PWMServoDriver(0x41);
};
void Robot::Init(){
_VD=_VI=_Velocidad=_Direccion=90;
_Marcha=false;
Wire.begin(); //inicialización I2C
//Inicialización Driver PWM
pwm->begin();
pwm->setPWMFreq(50); //Pulsos cada 20ms como indica elmanual de paralax (50Hz)
//Punto parada del servo 1,5ms. Maxima velociad atras en 1,3ms y adelante en 1,7ms
Stop();
}
//asigna velociad al servo -180 a 180 dado que el sevo alcanza 180rpm en cada sentido
// -180 rpm son 1,3ms, 0 pulso 1,5ms, 180 rpm de 1,7ms
// por lo tanto 180 rpm equivale a 0,2ms indicando el signo sumar o restar
// 20 ms del periodo son 4096 pulsos
// Debemos generar el pwm a 50Hz, 20ms según el manual del servo.
//Dado que la resolución del pca9658 es 12 bits (4096) tendremos 4096/20=204 pulsos por milisegundo
//Como el margen de regulación del servo está entre 1,3 y 1,7ms nos deja 0,4ms pararegular
// con la resolución del PCA9658 a esta frecuenca nos deja 0,4ms*204puslos por ms= 122 pulsos para definir todo el rango, 61 para cada lado
// Como el punto central son 1,5ms 1,5*204,8=307,2 pulsos sería el punto de parada.
// Como el servo tiene una cierta regulación provando se ve el punto central en 310ms, por lo que el rango de movimiento va de 250-310-370
// Experimentalmente se ha visto que parece estar mas bien en 260-310-360
//*********Por todo esto dejo Centro en 310 pulsos. La velocidad se codificará: -24,0,24
void Robot::setServoVel(uint8_t nservo, double Velocidad)
{
pwm->setPWM(nservo, 0, 310+Velocidad); //recodifica la velocidad
}
void Robot::Move()
{ int vi,vd;
if(_Velocidad==_OldVelocidad && _Direccion==_OldDireccion) return;
vi=vd=_Velocidad;
if (_Direccion<50) vd=_Velocidad*(_Direccion)/50;
if (_Direccion>50) vi=_Velocidad*(100-_Direccion)/50;
_VI=vi;_VD=-vd;
setServoVel(MotorD, _VD);
setServoVel(MotorI, _VI);
_Marcha=true;
_OldVelocidad=_Velocidad;
_OldDireccion=_Direccion;
}
void Robot::Stop()
{
_VI=_VD=0;
setServoVel(MotorD, _VD);
setServoVel(MotorI, _VI);
_Marcha=false;
_OldVelocidad=0;
_OldDireccion=0;
}
///Sensores Distancia
int Robot::DistFront(boolean medicion)
{ float Dist;
int av,i;
if (medicion==1)
{Dist=0;
for(i=1;i<2;i++) Dist+= ultrasonicD->convert(ultrasonicD->timing(), Ultrasonic::CM);
return Dist/2;}
else
Dist = ultrasonicD->convert(ultrasonicD->timing(), Ultrasonic::CM);
av=(int)Dist;
return av;
}
int Robot::DistBack(boolean medicion)
{ float Dist;
int av,i;
if (medicion==1)
{Dist=0;
for(i=1;i<2;i++) Dist+= ultrasonicI->convert(ultrasonicI->timing(), Ultrasonic::CM);
return Dist/2;}
else
Dist = ultrasonicI->convert(ultrasonicI->timing(), Ultrasonic::CM);
av=(int)Dist;
return av;
}
Robot.h
// ***********************************************
// Clase Robot
// Por Jos� Angel Moneo
// Gestiona el Robot creado por m�-
// LLeva dos ruedas mediante servos continuos
// Tarjeta de control de servos PWMServoDriver de 12 bits 16 canales Canales 0 y 1 para las ruedas
// Dos sensores de distancia ultras�nicos, uno adelante y oro atras
// Un m�dulo BLUETOOTH
// No necesitaremos calibrado de motores, pues cada motor se calibra independientemente al ser servos
// ****************************************************
#ifndef Robot_h
#define Robot_h
#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <Ultrasonic.h>
#include <Wire.h>
#include <Adafruit-PWM-Servo-Driver-Library-master/Adafruit_PWMServoDriver.h>
//ServoMotores asignación de salidas PWM del la tarjeta driverpwm a cada rueda
#define MotorI 0
#define MotorD 1
#define AV 1 //Avancxe
#define RT 0 //Retroceso
#define ST 2 //Parado
#define Iz 0
#define Dr 1
//ULTRASONIDOS
#define TRIGGER_PIND 6
#define TRIGGER_PINI 7
#define ECHO_PIND 8
#define ECHO_PINI 9
#define sonido A7
//D2 y D3 reservadas para encoder
//Reservadas SPI 10,11,12,13
//Reservadas I2C A4-A5
//Reservada Analogicas A7
//Libres Generales 5,6,A0,A1,A2,A3
class Robot
{ public:
int PosMI,PosMD; //posiciones de los motores
int _VD,_VI;
private:
int _Velocidad,_OldVelocidad; //Velocidad del robot
int _Direccion,_OldDireccion;
boolean _Marcha;
//Servos
Adafruit_PWMServoDriver *pwm;
//Ultrasonicos
Ultrasonic *ultrasonicI;
Ultrasonic *ultrasonicD;
public:
Robot();
void Init();
void SetVel(int Vel){_Velocidad=Vel; }
void SetDireccion(int direccion){_Direccion=direccion;} //Ajuste relacci�n potencias para determinar direcci�n 50 recto
void Move();
void Stop();
boolean EnMarcha(){return _Marcha;} // test estado robot
// sensores
int DistFront(boolean medicion);
int DistBack(boolean medicion);
private:
void setServoVel(uint8_t nservo, double rpm); //Asigna la velocidad rpm en pulsos pwm al servo servo
};
#endif
