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

}
 

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

FILTRO DE MEDIA MOVIL

  Introducción

   En esta entrada voy a resolver un típico problema que nos surge al leer sensores e intentar mostrar o tomar los datos.

   Normalmente los sensores, tienen un error suficientemente grande como para distorsionarnos la lectura. Por ejemplo, un termopar tipo k puede tener un error de +-2,2ºC. Suficiente, para volvernos locos la lectura.

   Pero eso no significa que mida mal, sino que va a estar oscilando sobre la medida real 2,2 grados.

   En el caso de otros sensores como son presostatos podremos encontrar igualmente un error sobre el fondo de escala de un 1%.

 Posíblemente el error de la sonda y la precisión de la medida no sean iguales. Si leemos un sensor con un 1% de error con un ADC de más de  7 bits estaremos viendo oscilaciones en la medida que no son realmente oscilaciones de presión sino errores en las medidas del sensor.

   Por lo tanto según esto ¿Para qué usar entradas analógicas  de 16 bits para leer un sensor, si todo lo  vamos a leer error?.

   Cuando leamos con 10 o 16 bits, vemos unas oscilaciones como ruido, que nos impedirán determinar exactamente donde se encuentra la sonda. Si visualizamos el dato en un display y lo hacemos rápido, veremos oscilar constantemente el dígito de menos peso, y si  lo hacemos cada segundo estaremos leyendo datos que aunque nos parezcan correctos, tienen un error.

   Planteamiento

  ¿ Entonces que podemos hacer?, ¿Usar solo resoluciones bajas?. Pues no.

    Aunque la sonda tiene un error, por ejemplo 2,2º en el termopar. Este error siempre oscila alrededor de la medida real. por lo tanto el error es como un ruido blanco aleatorio en radio.

   Por ello podremos promediarlo con un filtro de MEDIA MÓVIL, y obtener el valor real, con precisión.

   Si las medidas se realizan de forma que se procesen posteriormente, lo más correcto es tomar las medidas reales y almacenarlas tal cual, para poder aplicar el filtro posteriormente con un programa, de forma que pueda variarse las condiciones del filtro según se desee e incluso probar con varias opciones.

   Pero si lo que queremos es mostrarlo en un display, tenemos un pequeño problema.

  

    En un filtro de media móvil de largo M, la salida actual consiste en el promedio de las últimas M muestras de la entrada.

   La función de transferencia de este filtro es:

  

     M> significa más suavizado, y M< menos suavizado

   Para tomar el valor filtrado de un punto de la onda debemos conocer el valor real de la últimas M medidas.

 

   Solución

   Para que podamos utilizar los datos del display estos deben refrescarse con una cierta cadencia. Es decir debemos mostrar el dato con unos 200ms mínimo de cadencia. Más rápido es tontería, porque no veremos nada.

  Aprovechando este tiempo, normalmente se realiza un bucle de M lecturas para calcular la media y mostrarla en el display, Esto realmente genera un retraso de M lecturas y si el sensor es lento , como es el caso de un sensor de temperatura  DS18B20, o un sensor MAX6675,  la respuesta en el visor es muy lenta.
  Por ejemplo el MAX6675 debemos leerlo cada 200ms si queremos asegurarnos que de la lectura sea correcta. Si queremos un filtro de orden 5 (M=5), esto nos dará un bucle de 1000 ms. Por lo que no podremos refrescar la pantalla en intetervalos inferiores a un segundo. Si el orden es mayor el refresco está más comprometido aún.

  Para evitar este retraso voy a hacer una función que realice la meda sobre una cola. Así podré refrescar el dato en cada lectura del sensor, aun aplicando el filtro. Dado que los datos están en una pila, solo iré añadiendo la última lectura real a la pila en cada lectura al tiempo que realizaré la media con las M-1 lecturas anteriores que ya están en la pila. Así puedo conseguir un refresco de los datos cada 200ms independientemente del número de muestras que tome.
  El único retraso que veré al aumentar M  será el  retraso a  la respuesta a un escalón, que irá aumentando al aumentar M.

         

    Operatividad

   Para realizar el filtro, vamos a realizar una pila de M datos. Iremos empujando los datos que vayamos leyendo sobre la pila. En cada lectura realizaremos el cálculo del valor medio a mostrar. Trasladando los valores de la formula a la pila, y considerando que siempre almaceno el nuevo dato en x[0] empujando los demás hacia el fondo, el valor más antiguo está en x[M] y el más actual en x[0].

  Hay que tener en cuenta que un filtro genera un retraso. El filtro provoca una respuesta más lenta ante escalones bruscos. Esto es una desventaja y una ventaja.  El fin precisamente del filtro es eliminar los picos y escalones de frecuencia alta, que no dan datos, sino ruido y tomar una media con ellos para mostrar un valor más real. Pero si necesitamos mostrar  datos en una señal con rápidos cambios, puede que el filtro nos genere un retraso adicional. Este retraso se verá en el encendido inicial o cuando la sonda se conecte o desconecte, pues en estos casos el salto es muy elevado.
  Como el valor mostrado es la media, esta variará de forma progresiva desde el valor inicial al valor final al producirse un escalón. Si El escalón resultase muy alto, aparecerán medidas de valores intermedias durante algunos segundos. Pero esto ya es algo que estamos acostumbrados a ver en los sistemas comerciales.
   En cambio veremos la medida con mucha más precisión y estabilidad, eliminando reduciendo drásticamente el error del sensor.

   Por lo tanto, hay que valorar el parámetro M del filtro. Cuanto mayor sea más estable se verá la medida, y mejor filtrará el ruido, pero solo servirá para sistemas con alta inercia. Si el sistema puede variar sus parámetros de forma rápida habrá que reducir M a 3 o menos, para que el sistema tenga una respuesta más adecuada.

   En este gráfico puede verse la diferencia entre M altos y bajos.

  Gráfico tomado de "Introducción a los Filtros Digitales clase 10"

