Vistas de página en total

domingo, 1 de mayo de 2016

ESTROBOSCOPIO 1,09- 55,8 Hz LED ALTA POTENCIA REALIZADO CON PIC

     Por la red existen muchos que prometen un buen estroboscopio con micro, pero son en realidad pequeños y poco desarrollados. En cambio los más interesantes solo se venden, y a un precio descomunal.
     Dado que en realidad hacer un stroboscopio es un juego de niños, no entiendo, como no hay más documentación en la red.
     Yo la busqué y nada que valiese la pena ver.
     Por eso tuve que hacer el mío.
      Le voy a poner aquí para devolver los favores que otros me hacen colocando si información que si me sirve.
     En este caso realizaré un stroboscopio LED. Pero eso si con led de alta potencia, por lo que no tendremos problemas de luz. De hecho superará a uno flash.

  Hardware

     Para ello vamos a utilizar tres led de 30w. No necesitaremos disipadores, pues al ser pulsos muy cortos los que da el stroboscopio, no se calentaran ni un ápice.
     Los leds los podéis comprar en ebay por el módico precio de 1,07€ cada uno... Yo necesitaba mucha luz y coloque tres. Ya van 3€ para mi flash... vamos a ver si me gastaré los 400€ que me pidieron por uno... jaja, ni en sueños.
   Aquí los podéis conseguir: LEDS.

   Ya tenemos las iluminación... y para el control.... que puedo hacer... como yo lo quiero todo, quiero que tenga display y me indique la frecuencia y las revoluciones.
    Bueno tendréis que comprar un display Para poder escribir mucho que sea de 20x4. Aquí teneis el que puse yo. Display Ya llevo 8,5€.
    ¿ Y como lo controlo? Pues vamos a poner un PIC 16F88. ¿Para que más?.
    Ya está todo lo importante, solo necesitaremos añadir un par de transistores y una fuente de alimentación de 48V  suficientemente grande para poder con los LEDS.
     Cada led consume 1A, por lo que si usáis 3, como yo, necesitareis una fuente de alimentación de 3,5A.
     Si en vez de usar leds de 30W usáis leds de 10w, porque no necesitáis tanta luz, podréis encontrarlos a 12V.
     La parte más cara del sistema es precisamente esta fuente.
     Para los leds existen unos módulos de alimentación, pero para este caso no sirven, pues no son capaces de dar la corriente necesaria en un periodo tan corto como necesitamos. Si los usamos como fuente no conseguiremos encender los leds. Hace falta una fuente más estabilizada y rápida. Una Fuente conmutada.
    Aquí la tenéis de 48V 5A... para que sobre: Fuente
    Con esto llevamos 29€.

    El resto os lo dejo a vosotros, pues son componentes simples, fáciles de conseguir en cualquier tienda de electrónica.
 

    Montaje

    Ahora hay que saber que hacer y como conectarlo todo.
    Aquí os dejo el esquema. El display que he usado es paralelo, así que hay que conectar un poco más, pero como le sobra al pic entradas, no importa, gano velocidad de proceso.
 

   Como podéis ver... no puede ser mas sencillo.
   Aunque sí, puede ser más sencillo si en vez de usar un trnasistor de unión usamos un Mosfet. Entonces nos ahorramos el ULN.
    U4 es un regulador para tomar la alimentación y pasarla a 5v para alimentar el circuito. En este caso yo disponía de 24V a parte, Si se parte de los 48V es mejor usar dos reguladores en cascada ya que regular a 5 desde 48V con el 7805 es demasiado, así que deberíais poner uno para pasar a 24V.
    Si en vez de comprar una fuente de 48V usáis 2 de 12 en serie, pues ya está tomais la que de el  voltaje bajo para el circuito.
    Para asegurar la frecuencia el PIC va controlado por cristal, en este caso de 20Mhz, para que me de suficiente margen para controlar la secuencia a 100Hz.
    RV1 es la resistencia variable que configura el contraste del display
    RV3 es un potenciómetro multivuelta para seleccionar la frecuencia elegida. Sw2 permitirá indicar la ventana de selección del potenciómetro.
    Debido a que el valor total del potenciómetro solo se puede leer como 1024 valores por la entrada analógica, usaré el SW2 para indicar si el potenciómetro lo uso para frecuencias bajas o altas. Así con un solo potenciómetro podré dar 2048 puntos de resolución, aunque la entrada solo sea de 1024. Además invertiré el sentido del potenciómetro de manera que el cero quedará donde termina el maximo de la excala anterior.
   Y ya solo queda la salida a potencia. Como soy algo vago... tenía ya una unidad preparada con un uln2003. Por ello le tenía conectado ya dos salidas del PIC. Una de esas salidas la conecto a dos transistores para llevar el segundo a conmutación a 2A y dejar el sistema en lógica positiva. Es decir enciende el led cuando yo pongo 1 en la salida del PIC.
    Como veis arriba hay un conector para acoplar los 48V que necesitarán los leds.







