jhonatan angel
abrayan
Diff: main.cpp
- Revision:
- 6:8d7f6fe73ed1
- Parent:
- 2:3007b3c06d2c
--- a/main.cpp Tue Sep 11 01:27:25 2018 +0000
+++ b/main.cpp Sat Nov 10 22:41:02 2018 +0000
@@ -1,16 +1,45 @@
#include "mbed.h"
#include "main.h"
+ DigitalOut s0(PB_13);
+ DigitalOut s1(PB_14);
+ DigitalOut s2(PB_15);
+ DigitalOut s3(PB_1);
+ DigitalIn out (PB_2);
+
+ DigitalOut LED(LED1);
+ InterruptIn pulse(PC_13);
+ float retardo = 0;
+
+
+void ISR_Poff()
+
+ {
+
+ retardo = (0.05);
+
+ }
+
+
+
+ Timer tiempo;
+
+ unsigned int lectura (void);
+
Serial command(USBTX, USBRX);
int main() {
init_servo();
init_serial();
+ pulse.fall (&ISR_Poff);
debug_m("inicio \n");
uint32_t read_cc;
while(1)
{
+ LED = !LED;
+ wait (retardo);
+
read_cc=read_command();
switch (read_cc)
@@ -18,10 +47,21 @@
case 0x01: moving();
break;
+ case 0x02: movimiento();
+ break;
+
+ case 0x03: sensor();
+ break;
+
+ case 0x04: saludo();
+ break;
+
default: debug_m("error de comando.\n");
break ;
}
+
}
+
}
@@ -36,6 +76,7 @@
}
+
void init_serial()
{
command.baud(9600);
@@ -44,14 +85,386 @@
void moving()
{
- debug_m("se inicia el comado mover..\n");
+ command.printf("se inicia el 1er comado mover servos..\n");
char nmotor=command.getc();
char grados=command.getc();
char endc=command.getc();
mover_ser(nmotor,grados);
- debug_m("fin del comado guardar..\n");
+ debug_m("Moviendo servos...\n");
+}
+
+
+void movimiento()
+{
+ command.printf("se inicia el 2do comado mover patas..\n");
+ char lado=command.getc();
+ char movi=command.getc();
+ char endc=command.getc();
+
+ switch (lado)
+ {
+ case 0x01: switch (movi) //Pata Derecha_delantera
+ {
+ case 0x01: mover_ser(0x01,0x01);
+ command.printf("moviendo pata derecha delantera arriba..\n");
+ break;
+
+ case 0x02: mover_ser(0x01,0x2d);
+ command.printf("moviendo pata derecha delantera abajo..\n");
+ break;
+
+ case 0x03: mover_ser(0x02,0x5a);
+ command.printf("moviendo pata izquierda delantera adelante..\n");
+ break;
+
+ case 0x04: mover_ser(0x02,0x0a);
+ command.printf("moviendo pata izquierda delantera atras..\n");
+ break;
+ }
+ break;
+
+ case 0x02: switch (movi) //Pata Derecha_trasera
+ {
+ case 0x01: mover_ser(0x03,0x01);
+ command.printf("moviendo pata derecha trasera arriba..\n");
+ break;
+
+ case 0x02: mover_ser(0x03,0x2d);
+ command.printf("moviendo pata derecha trasera abajo..\n");
+ break;
+
+ case 0x03: mover_ser(0x04,0xaa);
+ command.printf("moviendo pata derecha trasera adelante..\n");
+ break;
+
+ case 0x04: mover_ser(0x04,0x5a);
+ command.printf("moviendo pata derecha trasera atras..\n");
+ break;
+ }
+ break;
+
+ case 0x03: switch (movi) //Pata Izquierda_delantera
+ {
+ case 0x01: mover_ser(0x05,0x01);
+ command.printf("moviendo pata izquierda delantera arriba..\n");
+ break;
+
+ case 0x02: mover_ser(0x05,0x2d);
+ command.printf("moviendo pata izquierda delantera abajo..\n");
+ break;
+
+ case 0x03: mover_ser(0x06,0x5a);
+ command.printf("moviendo pata izquierda delantera adelante..\n");
+ break;
+
+ case 0x04: mover_ser(0x06,0xaa);
+ command.printf("moviendo pata izquierda delantera atras..\n");
+ break;
+ }
+ break;
+
+ case 0x04: switch (movi) //Pata Izquierda_trasera
+ {
+ case 0x01: mover_ser(0x07,0x01);
+ command.printf("moviendo pata izquierda trasera arriba..\n");
+ break;
+
+ case 0x02: mover_ser(0x07,0x2d);
+ command.printf("moviendo pata izquierda trasera abajo..\n");
+ break;
+
+ case 0x03: mover_ser(0x08,0x0a);
+ command.printf("moviendo pata izquierda trasera adelante..\n");
+ break;
+
+ case 0x04: mover_ser(0x08,0x5a);
+ command.printf("moviendo pata izquierda trasera atras..\n");
+ break;
+ }
+ break;
+
+ default: debug_m("error de comando.\n");
+ break ;
+ }
+ //debug_m("fin del comado guardar..\n");
}
+void sensor()
+{
+ command.printf ("se inicia el 3er comando sensor de color..\n");
+ char leer=command.getc();
+ char endc=command.getc();
+
+ unsigned int ROJO=0;
+ unsigned int VERDE=0;
+ unsigned int AZUL=0;
+
+ if(leer==0x01)
+ {
+ s0=1;
+ s1=0;
+
+ s2=0;
+ s3=0;
+ ROJO= lectura();
+
+ s2=0;
+ s3=1;
+ AZUL= lectura();
+
+ s2=1;
+ s3=1;
+ VERDE= lectura();
+
+ if (ROJO<VERDE && VERDE>AZUL && AZUL>ROJO)
+ {
+ command.printf("Color detectado: ROJO \n");
+ command.printf("ADELANTE....\n");
+
+ for(int x=0;x<5;x++)
+ {
+ mover_ser(0x01,0x00);
+ wait(0.1);
+ mover_ser(0x02,0x32);
+ wait(0.1);
+ mover_ser(0x01,0x3c);
+ wait(0.1);
+ mover_ser(0x03,0x00);
+ wait(0.1);
+ mover_ser(0x04,0x5a);
+ wait(0.1);
+ mover_ser(0x03,0x3c);
+ wait(0.1);
+ mover_ser(0x02,0x5a);
+ wait(0.1);
+ mover_ser(0x04,0x82);
+ wait(0.1);
+ mover_ser(0x05,0x00);
+ wait(0.1);
+ mover_ser(0x06,0x82);
+ wait(0.1);
+ mover_ser(0x05,0x3c);
+ wait(0.1);
+ mover_ser(0x07,0x00);
+ wait(0.1);
+ mover_ser(0x08,0x5a);
+ wait(0.1);
+ mover_ser(0x07,0x3c);
+ wait(0.1);
+ mover_ser(0x06,0x5a);
+ wait(0.1);
+ mover_ser(0x08,0x32);
+ wait(0.1);
+
+ }
+ }
+ else
+ {
+ if (VERDE<AZUL && AZUL>ROJO && ROJO>VERDE)
+ {
+ command.printf("Color detectado: VERDE \n");
+ command.printf("ROTAR DERECHA\n");
+
+ for(int x=0;x<5;x++)
+ {
+ mover_ser(0x01,0x00);
+ wait(0.1);
+ mover_ser(0x02,0x32);
+ wait(0.1);
+ mover_ser(0x01,0x3C);
+ wait(0.1);
+ mover_ser(0x03,0x00);
+ wait(0.1);
+ mover_ser(0x04,0x32);
+ wait(0.1);
+ mover_ser(0x03,0x3C);
+ wait(0.1);
+ mover_ser(0x07,0x00);
+ wait(0.1);
+ mover_ser(0x08,0x32);
+ wait(0.1);
+ mover_ser(0x07,0x3C);
+ wait(0.1);
+ mover_ser(0x05,0x00);
+ wait(0.1);
+ mover_ser(0x06,0x32);
+ wait(0.1);
+ mover_ser(0x05,0x3C);
+ wait(0.1);
+ mover_ser(0x02,0x5a);
+ wait(0.1);
+ mover_ser(0x04,0x5a);
+ wait(0.1);
+ mover_ser(0x06,0x5a);
+ wait(0.1);
+ mover_ser(0x08,0x5a);
+ wait(0.1);
+ }
+ }
+ else
+ {
+ if (AZUL<ROJO && ROJO>VERDE && VERDE>AZUL)
+ {
+ command.printf("Color detectado: AZUL \n");
+ command.printf("ATRAS....\n");
+
+
+ for(int x=0;x<5;x++)
+ {
+ mover_ser(0x01,0x00);
+ wait(0.2);
+ mover_ser(0x02,0x5a);
+ wait(0.2);
+ mover_ser(0x01,0x3c);
+ wait(0.2);
+ mover_ser(0x03,0x00);
+ wait(0.2);
+ mover_ser(0x04,0x82);
+ wait(0.2);
+ mover_ser(0x03,0x3c);
+ wait(0.2);
+ mover_ser(0x02,0x32);
+ wait(0.2);
+ mover_ser(0x04,0x5a);
+ wait(0.2);
+ mover_ser(0x05,0x00);
+ wait(0.2);
+ mover_ser(0x06,0x5a);
+ wait(0.2);
+ mover_ser(0x05,0x3c);
+ wait(0.2);
+ mover_ser(0x07,0x00);
+ wait(0.2);
+ mover_ser(0x08,0x32);
+ wait(0.2);
+ mover_ser(0x07,0x3c);
+ wait(0.2);
+ mover_ser(0x06,0x82);
+ wait(0.2);
+ mover_ser(0x08,0x5a);
+ wait(0.2);
+
+ }
+ }
+ else
+ {
+ command.printf("otro color \n");
+ }
+ }
+ }
+
+ wait(0.5);
+ }
+}
+
+
+unsigned int lectura (void)
+{
+
+unsigned int inicio=0, final=0, resultado=0;
+ tiempo.start ();
+ while (out) {}
+ while (!out) {}
+ while (out) {}
+
+ inicio= tiempo.read_us();
+ while (!out) {}
+ final=tiempo.read_us();
+ resultado=(final-inicio);
+ tiempo.reset ();
+ return (resultado);
+
+
+}
+
+
+void saludo (void)
+{
+ char lado=command.getc();
+ switch (lado)
+ {
+ case 0x01: command.printf("HOLA!!!!....\n");
+ mover_ser(0x03,0x00);
+ mover_ser(0x07,0x00);
+ mover_ser(0x05,0x5a);
+ wait(0.1);
+ mover_ser(0x01,0x00);
+ wait(0.4);
+ mover_ser(0x02,0x5a);
+ wait(0.4);
+ mover_ser(0x02,0x0a);
+ wait(0.4);
+ mover_ser(0x02,0x5a);
+ wait(0.4);
+ mover_ser(0x02,0x0a);
+ wait(0.4);
+ mover_ser(0x02,0x5a);
+ wait(0.4);
+ mover_ser(0x01,0x3c);
+ mover_ser(0x03,0x3c);
+ mover_ser(0x07,0x3c);
+ mover_ser(0x05,0x3c);
+ wait(0.1);
+ break;
+
+ case 0x02: command.printf("EMOCIONADO!!!!....\n");
+ mover_ser(0x01,0x00);
+ mover_ser(0x03,0x00);
+ mover_ser(0x05,0x00);
+ mover_ser(0x07,0x00);
+ wait(0.3);
+ mover_ser(0x01,0x3c);
+ mover_ser(0x03,0x3c);
+ mover_ser(0x05,0x3c);
+ mover_ser(0x07,0x3c);
+ wait(0.3);
+ mover_ser(0x01,0x00);
+ mover_ser(0x03,0x00);
+ mover_ser(0x05,0x00);
+ mover_ser(0x07,0x00);
+ wait(0.3);
+ mover_ser(0x01,0x3c);
+ mover_ser(0x03,0x3c);
+ mover_ser(0x05,0x3c);
+ mover_ser(0x07,0x3c);
+ wait(0.3);
+ break;
+
+ case 0x03: command.printf("''GIRANDO''!!!!....\n");
+ mover_ser(0x01,0x00);
+ wait(0.3);
+ mover_ser(0x03,0x00);
+ wait(0.3);
+ mover_ser(0x07,0x00);
+ wait(0.3);
+ mover_ser(0x01,0x46);
+ wait(0.3);
+ mover_ser(0x05,0x00);
+ wait(0.3);
+ mover_ser(0x03,0x46);
+ wait(0.3);
+ mover_ser(0x01,0x00);
+ wait(0.3);
+ mover_ser(0x07,0x46);
+ wait(0.3);
+ mover_ser(0x03,0x00);
+ wait(0.3);
+ mover_ser(0x05,0x46);
+ mover_ser(0x01,0x46);
+ mover_ser(0x03,0x46);
+ mover_ser(0x07,0x46);
+ wait(0.3);
+ break;
+
+
+ default: command.printf("error de comando.\n");
+ break ;
+ }
+
+
+}
+
+
void debug_m(char *s , ... ){
#if DEBUG
command.printf(s);