Revision 1:17ea74f31633, committed 2021-06-24

Comitter:

thevic16

Date:

Thu Jun 24 19:50:59 2021 +0000

Parent:

0:b8adbf13199b

Commit message:

Prueba2;

Changed in this revision

--- a/main.cpp	Thu Jun 10 20:35:27 2021 +0000
+++ b/main.cpp	Thu Jun 24 19:50:59 2021 +0000
@@ -4,27 +4,81 @@
 
 #define M_PI 3.14159265358979323846
 
-
 Thread thread1;
 Thread thread2;
 Thread thread3;
 Thread thread4;
 Thread thread5;
+Thread thread6;
+Thread thread7;
 
-int distance1 = 0;
-int distance2 = 0;
-int distance3 = 0;
-int distance4 = 0;
+int distance1 = 0; //distancia  adelante.  
+int distance2 = 0; //distancia atras.
+int distance3 = 0; // distancia izquierda.
+int distance4 = 0; // distancia derecha. 
 
 int distanceLimit = 10;
 
+DigitalIn modo1(D8);  //Modo manual
+DigitalIn modo2(D9);  //Modo comandos joystick
+DigitalIn modo3(D10); //Modo comandos de voz
+DigitalIn modo4(D11); //Modo rutas autonomas
+
+Serial pc(USBTX, USBRX); //Para recibir los comandos de voz.
 
 void thread1_HCSR04()
 {
     DigitalOut trigger(D0);
     DigitalIn  echo(D1);
     Timer sonar;
+    int correction = 0;
+    sonar.reset();
+    // measure actual software polling timer delays
+    // delay used later in time correction
+    // start timer
+    sonar.start();
+    // min software polling delay to read echo pin
+    while (echo==2)
+    {
+        
+    };
+    // stop timer
+    sonar.stop();
+    // read timer
+    correction = sonar.read_us();
+    printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction);
+    //Loop to read Sonar distance values, scale, and print
+    while(1)
+    {
+        //trigger sonar to send a ping
+        trigger = 1;
+        sonar.reset();
+        wait_us(10.0);
+        trigger = 0;
+        //wait for echo high
+        while (echo==0)
+        {
+            
+        };
+        //echo high, so start timer
+        sonar.start();
+        //wait for echo low
+        while (echo==1) {};
+        //stop timer and read value
+        sonar.stop();
+        //subtract software overhead timer delay and scale to cm
+        distance1 = (sonar.read_us()-correction)/58.0;
+        //printf("Sensor proximidad 1: %d cm \n\r",distance1);
+        //wait so that any echo(s) return before sending another ping
+        thread_sleep_for(1000);
+    }
+}
 
+void thread2_HCSR04()
+{
+    DigitalOut trigger(D2);
+    DigitalIn  echo(D3);
+    Timer sonar;
     int correction = 0;
     sonar.reset();
     // measure actual software polling timer delays
@@ -32,48 +86,50 @@
     // start timer
     sonar.start();
     // min software polling delay to read echo pin
-    while (echo==2) {};
-    
+    while (echo==2)
+    {
+        
+    };
     // stop timer
     sonar.stop();
     // read timer
     correction = sonar.read_us();
-    printf("Sensor proximidad 1: Approximate software overhead timer delay is %d uS\n\r",correction);
- 
+    printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction);
     //Loop to read Sonar distance values, scale, and print
-    while(1) {
-    // trigger sonar to send a ping
+    while(1)
+    {
+        // trigger sonar to send a ping
         trigger = 1;
-        
-       
         sonar.reset();
         wait_us(10.0);
         trigger = 0;
-   
-    //wait for echo high
-        while (echo==0) {};
-        
-    //echo high, so start timer
+        //wait for echo high
+        while (echo==0)
+        {
+            
+        };
+        //echo high, so start timer
         sonar.start();
-    //wait for echo low
-        while (echo==1) {};
-    //stop timer and read value
+        //wait for echo low
+        while (echo==1)
+        {
+            
+        };
+        //stop timer and read value
         sonar.stop();
-    //subtract software overhead timer delay and scale to cm
-        distance1 = (sonar.read_us()-correction)/58.0;
-        
-        //printf("Sensor proximidad 1: %d cm \n\r",distance1);
-    //wait so that any echo(s) return before sending another ping
-    thread_sleep_for(1000);
-    }  
+        //subtract software overhead timer delay and scale to cm
+        distance2 = (sonar.read_us()-correction)/58.0;
+        //printf("Sensor proximidad 2: %d cm \n\r",distance2);
+        //wait so that any echo(s) return before sending another ping
+        thread_sleep_for(1000);
+    }
 }
 
