jesus antonio lopez
sumo
Dependencies: Sabertooth2x25 mbed
2doejemplo.cpp
- Committer:
- jalp89
- Date:
- 2013-11-29
- Revision:
- 0:8758d2e0e09d
File content as of revision 0:8758d2e0e09d:
#include "mbed.h"
#include "Sabertooth2x25.h"
Sabertooth2x25 m(p9,9600);
//*********************LEDs***************************
DigitalOut led1(p29); //Linea Derecha(rojo)
DigitalOut led2(p28); //Derecha
DigitalOut led3(p27); //Derecha Central
DigitalOut led4(p21); //Central
DigitalOut led5(p22); //izquierda Central
DigitalOut led6(p23); //Izquierda
DigitalOut led7(p24); //Linea Izquierda
//********************Sensores int*********************
DigitalIn senLinDer(p14);
DigitalIn senDer(p15);
DigitalIn senDerCen(p16);
DigitalIn senCen(p20);
DigitalIn senIzqCen(p19);
DigitalIn senIzq(p18);
DigitalIn senLinIzq(p17);
//*******************SW Int******************************
DigitalIn sw1(p10); //1
DigitalIn sw2(p11); //2
DigitalIn sw3(p12); //3
DigitalIn sw4(p13); //4
//*******************variables***********************
int seleccion_linea_derecha=0; ////////
int seleccion_derecha=0; ////////
int seleccion_enfrente_derecha=0; ////////
int seleccion_enfrente=0; ////////
int seleccion_enfrente_izquierda=0; ////////
int seleccion_izquierda=0; ////////
int seleccion_linea_izquierda=0; ////////
int seleccion_suma=0; ////////
int selec_programa_sw3=0;
int selec_programa_sw4=0;
int selec_programa_suma=0;
//********************
int Time=.2;
int tiempo=0;
//*****
void Seleccion();
void Inicio();
void Leds();
void Busqueda();
void Casos();
void Frente();
void Derecha();
void Izquierda();
void Atras();
void Paro();
//**********************
int main()
{
Seleccion();
wait(1);
Leds();//.7seg
Leds();//1.4seg
Leds();//2.1seg
Leds();//2.8seg
Leds();//3.5seg
Inicio();
while(1)
{
//Linea_derecha
if(seleccion_suma==1) ///-O-////
Derecha();
else
///////////////////////////////////////////////////////////////////////////////
//Linea izquierda
if(seleccion_suma==1000) ///-O-////
Izquierda();
else
//linea_derecha y linea izquierda //////////
if(seleccion_suma==1001) ///-O-////
{
Atras();
wait(.1);//100 ///////////////////////////////--------aqui-------------------------------------------------------
Derecha();
wait(Time); /// ///////////////////////////////--------aqui-------------------------------------------------------
}
else ///-O-////
Busqueda();
Casos();
}
}
void Leds()
{
led1=1;
wait(.1);
led1=0;
led2=1;
wait(.1);
led2=0;
led3=1;
wait(.1);
led3=0;
led4=1;
wait(.1);
led4=0;
led5=1;
wait(.1);
led5=0;
led6=1;
wait(.1);
led6=0;
led7=1;
wait(.1);
led7=0;
}
void Seleccion()
{
selec_programa_suma=0;
if(sw3==1)
{
selec_programa_sw3=100;
led2=1;
}
else
led2=0;
if(sw4==1)
{
selec_programa_sw4=1000;
led1=1;
}
else
led1=0;
//******************
selec_programa_suma=selec_programa_sw3+selec_programa_sw4;
selec_programa_sw3=0;
selec_programa_sw4=0;
}
void Inicio()
{ ///-C-////
///-A-////
switch( selec_programa_suma) ///-S-////
{ ///-O-////
///-S-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//sin oponente //////////
case 0 :
selec_programa_suma=0; ///-O-////
Frente();
break;
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
case 100 :
selec_programa_suma=0; ///-O-////
Izquierda();
wait(Time);
break;
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
case 1000 :
selec_programa_suma=0; ///-O-////
Derecha();
wait(Time);
break;
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
default: //////////
selec_programa_suma=0; ///-O-////
//////////
//////////
break; //////////
//////////
}
}
void Busqueda()
{
seleccion_suma=0;
if(senLinDer==0)
{
seleccion_linea_derecha=1;
led7=1;
}
else
led7=0;
if(senDer==0)
{
seleccion_derecha=5;
led6=1;
}
else
led6=0;
if(senDerCen==0)
{
seleccion_enfrente_derecha=10;
led5=1;
}
else
led5=0;
if(senCen==0)
{
seleccion_enfrente=50;
led4=1;
}
else
led4=0;
if(senIzqCen==0)
{
seleccion_enfrente_izquierda=100;
led3=1;
}
else
led3=0;
if(senIzq==0)
{
seleccion_izquierda=500;
led2=1;
}
else
led2=0;
if(senLinIzq==0)
{
seleccion_linea_izquierda=1000;
led1=1;
}
else
led1=0;
seleccion_suma=seleccion_derecha + seleccion_enfrente_derecha + seleccion_enfrente + seleccion_enfrente_izquierda + seleccion_izquierda ;
//seleccion_linea_derecha=0; ////////1
seleccion_derecha=0; ////////5
seleccion_enfrente_derecha=0; ////////10
seleccion_enfrente=0; ////////50
seleccion_enfrente_izquierda=0; ////////100
seleccion_izquierda=0; ////////500
//seleccion_linea_izquierda=0; ////////1000
if(seleccion_suma>0)
{
tiempo=0;
}
}
void Casos()
{ ///-C-////
///-A-////
switch( seleccion_suma) ///-S-////
{ ///-O-////
///-S-////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//sin oponente //////////
case 0 :
seleccion_suma=0; ///-O-////
tiempo++;
if(tiempo>=30000)
{
Derecha();
tiempo=0;
wait(.280);
}
else{
Paro();
}
//frente_inicio();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//derecha
case 5 :
seleccion_suma=0; ///-O-////
Derecha();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfre_derecha
case 10 :
seleccion_suma=0; ///-O-////
Derecha();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Frente
case 50 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfrente y enfrente derecha //////////
case 60 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Frente_Izqu
case 100 :
seleccion_suma=0; ///-O-////
Izquierda();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfrente y enfrente izquierda //////////
case 150 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfrente y enfrente izquierda y enfrente derecha //////////
case 160 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//Izquierda
case 500 :
seleccion_suma=0;
Izquierda(); ///-O-////
break; ///-O-////
///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//////////// enfrente derecha y derecha
case 15 :
seleccion_suma=0; ///-O-////
Derecha();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfrente derecha y enfrente izquierda
case 110:
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfrente izquierda e izquierda
case 600 :
seleccion_suma=0; ///-O-////
Izquierda();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//enfrente izquierda e izquierda y enfrente
case 650 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
//////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//derecha y enfrente //////////
case 55 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//izquierda y enfrente //////////
case 550 :
seleccion_suma=0; ///-O-////
Frente();
break; ///-O-////
///////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
//mas de una opcion //////////
default: //////////
seleccion_suma=0; ///-O-////
//////////
Paro(); //////////
//////////
break; //////////
//////////
}//signo del switch //////////
}//signo del fin de funcion casos
void Frente()
{
m.drive_motor_1(128, 0);
m.drive_motor_2(128, 0);
}
void Derecha()
{
m.drive_motor_1(128, 0);
m.drive_motor_2(128, 127);
}
void Izquierda()
{
m.drive_motor_1(128, 127);
m.drive_motor_2(128, 0);
}
void Atras()
{
m.drive_motor_1(128, 127);
m.drive_motor_2(128, 127);
}
void Paro()
{
m.drive_motor_1(128, 64);
m.drive_motor_2(128, 64);
}