Aquí le tenéis montado y en plena acción deteniendo el giro de una Turbina Pelton. Podéis ver los tres led. La banda oscura se produce por la sincronización entre la cámara y el destello.
  En este caso no necesito más de 58 Hz, pero es fácil subir a más si lo deseamos. Pero si no subimos el mínimo deberá ser a costa de perder resolución. Habrá que usar un PIC con mas bit en su conversor analógico o usar botones para aumentar y disminuir el valor de periodo. Eso ya queda en vuestras manos si lo necesitáis. Si por ejemplo lo programáis sobre un PIC2550 podréis dar 16 bits de resolución con lo que será facil ampliar el rango de frecuencias sin perder resolución.
  Al ser el cristal de 20Mhz cada pulso lo podemos definir de 51,2uS, pero ese pulso es demasiado pequeño para iluminar suficiente cuando el pulso se produce a velocidades bajas. La persistencia del ojo no es suficiente para llegar a ver la máquina. El ojo no llega a percibir lo que ve. Por eso el programa tiene un truco. Aumenta la duración del pulso en frecuencias bajas, para que se mantenga la misma impresión de iluminación.
  Como a frecuencias bajas el móvil se mueve más lento, no se percibe movimiento, aunque el pulso sea más largo.
   Para lograr este truco tenemos dos variables:
         Periodo --- define la frecuencia del  stroboscopio
         Ancho_pulso   --  define la longitud del destello, programado según la frecuencia

 Programa

   El programa está realizado en PICC.

#include "STROBO_INT_AN.h"

#include <LCD420.C>
struct strobo_pin_map {                 // estructura estroboscopio Puerto A
           BOOLEAN ADC;           // no usado
           BOOLEAN unused1;               // no usado
           BOOLEAN FBaja;               // Desplaza la frecuencia de 14Hz a 0Hz
           BOOLEAN led;           // salida led
           BOOLEAN Pulsador;        // bajar frecuencia
           BOOLEAN unused2;           // no usado
           BOOLEAN unused3;               // no usado
           BOOLEAN unused4;               // no usado
        } strobo;

#locate strobo = getenv("sfr:PORTA") 
#define set_tris_strobo(x) set_tris_a(x) 

struct strobo_pin_map const normal = {1,1,1,0,1,0,0,0}; // For write mode all pins are out
long periodo=0;  //guardará el numero de pulsos para formar el periodo Se usará solo 10 bits
long strobo_tmp=0;
long ancho_pulso;


#int_RTCC
void  RTCC_isr(void) 
{int1 led;
   //Interrupción cada 51.2us
  // periodo_min=0... T=350 pulsos ->17,92ms ->55,8 Hz
    //si periodo maximo 17408... T=17408+350=7408 pulsos ->909.2ms ->1.09 Hz
     led=0;
    if (--strobo_tmp==0) 
       {led=1; 
        strobo_tmp=periodo;
        strobo_tmp+=350;}  // 350 para posicionar a Fmax 55.8Hz 
     
    if(strobo_tmp<ancho_pulso) led=1;   //primeros 2 pulsos a 1. Por lo tanto pulso de 102.4us a 1
     
    strobo.led=led;
   // output_bit( PIN_B3,!led);
}



void main()
{
long hz,rpm,b;


   set_tris_strobo(normal);
   //Analogica
   setup_adc_ports(sAN0|sAN1|VSS_VDD);
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_spi(SPI_SS_DISABLED);
   //timer para pulsos
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   setup_timer_1(T1_DISABLED);

   //comparadores
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
   lcd_init();
   enable_interrupts(INT_RTCC);
   enable_interrupts(GLOBAL);
   


   set_adc_channel( 0 );
   delay_ms(20);
   // TODO: USER CODE!!

   
   while (TRUE) {
              
        set_adc_channel( 0 );
        delay_ms(100);
        b= Read_ADC();
        
       /*En frecuencias bajas el potenciometro funciona en sentido contrario, comenzando en 14Hz->1024 
        sube una resolución más alta 16 por cada valor del potenciometro, llegando a 1.09Hz-> 65rpm
       En frecuencias bajas periodo va de 1024->14,22Hz(853rpm) a 17408->1,09hz(65rpm)
       /Frecuencias altas peridodo va de 0 a 1024 55,8(3348rpm) a  14,22(853rpm)*/
       
       
       if (strobo.FBaja)
              {periodo=(1024-b)*16+1024;
                ancho_pulso=periodo/1000+1;}        
       else {periodo=b;  //Frecuencias altas peridodo va de 0 a 1024 55,8(3348rpm) a  14,22(853rpm)
            ancho_pulso=periodo/500+2;}
       
      
       /*si timer 51.2 us T=(periodo+350)*51.2/1000000  F=1/T=1000000/(periodo+350)*51.2
       Como deseamos presentar 2 decimales y solo podemos poner enteros vamos a calcular F*100
       F100=100000000/(periodo+350)*51.2= 100000000/512 /(periodo+350)=195312/(periodo+350)*/
        
       
       hz=1953125/(periodo+350); 
  

       rpm=hz*6;
       rpm/=10;
       
       
       printf(lcd_putc,"\f" );
       printf(lcd_putc,"ESTROBOSCOPIO PELTON\n Departamento X.X.X.\n" );
       printf(lcd_putc,"RECUENCIA:%4.2w Hz,\nR.P.M.: %6.0lu rpm" ,hz,rpm);
     
  


 }
}

   Espero que os haya gustado.



miércoles, 9 de diciembre de 2015