Realizaré el código en C para Pic.

Código 1

 

////////////////////////////////////////////////////////////////////
// Librer¡a Filtro Media Movil                                    //
// compilador PIC CCS                                             //
// Realizado por Jose Angel Moneo                                 //
// Funciones:                                                     //
//  float MediaMovil(int ntabla,float valor)  
//    devuelve el valor medio calculado al almacenar un nuevo dato //
//    El valor se calcula y maneja con la tabla ntabla            //
////////////////////////////////////////////////////////////////////


 //*********************** Definición de pila*********************

//Estas constantes deben ser definidas en el programa principal, pues podrá manejarse varias pilas por el programa.


//#define NSensor_MediaMovil 1 //defineimos el número de sensores

//#define Muestras_MediaMovil 10 //definimos el número de muestras de la media movil


//En caso de no haber definidio las tablas se define 1 con 10 muestras

#ifndef NSensor_MediaMovil
   #define NSensor_MediaMovil 1
#endif


#ifndef Muestras_MediaMovil
  #define Muestras_MediaMovil 5
#endif

//Pilas de datos para el cálculo
float  PilaSensor[NSensor_MediaMovil][Muestras_MediaMovil];


//Devuelve un nuevo valor de la media al añadir un nuevo dato a la pila
float MediaMovil(int ntabla,float valor)
{ int i;
  float Ynuevo;
  Ynuevo=0;
   //Hace hueco moviendo los datos hacia el fondo y suma los datos para la media
  for(i=Muestras_MediaMovil-1;i>0;i--)
       {PilaSensor[ntabla][i] = PilaSensor[ntabla][i-1];
        Ynuevo+=PilaSensor[ntabla][i];}
   //Empuja el nuevo dato en la pila y suma el nuevo dato
   PilaSensor[ntabla][0]=valor;
   Ynuevo+=PilaSensor[ntabla][0];
   //Calcula el nuevo valor medio
   Ynuevo/=Muestras_MediaMovil;  //calcula la media
   return Ynuevo;

}

Código 2. El mismo algoritmo pero con cola circular mediante punteros (más rapido)

////////////////////////////////////////////////////////////////////
// Librer¡a Filtro Media Movil  V2                                //
// compilador PIC CCS                                             //
// Realizado por Jose Angel Moneo                                 //
// Funciones:                                                     //
//  float MediaMovil(int ntabla,float valor)  
//    devuelve el valor medio calculado al almacenar un nuevo dato //
//    El valor se calcula y maneja con la tabla ntabla            //
////////////////////////////////////////////////////////////////////



 //*********************** Definición de pila*********************

