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