Control Belén con Pic

    Dado que llega Navidad, voy a aprovechar para añadir un circuito para controlar un belén.
    La tecnología ha cambiado en cuanto a la iluminación y ahora se pueden hacer cosas que antes eran impensables, como por ejemplo iluminar en RGB, o utilizar solo tensiones de seguridad.



     Características del circuito

  • Alimentación circuito y salidas 1A. 6-12V
  • Alimentación  salidas 2A 5-48V
  • 8  Salidas 2A.
  • 8 salidas  1A.
  • Conector ICSP
  • Conector Serire TTL para futuras ampliaciones.
  • Conector SPI. para futuras ampliaciones.
  •  

      Uso de las salidas con el firmware actual

      En esta versión inicial he implementado en el firmware unas cuantas características básicas para el belén. Eso no implica que no se puedan usar las salidas transistorizadas para otros usos en otro belén.
      En el caso que propongo actualmente el belén dispondrá de :
  • 2 Salidas de 2A con posibilidad de alimentar a 48V, para 2 conjuntos de LEDs RGB. Uno para generar el atardecer y otro para generar el amanecer
  • 2 Salidas de 2A con posibilidad de alimentar a 48V, para colocar LEDs de potencia cálidos, para la iluminación central y la transición del amanecer al atardecer.
  • 4 salidas de 1A 12V para el encedido de estrellas en 4 grupos diferentes.
  • 3 Salidas de 1A 12V para el encendido de casas en 4 grupos diferentes.
  • 1 Salida de 1A 12V para el encendido de antorchas y hogueras.
        Todo está debe de ser realizado con LEDs.
                 El atardecer y el amanecer con leds RGB de alta potencia (10W) (20W).
                 La mañana y la tarde con LEDS cálidos de potenia (20W)
                 Las estrellas con leds alto brillo agrupados en series de 4 leds o 5 leds.
                 Las casas igualmente con leds alto brillo agrupados en serie de 4 leds
                 La hogueras y antorchas con leds rojos alto brillo agrupados en series de 5 leds.

    Esquema

       Como se puede ver el esquema no puede ser más simple. Todo se hace con un Pic16F73 al que se le amplifica la potencia de las salidas mediante ULN2003 y transistores.
       Las salidas que van diréctamente a ULN2003, tendrán 1A máximo de salida, y funcionarán cerrando el circuito a masa. Es decir el + es el común y la salida debe de conectarse al negativo del diodo.
        En cambio las salidas conectadas a los transistores funcionan al revés. Es decir, el común es el negativo y las salidas deberán ser conectadas al + del diodo.


    Implementaciones en el Firmware actual

        El programa del pic está dividido en tres partes.
  1.         Control de las estrellas
  2.         Control de los fundidos
  3.         Control del ciclo.

   Control de Estrellas

          El control de las estrellas, su tintineo, está implementado en la interrupción RTCC, de forma que periódicamente se entra en esta interrupción y se valora si se deben o no apagar momentaneamente las estrellas. El tiempo que tardará entre dos tintineos es aleatorio, y diferente para cada grupo de estrellas, por lo que no se verán tintinear todas a la vez, sino por grupos. como se ve en el programa he llamado a las estrellas ESTRELLAS1 y ESTRELLAS2 y para cada una hay H y L.
     ¿Qué significa esto?   Que hay cuatro salidas de estrellas. Los grupos 1 y 2 están pensados para ser puestos por el lado del amanecer y por el lado del atardecer, y los H y L son partes del mismo conjunto de estrellas, pero que serán calculadas para que den diferente luminosidad.
     Es decir para formar la constelación de Orión, usamos Estellas1_H para las estrellas de primera magnitud y Estrellas1_L para las de segunda magnitud. Para conseguir mayor iluminación en la primera magnitud con respecto de la segunda, bastará colocar una resistencia limitadora en las de segunda magnitud.
     Así podremos emular mejor una constelación. Además en el ciclo encenderé antes las de 1ª magnitud que las de la segunda durante el atardecer. igualmente apagaré antes las de 2ª magnitud que las de 1ª magnitud durante el amanecer.
     Que haya dos grupos, 1 y 2, nos permite colocar un grupo en el amanecer y otro en el atardecer del belén, de forma que podemos encender antes las del lado de amanecer cuando atardece y apagar antes las del lado del atardecer cuando amanece.

      Control de fundidos

     Para conseguir realizar el amanecer y la transición del amanecer al atardecer, he usado fundidos, más o menos complejos.
     Para gestionar esos fundidos uso PWM. Este PWM está programado en la interrupción del TIMER1.
     En ella, según el valor hayamos dado a la intensidad de luz, 0 a 255, se generará un PWM para controlar la potencia de la salida.
     En este caso las salidas controladas por este método son los tres colores del atardecer, y los tres del amanecer, la mañana y la tarde. En total 8 salidas, que podrían ser usadas para otras cosas en el futuro.
     Para mayor comodidad en la gestión de los valores he creado dos estructuras
      La priemera Tipo_crepusculo, permite definir los colores el atardecer y el amanecer, para conseguir pasar del blanco del día al azul de la noche, a través de un rojo del atardecer.

                typedef struct { //estructura para control rgb del
                           int rojo;
                           int verde;
                            int azul;
                         }Tipo_crepusculo;
     