//Estas constantes deben ser definidas en el programa principal, pues podrá manejarse varias pilas por el programa.


//#define NSensor_MediaMovil 1 //defineimos el número de sensores

//#define Muestras_MediaMovil 3 //definimos el número de muestras de la media movil


//En caso de no haber definidio las tablas se define 1 con 10 muestras

#ifndef NSensor_MediaMovil
   #define NSensor_MediaMovil 1
#endif


#ifndef Muestras_MediaMovil
  #define Muestras_MediaMovil 3
#endif

//Pilas de datos para el cálculo
int16  ColaSensor[NSensor_MediaMovil][Muestras_MediaMovil];  //Tabla historico últimos valores
int16 VADC[NSensor_MediaMovil];                  //Suma Valores de las entradas analógicas pasadas por filtro media movil
int antiguo=0;
int nuevo=0;


//Devuelve un nuevo valor de la media al añadir un nuevo dato a la pila
int16 MediaMovil(int ntabla,int16 valor)
{    // Calcula la nueva suma
  VADC[NSensor_MediaMovil]-=VADC[ntabla]-ColaSensor[ntabla][antiguo]; //Resta el valor más antiguo
  VADC[NSensor_MediaMovil]+=valor;  //Calcula la nueva suma

    ColaSensor[ntabla][nuevo]=valor;  //Almacena el nuevo valor en la cola

   //Actualiza los punteros de cola    
    if(antiguo++>Muestras_MediaMovil)  antiguo=0;//Hace la cola circular  
    if(nuevo++>Muestras_MediaMovil)  nuevo=0;//Hace la cola circular    
   //Devuelve el nuevo valor medio
   return (VADC[NSensor_MediaMovil]/Muestras_MediaMovil);


}

 Implementación

   Para implementarlo en el programa principal donde se toman los datos, basta con llamar a la función con la lectura actual.

     Ejemplo de implementación.

     Voy a usar el filtro para estabilizar y mejorar la lectura de tres sensor termopar tipo k leído con un  MAX6675 con PIC.

      Para leer el sensor a través del MAX6675 lo haremos por SPI por lo que la instrucción de lectura usaremos la función SPI_XFER

    Configuraremos la lectura de SPI de los tres sensores mediante

  #use spi(MASTER, CLK=PIN_C7, DI=PIN_C6, ENABLE=PIN_C1, MODE=0, BITS=16, STREAM=Sensor1)

    Cambiando el ENABLE de cada MAX6675 y el STREAM por el nombre que deseemos.

   Para leer el valor del sensor usaríamos SPI_XFER(Sensor1,0)>>3, despreciando los bits de control, y para obtener su valor en grados lo multiplicaríamos por 0.25.

  Por lo tanto, leer su valor en grados sería (SPI_XFER(Sensor1,0)>>3)*0.25.

   Esto lo podríamos mostrar ya en el display, pero veremos como el valor puede oscilar más de un grado en cada lectura.

   Para corregirlo vamos a implementar la función de media móvil. 

  Primero incluimos la función en el programa y definimos los parámetros que deseamos.

#define NSensor_MediaMovil 3
#define Muestras_MediaMovil 3
#include "Filtro_MediaMovil.c"

   En este caso definimos 3 tablas, una por sensor y um M de 3, para que la respuesta a cambios sea rápida.

   Para tomar el dato vamos a pasar todas las lecturas por la media movil. Para ello usaremos la instrucicón:

 T_Sensor1= MediaMovil(0,(float)(SPI_XFER(Sensor1,0)>>3)*0.25); 

  Esto nos leerá el sensor, lo convertirá en grados y lo almacenará en la tabla de la media móvil ya en coma flotante. Así mismo la función MediaMovil, nos devolverá el valor filtrado en coma flotante, el cual almacenamos en una variable en coma flotante llamada T_Sensor1. que posteriormente usaremos es para mostrar en el display.

  Esto mismo lo haremos con todos los sensores.

 T_Sensor1= MediaMovil(0,(float)(SPI_XFER(Sensor1,0)>>3)*0.25); 

 T_Sensor2= MediaMovil(0,(float)(SPI_XFER(Sensor2,0)>>3)*0.25); 

 T_Sensor3= MediaMovil(0,(float)(SPI_XFER(Sensor3,0)>>3)*0.25); 

Ya está... ahora lo mostraremos donde queramos.