-void thread2_HCSR04()
+void thread3_HCSR04()
 {
-    DigitalOut trigger(D2);
-    DigitalIn  echo(D3);
+    DigitalOut trigger(D4);
+    DigitalIn  echo(D5);
     Timer sonar;
-    
     int correction = 0;
     sonar.reset();
     // measure actual software polling timer delays
@@ -81,49 +137,50 @@
     // start timer
     sonar.start();
     // min software polling delay to read echo pin
-    while (echo==2) {};
-    
+    while (echo==2)
+    {
+        
+    };
     // stop timer
     sonar.stop();
     // read timer
     correction = sonar.read_us();
-    printf("Sensor proximidad 2: Approximate software overhead timer delay is %d uS\n\r",correction);
- 
+    printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction);
     //Loop to read Sonar distance values, scale, and print
-    while(1) {
-    // trigger sonar to send a ping
+    while(1)
+    {
+        // trigger sonar to send a ping
         trigger = 1;
-        
-       
         sonar.reset();
         wait_us(10.0);
         trigger = 0;
-   
-    //wait for echo high
-        while (echo==0) {};
-        
-    //echo high, so start timer
+        //wait for echo high
+        while (echo==0)
+        {
+            
+        };
+        //echo high, so start timer
         sonar.start();
-    //wait for echo low
-        while (echo==1) {};
-    //stop timer and read value
+        //wait for echo low
+        while (echo==1)
+        {
+            
+        };
+        //stop timer and read value
         sonar.stop();
-    //subtract software overhead timer delay and scale to cm
-        distance2 = (sonar.read_us()-correction)/58.0;
-        
-        //printf("Sensor proximidad 2: %d cm \n\r",distance2);
-    //wait so that any echo(s) return before sending another ping
-    thread_sleep_for(1000);
-    }  
+        //subtract software overhead timer delay and scale to cm
+        distance3 = (sonar.read_us()-correction)/58.0;
+        //printf("Sensor proximidad 3: %d cm \n\r",distance3);
+        //wait so that any echo(s) return before sending another ping
+        thread_sleep_for(1000);
+    }
 }
- 
- 
-void thread3_HCSR04()
+
+void thread4_HCSR04()
 {
-    DigitalOut trigger(D4);
-    DigitalIn  echo(D5);
+    DigitalOut trigger(D6);
+    DigitalIn  echo(D7);
     Timer sonar;
-    
     int correction = 0;
     sonar.reset();
     // measure actual software polling timer delays
@@ -131,166 +188,216 @@
     // start timer
     sonar.start();
     // min software polling delay to read echo pin
-    while (echo==2) {};
-    
-    // stop timer
-    sonar.stop();
-    // read timer
-    correction = sonar.read_us();
-    printf("Sensor proximidad 3: Approximate software overhead timer delay is %d uS\n\r",correction);
- 
-    //Loop to read Sonar distance values, scale, and print
-    while(1) {
-    // trigger sonar to send a ping
-        trigger = 1;
-        
-       
-        sonar.reset();
-        wait_us(10.0);
-        trigger = 0;
-   
-    //wait for echo high
-        while (echo==0) {};
+    while (echo==2)
+    {
         
-    //echo high, so start timer
-        sonar.start();
-    //wait for echo low
-        while (echo==1) {};
-    //stop timer and read value
-        sonar.stop();
-    //subtract software overhead timer delay and scale to cm
-        distance3 = (sonar.read_us()-correction)/58.0;
-        
-        //printf("Sensor proximidad 3: %d cm \n\r",distance3);
-    //wait so that any echo(s) return before sending another ping
-    thread_sleep_for(1000);
-    }  
-}
-
-
-void thread4_HCSR04()
-{
-    DigitalOut trigger(D6);
-    DigitalIn  echo(D7);
-    Timer sonar;
-    
-    int correction = 0;
-    sonar.reset();
-    // measure actual software polling timer delays
-    // delay used later in time correction
-    // start timer
-    sonar.start();
-    // min software polling delay to read echo pin
-    while (echo==2) {};
-    
+    };
     // stop timer
     sonar.stop();
     // read timer
     correction = sonar.read_us();
     printf("Sensor proximidad 4: Approximate software overhead timer delay is %d uS\n\r",correction);
- 
     //Loop to read Sonar distance values, scale, and print
-    while(1) {
-    // trigger sonar to send a ping
+    while(1)
+    {
+        // trigger sonar to send a ping
         trigger = 1;
-        
-       
         sonar.reset();
         wait_us(10.0);
         trigger = 0;
-   
-    //wait for echo high
-        while (echo==0) {};
-        
-    //echo high, so start timer
+        //wait for echo high
+        while (echo==0)
+        {
+            
+        };
+        //echo high, so start timer
         sonar.start();
-    //wait for echo low
-        while (echo==1) {};
-    //stop timer and read value
+        //wait for echo low
+        while (echo==1)
+        {
+            
+        };
+        //stop timer and read value
         sonar.stop();
-    //subtract software overhead timer delay and scale to cm
+        //subtract software overhead timer delay and scale to cm
         distance4 = (sonar.read_us()-correction)/58.0;
-        
         //printf("Sensor proximidad 4: %d cm \n\r",distance4);
-    //wait so that any echo(s) return before sending another ping
-    thread_sleep_for(1000);
-    }  
+        //wait so that any echo(s) return before sending another ping
+        thread_sleep_for(1000);
+    }
 }
 