Esta estrucutra es usada a su vez para crear unas variables de nivel de potencia de las luces, de forma que sean fáciles de identificar y manejar en el programa.

                 struct Niveles_Str {                
                             Tipo_crepusculo LUZ_Amanecer, LUZ_atardecer;
                              int LUZ_manana;
                              int LUZ_tarde;
                              } Nivel, Nivel_tmp;

      Control de ciclo

       Una vez que tenemos la estructura de mando del sistema, solo queda pensar en el ciclo.
       El ciclo, en este caso es muy complejo. Por pasos sería
  1.  Subimos la Nivel.LUZ_Amanecer pasándolo del azul de la noche al blanco
  2. Antes incluso de alcanzar el blanco en el amanecer, comenzamos a subir la mañana, que es luces colocada a 1/3 de la longitud del belén.
  3. Cuando se llega a un nivel en la mañana comenzamos a subir la tarde, y luego subimos el Nivel.LUZ_Atardecer, con todos los colores igual ,para subir en blanco.
  4.  Esto nos ilumina completamente el belén desde el amanecer a toda la escena de forma progresiva.
  5. Una vez alcanzado el día, esperamos un tiempo y comenzamos el atardecer.
  6. Para ello vamos bajando la luz del amanecer del blanco al azul, pasando por el rojo.
  7. De forma sincronizada, vamos apagando las luces de la mañana,y la tarde. De forma que vayan en escala descendente.
  8. Cuando  el amanecer esta en rojo y está apagada casi la mañana y ya ha ido bajando la tarde, comienzo el descenso del atardecer del blanco al rojo.
  9. Para finalizar, el amanecer queda en azul, la mañana y la tarde se apagan y el rojo del atardecer pasa finalmente a azul. Se ha hecho la noche.
     Durante este proceso, deberemos ir encendiendo y apagando las estrellas, las casas y las antorchas y fuego.
     Como en el caso anterior lo iremos haciendo progresivamente de forma sincronizada con las luces del atardecer y el amanecer.
     Cuando comienza a caer la tarde, vamos encendiendo las antorchas, algunas casas y las estrellas del la de del amanecer , y cuando aparece el crepúsculo se van encendiendo las estrellas  del lado del atardecer. Todas ellas en dos fases, Primero las más intensas y luego las débiles.
     La noche queda como se desee, alternando casas y dejando encendidas las estrellas. El fuego y las casas se apaga al cabo de un tiempo.
  
      Llegamos al amanecer. Entonces, vuelvo a encender las casa y el fuego. Y volvemos a ir subiendo el amanecer del azul al blanco. Volviendo al paso 1. Durante un tiempo, mientras amanece, permanecerán las hogueras y algunas casas encendidas.
       Para implementar este ciclo, por lo tanto, en el programa he creado cuatro rutinas. Amanecer, Atardecer, Día y Noche, en las cuales se irá colocando el ciclo decada parte. Igualmente he creado constantes de tiempo para el fundido y los retardos principales, para poder variarlos de forma rápida.
  

      Mejoras

          En otra entrega usaré lasa salidas de comunicación reservadas RS-232 y SPI.
       Con el SPI controlarré tarjetas adicionales, tales como figuras con servocontroles, o motorrizadas, las cuales se crearán con su propio contról de ciclo y serán gestionadas mediante esta tarjeta mediante comandos.
       Igualmente en el RS-232, podremos conectar un Bluetooth o un ESP8266, para gestionar el ciclo, tiempos, puesta en marcha u otras características del belén mediante un móvil o internet.
       Pero eso queda para otro día.

       Descargar Control_Belén

        

viernes, 13 de noviembre de 2015

Robot 2W controlado por ESP8255

   Tenía pensado espera a tener acabado una rutina para controlar el robor en wifi, pero debido a la falta de tiempo, no puedo pasar el código inicial que hice, y que está aquí a la versión posterior con bluetooth que he puesto en la entrad anterior.
    Por ello, debido a que me han pdido un código para mover un robot mandado por ESP8266, pongo el código, tal como lo tenía en el vido de youtube, para que pueda ampliarlo y terminarlo el que lo desee.

    Igualmente coloco el MainActivity del Android del programa que está en el video.





Código Arduino Main.
#include <SoftwareSerial.h>
#include <ESP8266.h>
#include <Ultrasonic.h>
#include <Servo.h>
#include <Wire.h>
#include "Robot.h"

     


#define PASSWORD "xxxxxxxxxxxxxxxxxx"
#define SSID "xxxxxxxxxxxx"



//#define control_automatico
#define control_wifi

SoftwareSerial esp8266Serial = SoftwareSerial(10, 11);
ESP8266 wifi = ESP8266(esp8266Serial);

Robot MyRobot=Robot();


