jesus antonio lopez
sumo
Dependencies: Sabertooth2x25 mbed
Diff: 2doejemplo.cpp
- Revision:
- 0:8758d2e0e09d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/2doejemplo.cpp Fri Nov 29 09:42:54 2013 +0000
@@ -0,0 +1,437 @@
+#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);
+}
\ No newline at end of file