-void thread5_Joystick(){
-    SparkfunAnalogJoystick JoyStick(A1, A0, D1);
-    
+void thread5_Joystick()
+{
+    SparkfunAnalogJoystick JoyStick(A1, A0, D12);
     float X;
     float Y;
-    
+    while(1)
+    {
+        if(!modo1 && modo2 && !modo3 && !modo4)
+        {
+            X = JoyStick.xAxis();
+            Y = JoyStick.yAxis();
+            
+            printf("X-Axis: %f\n\r", X);
+            printf("Y-Axis: %f\n\r", Y);
+            printf(" \n\r");
+            
+            if(X >= -0.60f && X  <= 0.60f && Y >= 0.90f && Y <= 1.00f )
+            {        
+                if(distance1 > distanceLimit)
+                {
+                    printf("Comandos joystick: Hacia adelante \r \n"); 
+                }
+                else
+                {
+                    printf("Comandos joystick: Obstaculo adelante \r \n"); 
+                }
+                thread_sleep_for(1000);   
+            }
+            if(X >= -0.60f && X  <= 0.60f && Y <= -0.90f && Y >= -1.00f)
+            {
+                if(distance2 > distanceLimit)
+                {
+                    printf("Comandos joystick: Hacia atras \r \n"); 
+                }
+                else
+                {
+                    printf("Comandos joystick: Obstaculo atras \r \n"); 
+                } 
+                thread_sleep_for(1000); 
+            }
+            if(Y >= -0.60f && Y  <= 0.60f && X <= -0.90f && X >= -1.00f)
+            {
+                if(distance3 > distanceLimit)
+                {
+                    printf("Comandos joystick: Hacia izquierda \r \n"); 
+                }
+                else
+                {
+                    printf("Comandos joystick: Obstaculo izquierda \r \n"); 
+                }
+                thread_sleep_for(1000);    
+            }
+            if(Y >= -0.60f && Y  <= 0.60f && X >= 0.90f && X <= 1.00f)
+            {
+                if(distance4 > distanceLimit)
+                {
+                    printf("Comandos joystick: Hacia derecha \r \n"); 
+                }
+                else
+                {
+                    printf("Comandos joystick: Obstaculo derecha \r \n"); 
+                }
+                thread_sleep_for(1000);   
+            }
+            thread_sleep_for(5); 
+        }
+    }
+}
+
+void thread6_ComandosVoz()
+{
+    char c = pc.getc(); 
     while(1)
     {
-        
-        X = JoyStick.xAxis();
-        Y = JoyStick.yAxis();
-        
-        /*
-        printf("X-Axis: %f\n\r", X);
-        printf("Y-Axis: %f\n\r", Y);
-        printf(" \n\r");
-        */
-        
-        if(X >= -0.60f && X  <= 0.60f && Y >= 0.90f && Y <= 1.00f ){
-            if(distance1 > distanceLimit)
+        if(!modo1 && !modo2 && modo3 && !modo4)
+        {
+            if(c == 'w')
+            {
+                if(distance1 > distanceLimit )
+                {
+                    pc.printf(" Comandos de voz: Hacia adelante \r \n");
+                    thread_sleep_for(500);
+                }
+                else
+                {
+                    printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");      
+                }  
+            }        
+            if(c == 'd')
             {
-               printf(" Hacia adelante \r \n");  
+                if(distance2 > distanceLimit)
+                {
+                    pc.printf(" Comandos de voz: Hacia la derecha \r \n");
+                    thread_sleep_for(500);
+                }
+                else
+                {
+                    printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");      
+                }
+            }                           
+            if(c == 's')
+            {
+                if(distance3 > distanceLimit)
+                {
+                    pc.printf(" Comandos de voz: Hacia atrás \r \n");
+                    thread_sleep_for(500);
+                }
+                else
+                {
+                    printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la izquierda. \r \n");      
+                }
             }
-            else{
-                printf(" Obstaculo! No se puede ir hacia adelante. \r \n");      
-            }
-            
-            thread_sleep_for(1000);   
-        }
-        if(X >= -0.60f && X  <= 0.60f && Y <= -0.90f && Y >= -1.00f){
-            
-            if(distance2 > distanceLimit)
-            {
-               printf(" Hacia atras \r \n"); 
-            }
-            else{
-              printf(" Obstaculo! No se puede ir hacia atras. \r \n");      
-            }
-             
-            thread_sleep_for(1000); 
+            if(c == 'a')
+            { 
+                if(distance4 > distanceLimit)
+                {
+                    pc.printf(" Comandos de voz: Hacia la izquierda \r \n");
+                    thread_sleep_for(500);
+                } 
+                else
+                {
+                    printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la derecha. \r \n");      
+                }               
+            }                 
+        }                        
+    }
+}
+
+void thread7_IndicarModo()
+{
+    bool estadomodo1 = false;
+    bool estadomodo2 = false;
+    bool estadomodo3 = false;
+    bool estadomodo4 = false;
+    while (true)
+    {
+        if(modo1 && !modo2 && !modo3 && !modo4 && !estadomodo1)
+        {
+            printf("Modo manual  \r \n");
+            estadomodo1 = true;
+            estadomodo2 = false;
+            estadomodo3 = false;
+            estadomodo4 = false;
         }
-        if(Y >= -0.60f && Y  <= 0.60f && X <= -0.90f && X >= -1.00f){
-            
-            if(distance3 > distanceLimit)
-            {
-               printf(" Hacia la izquierda \r \n"); 
-            }
-            else{
-              printf(" Obstaculo! No se puede ir hacia la izquierda. \r \n");      
-            }
-            
-            thread_sleep_for(1000);    
-        }
-        if(Y >= -0.60f && Y  <= 0.60f && X >= 0.90f && X <= 1.00f){
-            
-            if(distance4 > distanceLimit)
-            {
-               printf(" Hacia la derecha \r \n"); 
-            }
-            else{
-              printf(" Obstaculo! No se puede ir hacia la derecha. \r \n");      
-            }
-            
-    
-            thread_sleep_for(1000);   
-        }
-        
-        //thread_sleep_for(1000); 
+        if(!modo1 && modo2 && !modo3 && !modo4 && !estadomodo2)
+        {
+            printf("Modo comandos joystick  \r \n");
+            estadomodo1 = false;
+            estadomodo2 = true;
+            estadomodo3 = false;
+            estadomodo4 = false;
+        }   
+        if(!modo1 && !modo2 && modo3 && !modo4 && !estadomodo3)
+        {
+            printf("Modo comandos de voz  \r \n");
+            estadomodo1 = false;
+            estadomodo2 = false;
+            estadomodo3 = true;
+            estadomodo4 = false;
+        }   
+        if(!modo1 && !modo2 && !modo3 && modo4 && !estadomodo4)
+        {
+            printf("Modo rutas autonomas \r \n");
+            estadomodo1 = false;
+            estadomodo2 = false;
+            estadomodo3 = false;
+            estadomodo4 = true;
+        }       
     }
-    
+}
 
- }
- 
- 
 int main()
 {
     thread1.start(thread1_HCSR04);
@@ -298,5 +405,6 @@
     thread3.start(thread3_HCSR04);
     thread4.start(thread4_HCSR04);
     thread5.start(thread5_Joystick);
-    
+    thread6.start(thread6_ComandosVoz);
+    thread7.start(thread7_IndicarModo);
 }
\ No newline at end of file