void setup() {
   
  Serial.begin(9600);
  esp8266Serial.begin(9600);
  Serial.println("Realizando Setup");
 setup_wifi();
  MyRobot.Init();
  MyRobot.Stop();
  
}

 void loop() {
unsigned int id;
int comando,i;
int length;
int totalRead,caracter;
char buffer[128] = {};
String entrada;

  if(MyRobot._Autonomo) MyRobot.Autonomo();

  //  **********Wifi
  if ((length = wifi.available()) > 0) {
       id = wifi.getId();
       caracter=wifi.read();  //lee caracter esperando comando $
       if (length > 0 && caracter=='$') {

           caracter=wifi.read();
           switch(caracter) {
               case'V':
               totalRead = wifi.readLine(buffer, 10);             // Cambia velocidad
               entrada=buffer;
                  MyRobot.SetVel((entrada.toInt()-90)/5+90);
              break;
               case'D':
               totalRead = wifi.readLine(buffer, 10);             // Cambia direccion
               entrada=buffer;
               MyRobot.SetDireccion((entrada.toInt()-90)/10+90);
                 break;
              
               case 'C':
               totalRead = wifi.readLine(buffer, 10);             // Comando
               entrada=buffer;
               comando=entrada.toInt();
               switch(comando){
                   case 0: //AVance continuo
                      if (MyRobot.EnMarcha()) MyRobot.Stop();
                      else   MyRobot.Move();
                   break;
                   case 1: //Mueve 10
                    MyRobot.Move(Av,10);
                   break;
                   case 2://Ret 10
                      MyRobot.Move(Rt,10);
                   break;
                   case 3: //GIRA  10º der
                    MyRobot.Giro(Dr,0,25);
                   break;
                   case 4: //-Gira 10 izq
                     MyRobot.Giro(Iz,,25);
                   break;
                   case 5: //GIRA 90º der
                   MyRobot.Giro(Dr,0,90);
                   break;
                   case 6: //Gira 90 izq
              MyRobot.Giro(Iz,90);
                   break;
                   case 10:
                    if(MyRobot._Autonomo) MyRobot._Autonomo=false;
                    else MyRobot._Autonomo=true;
                   break;
               }
              
               break;
              
           }
       }
   }

 
 }


void setup_wifi(){
    wifi.begin();
    wifi.setTimeout(2000);
    delay(2000);
    /****************************************/
    /******        WiFi commands       ******/
    /****************************************/
    // setWifiMode
   

//     wifi.setMode(ESP8266_WIFI_ACCESSPOINT);
//     wifi.setAPConfiguration("ESP8266", "awesomelib", 10, ESP8266_ENCRYPTION_WPA_WPA2_PSK);
//     wifi.restart();

    //Serial.println("Conectando Wifi");
    wifi.setMode(ESP8266_WIFI_STATION);



    if(!getStatus(wifi.joinAP(SSID, PASSWORD)))
                       Serial.println("Error al conectar al router");
                      

   
    // setMultipleConnections
    wifi.setMultipleConnections(true);

   
    // createServer
    //Serial.println("Creando servidor Telnet");
    wifi.createServer(23);
    }
   
    String getStatus(bool status)
    {
        if (status)
        return "OK";

        return "KO";
    }

    String getStatus(ESP8266CommandStatus status)
    {
        switch (status) {
            case ESP8266_COMMAND_INVALID:
            return "INVALID";
            break;

            case ESP8266_COMMAND_TIMEOUT:
            return "TIMEOUT";
            break;

            case ESP8266_COMMAND_OK:
            return "OK";
            break;

            case ESP8266_COMMAND_NO_CHANGE:
            return "NO CHANGE";
            break;

            case ESP8266_COMMAND_ERROR:
            return "ERROR";
            break;

            case ESP8266_COMMAND_NO_LINK:
            return "NO LINK";
            break;

            case ESP8266_COMMAND_TOO_LONG:
            return "TOO LONG";
            break;

            case ESP8266_COMMAND_FAIL:
            return "FAIL";
            break;

            default:
            return "UNKNOWN COMMAND STATUS";
            break;
        }
    }



Clase Robot
   Cabecera clase
 // ***********************************************
// Clase Robot
// Por Jos� Angel Moneo
// Gestiona el Robot creado por m�-
// LLeva dos ruedas mediante servos continuos
// Dos sensores de distancia ultras�nicos, uno adelante y oro atras
// Un sensor de audio
// Un m�dulo ESP8266 wifi
// 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 <Servo.h>
#include <Ultrasonic.h>


//ServoMotores
#define MotorI  4
#define MotorD  5


#define Av 1
#define Rt 0
#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
        boolean _Autonomo;
     private:
       int _VD,_VI;
     int _Velocidad;    //Velocidad del robot
     int _Direccion;
     int _Nataques; //cuenta ataques
      double cntEncoderD;
      double cntEncoderI;
     boolean _Marcha;
        boolean Marcha; //Robot en marcha
     //Servos
      Servo _MotorI;
      Servo _MotorD;

      //Ultrasonicos
       Ultrasonic *ultrasonicI;
       Ultrasonic *ultrasonicD;

    public:
        Robot();
    void Init();

        void SetVel(int Vel){_Velocidad=Vel; Move();}
        void SetDireccion(int direccion){_Direccion=direccion;Move();}  //Ajuste relacci�n potencias para determinar direcci�n 50 recto
        void Giro(boolean sent,int dist,int angulo);
        void Move();
        void Move(boolean sent,int Dist); //Mueve los servos hasta una posici�n
        void Stop();
        boolean EnMarcha(){return _Marcha;}  // test estado robot

