Diff: main.cpp

Revision:

2:ff10ffd246e2

Parent:

1:526bdd5faa37

Child:

4:05cffda0b6a6

--- a/main.cpp	Tue Sep 04 02:15:49 2018 +0000
+++ b/main.cpp	Tue Nov 20 02:26:04 2018 +0000
@@ -1,62 +1,198 @@
 #include "mbed.h"
-
 #include "main.h"
 
-
-
-
+Timer tiempo;
 Serial command(USBTX, USBRX);
 
+DigitalIn entrada (PC_9);
+DigitalOut S0 (PA_9);
+DigitalOut S1 (PA_8);
+DigitalOut S3 (PC_7);
+DigitalOut S2 (PA_10);
 
+AnalogIn analog_valueX(A0);
+AnalogIn analog_valueY(A1);
+
+int valor;
+int color=0;
+int rojo=0;
+int azul=0;
+int verde=0;
+int Detectar();
+int tme=0;
 
 int main() {
     init_servo();
-    init_serial();
-
+    init_serial();    
     debug_m("inicio \n");
-    uint32_t read_cc;
+    uint32_t read_cc;   
     while(1)
     {
-        read_cc=read_command();
+        read_cc=read_command();        
         switch (read_cc) {
-            case  0x01: moving(); break;
-            default: debug_m("error de comando. \nSe espera  0xFEF0 o 0xFFF0 \n");break ;      
+            case  0x01: moving(); break ;             
+            case  0x02: moving2();break ;              
+            case  0x03: sensor();break ;  
+            case  0x04: joystick();break;
         }
     }
 }
 
-
-
 uint32_t read_command()
 {
-   // retorna los byte recibidos concatenados en un entero, 
    
-
-    
-    char intc=command.getc();
-    
-    while(intc != '<')
-        intc=command.getc();
-    return intc;
+      
+    char intc=command.getc();    
+    while(intc != 0xFF)
+    intc=command.getc();
+    return command.getc();
 }
-
-
 void init_serial()
 {
     command.baud(9600);    
 }
+void moving(){
+    debug_m("se inicia el comado mover..\n");   
+    char nmotor=command.getc();
+    char pos=command.getc();
+    char endc=command.getc();
+    mover_ser(nmotor,pos); 
+    debug_m("fin del comado guardar..\n");    
+} 
+void moving2(){
+    debug_m("se inicia el comado mover arana..\n");       
+    char nmotor=command.getc();
+    char pos=command.getc();
+    char endc=command.getc();
+    mover_ser2(nmotor,pos); 
+    debug_m("fin del comado guardar..\n");    
+}
+
+void moving3(){
+    debug_m("se inicia el comado caminar adelante..\n");    
+    derecha();
+    debug_m("fin del comado guardar..\n");    
+}
+
+void joystick(){
+    
+    double posX;
+    double posY;
+    
+    printf("\nJoystick\n");
+    
+    while(1) {
+       
+        posX = analog_valueX.read(); 
+        posY = analog_valueY.read(); 
+    
+        posX = posX * 3300; 
+        posY = posY * 3300; 
+        if (posX > 3000) { 
+        debug_m("se inicia el comado mover a la derecha por joystick..\n");  
+        derecha();
+        }
+        
+        else if (posX < 1000) { 
+        debug_m("se inicia el comado mover a la izquierda por joystick..\n");  
+        izquierda();
+        }
+        
+        else if (posY > 3000) { 
+        debug_m("se inicia el comado mover a la adelante por joystick..\n");  
+        adelante();
+        }
+        
+        else if (posY < 1000) { 
+        debug_m("se inicia el comado mover hacia atras por joystick..\n");  
+        atras();
+        }
+        
+        else {
+        }
+        wait(1); // 
+    }
+
+   } 
 
 
-void moving(){
-    debug_m("se inicia el comado mover..\n");    
+void sensor()
+{
+    while(1){
+        color=command.getc();        
+                S0=1;
+                S1=1; 
+                S2=0;
+                S3=0;                
+                rojo=Detectar();
+                command.printf("ROJO ");
+                command.printf("%d  ",rojo);
+                S0=1;
+                S1=1; 
+                S2=0;
+                S3=1;
+                azul=Detectar();
+                command.printf("AZUL ");
+                command.printf("%d   ",azul);
+                S0=1;
+                S1=1;             
+                S2=1;
+                S3=1;
+                verde=Detectar();
+                command.printf("VERDE");
+                command.printf("%d\n   ",verde);
+                
+                
+                if ((rojo>500)&(rojo<700) & (azul>100)&(azul<200) & (verde>100)&(verde<200)){
+                    
+                   abajo();
+                    
+                    
+                    command.printf("AZUL \n");}
+                    
+                    else{
+                        if ((rojo>0)&(rojo<200) & (azul>200)&(azul<400) & (verde>200)&(verde<400)){
+                            
+                           arriba();
+                            
+                            command.printf("ROJO\n");}
+                            
+                            else{
+                                
+                        if ((rojo>400)&(rojo<600) & (azul>200)&(azul<300) & (verde>200)&(verde<300)){
+                            
+                            adelante();
+                            
+                            command.printf("VERDE\n");}
+                            
+                            else{
+                                command.printf("COLOR NO VALIDO\n");
+                                }
+                                }
+                            }
+   }
+   }
     
-    char nmotor=command.getc();
-    char grados=command.getc();
-    char endc=command.getc();
-    mover_ser(nmotor,grados); 
-    debug_m("fin del comado guardar..\n");    
+    int Detectar(){
+    tme=0;
+    while (!entrada){}
+    while (entrada) {}
+    while (!entrada){}
+    tiempo.start();
+    while (entrada) {}
+    while (!entrada){}
+    while (entrada) {}
+    while (!entrada){}
+    while (entrada) {}
+    tiempo.stop();
+    tme=tiempo.read_us();
+    tiempo.reset();
+    return(tme);
     
-}
+    
+    }
+
+
 
 void debug_m(char *s , ... ){
     #if DEBUG