// sensores de distancia
        int DistFront(boolean medicion);
        int DistBack(boolean medicion);
        void  Huida();
        void Autonomo();
      
    private:
       void  Amenaza(int v);
       int MinDist();
      void msg(String m,int val);
};


#endif




Funciones clase

#include "Robot.h"
#include <Servo.h>
#include <Ultrasonic.h>




Robot::Robot(){
    ultrasonicI=new Ultrasonic(TRIGGER_PINI, ECHO_PINI);
    ultrasonicD=new Ultrasonic(TRIGGER_PIND, ECHO_PIND);

    };



void Robot::Init(){
    pinMode(MotorI, OUTPUT);
    pinMode(MotorD, OUTPUT);
    _MotorI.attach(MotorI);
    _MotorD.attach(MotorD);
    _VD=_VI=_Velocidad=_Direccion=90;
    _Marcha=false;
    // inicialización compas
    Wire.begin();
    Stop();
   
   
}


void Robot::Move()
{  //Da orden de velocidades para movimiento conituno
    if(_Direccion>90) //Direccion a derecha
      {
          _VD=_Velocidad;
           if (_Velocidad<90)  _VI=90-(90-_Velocidad)/(_Direccion-90);
           else _VI=(_Velocidad-90)/(_Direccion-90)+90;
      }
     
     if(_Direccion<90) //Dirección a Izquierda
         {
             _VI=_Velocidad;
            if (_Velocidad<90)  _VD=90-(90-_Velocidad)/(90-_Direccion);
            else _VD=(_Velocidad-90)/(90-_Direccion)+90;
           
         }
      if(_Direccion==90)
      {
             _VI=_Velocidad; _VD=_Velocidad;
      }
    _MotorI.write(180-_VI); _MotorD.write(_VD);
    //ShowParam();
    _Marcha=true;
}

void Robot::Move(boolean sent,int Dist)
{  //invertimos el sentido si es necesario
    if (sent==Av) _VI=_VD=abs(_Velocidad-90)+90;
    if (sent==Rt) _VI=_VD=90-abs(_Velocidad-90);
    _MotorI.write(180-_VI); _MotorD.write(_VD);
    delay(Dist*10);
    Stop();
    _Marcha=false;   
}

void Robot::Stop()
{_MotorD.write(90);
_MotorI.write(90);   
_Marcha=false;
   
}


void Robot::Giro(boolean sent,int angulo)
{

    if (sent==Dr) _VI=_VD=abs(_Velocidad-90)/2+1+90;
    if (sent==Iz) _VI=_VD=90-abs(_Velocidad-90)/2-1;

_MotorI.write(_VI); _MotorD.write(_VD);

  }

  delay(angulo*10); 

  Stop();

   
}


///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;
    if (av<10) av=0;
    if (av>35) av=35;
    if (av!=0) av-=5;
    return av;
}


int Robot::DistBack(boolean medicion)
{  float Dist;
    int av,i;

    if (medicion==1)
    {Dist=0;
        for(i=1;i<4;i++)    Dist+= ultrasonicI->convert(ultrasonicI->timing(), Ultrasonic::CM);

    return Dist/2;}
    else
    Dist = ultrasonicI->convert(ultrasonicI->timing(), Ultrasonic::CM);
   

    av=(int)Dist;
    if (av<10) av=0;
    if (av>35) av=35;
    if (av!=0) av-=5;
    return av;
}

int Robot::MinDist()
{int i,d,maximo;

    // Busquea distancia minima
    maximo=DistFront(1);

    for (i=0;i<10;i++)
    {
        Giro(1,30);
        if ((d=DistFront(1))<maximo) maximo=d;
        if ((d=DistFront(1))<maximo) maximo=d;
       
    }
    return maximo;
}

void Robot::Huida()
{int d,dd;
   
        //msg("inicio huida ",0);
        //prueba de huida. 10 veces
        //Ataque por detras
        //msg("Espera acoso ",0);
        if(DistBack(1)<10) {_Nataques++;
                 SetVel(95);
                 SetDireccion(89);
            //SE ALEJA,manteniendose a 30 cm
            while ((d=DistBack(1))<10)
            { if((dd=DistFront(1))<20)  {Giro(Dr,90);/* msg("Gira 90 ",dd);*/ } //Giro 90º si no tengo espacio delante
            Move(Av,30);
            // msg("Avanza ",30-d);
        }
        if (_Nataques==5) Amenaza(0);
        }
        //Ataque por delante
        if(DistFront(0)<10){ _Nataques++;
                 SetVel(95);
                 SetDireccion(89);
            //SE ALEJA,manteniendose a 30 cm
            while ((d=DistFront(1))<10)
            { if((dd=DistBack(1))<20)  {Giro(Iz,90);/* msg("Gira 90 ",dd);*/ } //Giro 90º si no tengo espacio delante
            Move(Rt,30);
            //msg("Retrocede ",30-d);
        }
        if (_Nataques==5) Amenaza(1);
         }

   
   
   

}


void Robot::Amenaza(int v)
{int i,d,dd;
    SetVel(120);
    //por detras
        if(v==0) {   
           
        Giro(Dr,90);
        Giro(Dr,90);
        // msg("Caballo loco",0);
        //Caballo loco
        for(i=0;i<5;i++){
            Move(Av,15);
            Move(Rt,15);
            _Nataques=0;
        }
    }
    // por delante
    if(v==1) {
        // msg("Caballo loco",0);
        //Caballo loco
        for(i=0;i<5;i++){
            Move(Av,15);
            Move(Rt,15);
            _Nataques=0;
        }
    }
   
}



void Robot::Autonomo()
{ int d;
     SetVel(95);
     SetDireccion(88);
   
    //Recorrido autonomo
    // msg("Programa Autonomo ",0);
    //Busqueda pared
    while ((d=DistFront(1))>10)   Move(Av,d);
   
    while ((d=DistFront(1))<11) Giro(Dr,45);
   
}

void Robot::msg(String m,int val)
{ Serial.print(m);
  Serial.println(val);
  }


MainActivity de Android
package com.example.joseangel.Robot;

import android.support.v7.app.ActionBarActivity;
import android.view.Menu;
import android.view.MenuItem;

import java.io.BufferedWriter;
import java.io.IOException;
import java.io.OutputStreamWriter;
import java.io.PrintWriter;
import java.net.InetAddress;
import java.net.Socket;
import java.net.UnknownHostException;
import android.graphics.Color;
import android.os.Bundle;
import android.util.Log;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.EditText;
import android.widget.SeekBar;
import android.widget.TextView;
import android.view.View;
import android.widget.ImageView;


public class MainActivity extends ActionBarActivity {
    private Button btconect, btdisconect, btsndtxt, btnizq, btnder, bt0, bt1,
            bt2,bt3,bt4,bt5,bt6,bt10;
    private SeekBar vel,dir;
    private TextView txtstatus;
    private EditText ipinput, portinput, input_txt;
    private ImageView leds;
    private boolean connected = false;
    private Socket socket;
    private String serverIpAddress = "192.168.1.17";
    private static final int REDIRECTED_SERVERPORT = 23;

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        leds = (ImageView) findViewById(R.id.leds);
        btconect = (Button) findViewById(R.id.btcnt);
        btdisconect = (Button) findViewById(R.id.btdisc);
        bt0 = (Button) findViewById(R.id.bt0);
        bt1 = (Button) findViewById(R.id.bt1);
        bt2 = (Button) findViewById(R.id.bt2);
        bt3 = (Button) findViewById(R.id.bt3);
        bt4 = (Button) findViewById(R.id.bt4);
        bt5 = (Button) findViewById(R.id.bt5);
        bt6 = (Button) findViewById(R.id.bt6);
        bt10 = (Button) findViewById(R.id.bt10);
        txtstatus = (TextView) findViewById(R.id.txtstatus);
        vel = (SeekBar) findViewById(R.id.Posicion);
        dir = (SeekBar) findViewById(R.id.Direcc);

        vel.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener(){

            @Override
            public void onProgressChanged(SeekBar seekBar, int progress,
                                          boolean fromUser) {
                // TODO Auto-generated method stub
                String msg;
                msg=String.valueOf(progress);  // o Integer.toString(pos_bar):
                boolean val_acc = Snd_Msg("$V"+msg);
                //error al enviar
                if (!val_acc) {
                    Set_txtstatus(" Error  ", 0);
                    Change_leds(false);
                    Log.e("Snd_Action() -> ", "!ERROR!");

                }

                if (!socket.isConnected())
                    Change_leds(false);

            }

            @Override
            public void onStartTrackingTouch(SeekBar seekBar) {
                // TODO Auto-generated method stub
            }

            @Override
            public void onStopTrackingTouch(SeekBar seekBar) {
                // TODO Auto-generated method stub
            }
        });
       dir.setOnSeekBarChangeListener(new SeekBar.OnSeekBarChangeListener(){

            @Override
            public void onProgressChanged(SeekBar seekBar, int progress,
                                          boolean fromUser) {
                // TODO Auto-generated method stub
                String msg;
                msg=String.valueOf(progress);  // o Integer.toString(pos_bar):
                boolean val_acc = Snd_Msg("$D"+msg);
                //error al enviar
                if (!val_acc) {
                    Set_txtstatus(" Error  ", 0);
                    Change_leds(false);
                    Log.e("Snd_Action() -> ", "!ERROR!");

                }

                if (!socket.isConnected())
                    Change_leds(false);

            }

            @Override
            public void onStartTrackingTouch(SeekBar seekBar) {
                // TODO Auto-generated method stub
            }

            @Override
            public void onStopTrackingTouch(SeekBar seekBar) {
                // TODO Auto-generated method stub
            }
        });
        //Botones de Accion
        bt0.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(0);
            }
        });

        bt1.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(1);
            }
        });

        bt2.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(2);
            }
        });

        bt3.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(3);
            }
        });
        bt4.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(4);
            }
        });

        bt5.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(5);
            }
        });

        bt6.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(6);
            }
        });

        bt10.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                Snd_Action(10);
            }
        });


        //Al clickear en conectar
        btconect.setOnClickListener(new OnClickListener() {
            @Override
            // conectar
            public void onClick(View v) {
                //Nos conectamos y obtenemos el estado de la conexion
                boolean conectstatus = Connect();
                //si nos pudimos conectar
                if (conectstatus) {//mostramos mensaje
                    Set_txtstatus("Conexion OK ", 1);
                    Change_leds(true);//camiamos img a verde

                } else {//error al conectarse
                    Change_leds(false);//camiamos img a rojo
                    //mostramos msg de error
                    Set_txtstatus("Esconectado.. ", 0);
                }
            }
        });
        //Al clickear en desconectar
        btdisconect.setOnClickListener(new OnClickListener() {
            @Override
            public void onClick(View v) {
                boolean conectstatus=Disconnect();
                if (conectstatus) {//mostramos mensaje
                    Set_txtstatus(".. En espera ..", 1);
                    Change_leds(false);//camiamos img a verde

                }
                else {//error al conectarse
                    Change_leds(false);//camiamos img a rojo
                    //mostramos msg de error
                    Set_txtstatus("Desconectado.. ", 0);
                }

            }
        });


    }


    @Override
    public boolean onCreateOptionsMenu(Menu menu) {
        // Inflate the menu; this adds items to the action bar if it is present.
        getMenuInflater().inflate(R.menu.menu_main, menu);
        return true;
    }

    @Override
    public boolean onOptionsItemSelected(MenuItem item) {
        // Handle action bar item clicks here. The action bar will
        // automatically handle clicks on the Home/Up button, so long
        // as you specify a parent activity in AndroidManifest.xml.
        int id = item.getItemId();

        //noinspection SimplifiableIfStatement
        if (id == R.id.action_settings) {
            return true;
        }

        return super.onOptionsItemSelected(item);
    }


    //Enviamos mensaje de accion segun el boton q presionamos
    public void Snd_Action(int bt) {
        String msg="";
        //no hay texto

        //seteo en el valor action el numero de accion
        switch (bt) {
            case 0:
                msg = "$C0";
                break;
            case 1:
                msg = "$C1";
                break;
            case 2:
                msg = "$C2";
                break;
            case 3:
                msg = "$C3";
                break;
            case 4:
                msg = "$C4";
                break;
            case 5:
                msg = "$C5";
                break;
            case 6:
                msg = "$C6";
                break;
            case 10:
                msg = "$C10";
                break;
        }
             //mando msg
        boolean val_acc = Snd_Msg(msg);
        //error al enviar
        if (!val_acc) {
            Set_txtstatus(" Error  ", 0);
            Change_leds(false);
            Log.e("Snd_Action() -> ", "!ERROR!");

        }

        if (!socket.isConnected())
            Change_leds(false);
    }


    /*Metodo para enviar mensaje por socket
     *recibe como parmetro un objeto Mensaje_data
     *retorna boolean segun si se pudo establecer o no la conexion
     */
    public boolean Snd_Msg(String msg) {
        try {
            //Accedo a flujo de salida
            PrintWriter out = new PrintWriter(new BufferedWriter(new OutputStreamWriter(socket.getOutputStream())),true);
            if (socket.isConnected())// si la conexion continua
            {
                //Envio mensaje por flujo
                out.println(msg);
                //envio ok
                return true;
            } else {//en caso de que no halla conexion al enviar el msg
                Set_txtstatus("Error...", 0);//error
                return false;
            }

        } catch (IOException e) {// hubo algun error
            Log.e("Snd_Msg() ERROR -> ", "" + e);
            return false;
        }
    }


    //cambia el imageview segun status
    public void Change_leds(boolean status) {
       if (status)
           leds.setImageResource(R.drawable.on);
        else
           leds.setImageResource(R.drawable.off);
    }

    /*Cambiamos texto de txtstatus segun parametro flag_status
     * flag_status 0 error, 1 ok*/
    public void Set_txtstatus(String txt, int flag_status) {
        // cambiel color
        switch (flag_status){
        case 0:
            txtstatus.setTextColor(Color.RED);
             break;
        case 1:
            txtstatus.setTextColor(Color.GREEN);
            break;
        case 2:
            txtstatus.setTextColor(Color.BLACK);
            break;
        }
        txtstatus.setText(txt);
    }

    //Conectamos
    public boolean Connect() {
        //Obtengo datos ingresados en campos
        try {
            InetAddress serverAddr = InetAddress.getByName(serverIpAddress);
            socket = new Socket(serverAddr, REDIRECTED_SERVERPORT);
            //si nos conectamos
            if (socket.isConnected() == true) {
                return true;
            } else {
                return false;
            }
        } catch (UnknownHostException e1) {
            e1.printStackTrace();
            //Si hubo algun error mostrmos error
            txtstatus.setTextColor(Color.RED);
            txtstatus.setText(" !!! ERROR  !!!");
            Log.e("Error connect()","");
            return false;
        } catch (IOException e1) {
            e1.printStackTrace();
            //Si hubo algun error mostrmos error
            txtstatus.setTextColor(Color.RED);
            txtstatus.setText(" !!! ERROR  !!!");
            Log.e("Error connect()","");
            return false;

        }

    }

    //Metodo de desconexion
    public boolean Disconnect() {

        //Prepramos mensaje de desconexion
        //avisamos al server que cierre el canal
        try {
        socket.close();
        } catch (IOException e) {
            // TODO Auto-generated catch block
            e.printStackTrace();
            return false;
        }
        if (socket.isConnected())
             return false;
        return true;
    }

}
 

jueves, 1 de octubre de 2015

Robot 2W controlado por bluetooth

  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.
   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