Revision 9:4a1c40f8b2d7, committed 2021-11-04

Comitter:

thevic16

Date:

Thu Nov 04 20:02:56 2021 +0000

Parent:

8:418ff0ef9463

Commit message:

prueba;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/23-WheelchairLogicv5-test-version.lib	Thu Nov 04 20:02:56 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/thevic16/code/20-WheelchairLogicv4/#418ff0ef9463

--- a/main.cpp	Fri Oct 01 12:53:13 2021 +0000
+++ b/main.cpp	Thu Nov 04 20:02:56 2021 +0000
@@ -28,10 +28,17 @@
 int DistanceLimit = 100; // Distancia límite de acercamiento a un Obstaculo permitida por la silla.
 
 // Entradas digitales para selección de modos de la silla.
-DigitalIn Modo1(D8); // Modo manual.
-DigitalIn Modo2(D9); // Modo por comandos del joystick.
-DigitalIn Modo3(D10); // Modo por comandos de voz.
-DigitalIn Modo4(D11); // Modo por rutas autónomas
+DigitalIn modo1(D8); // Modo manual.
+DigitalIn modo2(D9); // Modo por comandos del joystick.
+DigitalIn modo3(D10); // Modo por comandos de voz.
+DigitalIn modo4(D11); // Modo por rutas autónomas
+
+//Variables modos globales
+int Modo1 =0;
+int Modo2 =0;
+int Modo3 =0;
+int Modo4 =0;
+
 
 // Interfaz serial para comunicación con la Raspberry PI.
 Serial PC(USBTX,USBRX); // Por aquí se reciben caracteres que el miprocesador interpreta para ejecutar una acción, como los comandos de voz o alguna alerta.
@@ -73,33 +80,56 @@
         MPUsensor.getAccelero(Acelerometro); // Obtener lectura del sensor
         if(Acelerometro[0] >= 3)
         {
-            printf("Sensor de aceleracion: Movimiento brusco a la derecha.\n\r");
+            //printf("Sensor de aceleracion: Movimiento brusco a la derecha.\n\r");
+            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
+            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
+            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
+
+            PC.printf("Fall Event \n");
             Reproducir_Buzzer_Caidas();
-            PC.printf("Fall Event \n");
         }
         if(Acelerometro[0] <= -3)
         {
-            printf("Sensor de aceleracion: Movimiento brusco a la izquierda.\n\r");
+            //printf("Sensor de aceleracion: Movimiento brusco a la izquierda.\n\r");
+            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
+            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
+            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
+            
+            PC.printf("Fall Event \n");
             Reproducir_Buzzer_Caidas();
-            PC.printf("Fall Event \n");
+           
         }
         if(Acelerometro[1] >= 1)
         {
-            printf("Sensor de aceleracion: Movimiento brusco adelante.\n\r");
+            //printf("Sensor de aceleracion: Movimiento brusco adelante.\n\r");
+            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
+            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
+            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
+        
+            PC.printf("Fall Event \n");
             Reproducir_Buzzer_Caidas();
-            PC.printf("Fall Event \n");
+            
         }
         if(Acelerometro[1] <= -6)
         {
-            printf("Sensor de aceleracion: Movimiento brusco atras.\n\r");
+            //printf("Sensor de aceleracion: Movimiento brusco atras.\n\r");
+            //printf("Acelerometro en x: %f \n\r",Acelerometro[0]);
+            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
+            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
+            
+            PC.printf("Fall Event \n");
             Reproducir_Buzzer_Caidas();
-            PC.printf("Fall Event \n");
         }
         if(Acelerometro[2] <= -8)
         {
-            printf("Sensor de aceleracion: Silla girada de cabeza.\n\r");
+            //printf("Sensor de aceleracion: Silla girada de cabeza.\n\r");
+            //printf("Acelerometro en x: %f \n\r ",Acelerometro[0]);
+            //printf("Acelerometro en y: %f \n\r",Acelerometro[1]);
+            //printf("Acelerometro en z: %f \n\r",Acelerometro[2]);
+            
+            PC.printf("Fall Event \n");
             Reproducir_Buzzer_Caidas();
-            PC.printf("Fall Event \n");
+            
         }
         thread_sleep_for(200);
     }
@@ -139,10 +169,10 @@
     Direccion2 = 0; // En dirección de las manecillas del reloj.
 
     PWM_Velocidad1.period(0.0003f); // Declaramos el período.
-    PWM_Velocidad1.write(0.15f); // %15 del duty cicle.
+    PWM_Velocidad1.write(0.6f); // %15 del duty cicle.
 
     PWM_Velocidad2.period(0.0003f); // Declaramos el período.
-    PWM_Velocidad2.write(0.15f); // %15 del duty cicle.
+    PWM_Velocidad2.write(0.6f); // %15 del duty cicle.
 
     thread_sleep_for(Tiempo);
 
@@ -157,10 +187,10 @@
     Direccion2 = 1; // En dirección contraria a las manecillas del reloj.
 
     PWM_Velocidad1.period(0.0003f); // Declaramos el período.
-    PWM_Velocidad1.write(0.15f); // %15 del duty cicle.
+    PWM_Velocidad1.write(0.6f); // %15 del duty cicle.
     
     PWM_Velocidad2.period(0.0003f); // Declaramos el período.
-    PWM_Velocidad2.write(0.15f); // %15 del duty cicle.
+    PWM_Velocidad2.write(0.6f); // %15 del duty cicle.
 
     thread_sleep_for(Tiempo); 
 
@@ -171,37 +201,37 @@
 // Función para moder la silla hacia la izquierda.
 void Mover_Hacia_Izquierda(int Tiempo)
 {
-    Direccion1 = 1; // En dirección contraria a las manecillas del reloj.
-    Direccion2 = 0; // En dirección de las manecillas del reloj.
+    Direccion1 = 0; // En dirección contraria a las manecillas del reloj.
+    Direccion2 = 1; // En dirección de las manecillas del reloj.
 
-    //PWM_Velocidad1.period(0.0001f); // Declaramos el período.
-    //PWM_Velocidad1.write(0.25f); // %25 del duty cicle.
+    PWM_Velocidad1.period(0.0003f); // Declaramos el período.
+    PWM_Velocidad1.write(0.6f); // %25 del duty cicle.
 
     PWM_Velocidad2.period(0.0003f); // Declaramos el período.
-    PWM_Velocidad2.write(0.15f); // %15 del duty cicle.
+    PWM_Velocidad2.write(0.6f); // %15 del duty cicle.
 
     thread_sleep_for(Tiempo); 
 
-    //PWM_Velocidad1.write(0.0f);
+    PWM_Velocidad1.write(0.0f);
     PWM_Velocidad2.write(0.0f);
 }
 
 // Función para moder la silla hacia la derecha.
 void Mover_Hacia_Derecha(int Tiempo)
 {
-    Direccion1 = 0; // En dirección de las manecillas del reloj.
-    Direccion2 = 1; // En dirección contraria a las manecillas del reloj.
+    Direccion1 = 1; // En dirección de las manecillas del reloj.
+    Direccion2 = 0; // En dirección contraria a las manecillas del reloj.
 
     PWM_Velocidad1.period(0.0003f); // Declaramos el período.
-    PWM_Velocidad1.write(0.15f); // %15 del duty cicle.
+    PWM_Velocidad1.write(0.6f); // %15 del duty cicle.
 
-    //PWM_Velocidad2.period(0.0001f); // Declaramos el período.
-    //PWM_Velocidad2.write(0.25f); // %25 del duty cicle.
+    PWM_Velocidad2.period(0.0003f); // Declaramos el período.
+    PWM_Velocidad2.write(0.6f); // %25 del duty cicle.
 
     thread_sleep_for(Tiempo); 
 
     PWM_Velocidad1.write(0.0f);
-    //PWM_Velocidad2.write(0.0f);
+    PWM_Velocidad2.write(0.0f);
 }
 
 // Función para leer el sensor de proximidad 1. ADELANTE
@@ -359,7 +389,7 @@
 // Función para leer valores del joystick y ejecutar sus comandos.
 void Thread5_Joystick()
 {
-    SparkfunAnalogJoystick JoyStick(A0,A1,PE_0);
+    SparkfunAnalogJoystick JoyStick(A1,A0,PE_0);
     float X;
     float Y;
     while(1)
@@ -373,61 +403,71 @@
             printf("Y-Axis: %f\n\r",Y);
             printf(" \n\r");
             */
-            if(X >= -0.60f && X <= 0.60f && Y >= 0.90f && Y <= 1.00f)
+            if(X >= -0.70f && X <= 0.70f && Y >= 0.90f && Y <= 1.00f)
             {
                 if(Distance2 > DistanceLimit)
                 {
                     printf("Comandos del joystick: Hacia atras. \r \n");
+                    printf("Distancia medida por sensor atras:%d cm \r \n",Distance2);
                     Mover_Hacia_Atras(3000);
                 }
                 else
                 {
                     printf("Comandos del joystick: Obstaculo hacia atras. \r \n");
-                    Reproducir_Buzzer_Proximidad();
+                    printf("Distancia medida por sensor atras:%d cm \r \n",Distance2);
+                    //Reproducir_Buzzer_Proximidad();
                 }
-                thread_sleep_for(500);
+                //thread_sleep_for(500);
             }
-            if(X >= -0.60f && X <= 0.60f && Y <= -0.90f && Y >= -1.00f)
+            if(X >= -0.70f && X <= 0.70f && Y <= -0.90f && Y >= -1.00f)
             {   
                 if(Distance1 > DistanceLimit)
                 {
                     printf("Comandos del joystick: Hacia adelante. \r \n");
+                    printf("Distancia medida por sensor adelante:%d cm \r \n",Distance1);
                     Mover_Hacia_Adelante(3000);
                 }
                 else
                 {
                     printf("Comandos del joystick: Obstaculo hacia adelante. \r \n");
-                    Reproducir_Buzzer_Proximidad();
+                    printf("Distancia medida por sensor adelante:%d cm \r \n",Distance1);
+                    //Reproducir_Buzzer_Proximidad();
                 }
-                thread_sleep_for(500);
+                //thread_sleep_for(500);
             }
-            if(Y >= -0.60f && Y  <= 0.60f && X <= -0.90f && X >= -1.00f)
+            if(Y >= -0.70f && Y <= 0.70f && X <= -0.90f && X >= -1.00f)
+            
             {
+                if(Distance4 > DistanceLimit)
+                {
+                    printf("Comandos del joystick: Hacia la derecha. \r \n");
+                    printf("Distancia medida por sensor derecha:%d cm \r \n",Distance4);
+                    Mover_Hacia_Derecha(3000);
+                }
+                else
+                {
+                    printf("Comandos del joystick: Obstaculo hacia la derecha. \r \n");
+                    printf("Distancia medida por sensor derecha:%d cm \r \n",Distance4);
+                    //Reproducir_Buzzer_Proximidad();
+                }
+                //thread_sleep_for(500);
+            }
+            if(Y >= -0.70f && Y  <= 0.70f && X >= 0.90f && X <= 1.00f)
+            {
+     
                 if(Distance3 > DistanceLimit)
                 {
                     printf("Comandos del joystick: Hacia la izquierda. \r \n");
+                    printf("Distancia medida por sensor izquierda:%d cm \r \n",Distance3);
                     Mover_Hacia_Izquierda(3000);
                 }
                 else
                 {
                     printf("Comandos del joystick: Obstaculo hacia la izquierda. \r \n");
-                    Reproducir_Buzzer_Proximidad();
+                    printf("Distancia medida por sensor izquierda:%d cm \r \n",Distance3);
+                    //Reproducir_Buzzer_Proximidad();
                 }
-                thread_sleep_for(500);
-            }
-            if(Y >= -0.60f && Y <= 0.60f && X >= 0.90f && X <= 1.00f)
-            {
-                if(Distance4 > DistanceLimit)
-                {
-                    printf("Comandos del joystick: Hacia la derecha. \r \n");
-                    Mover_Hacia_Derecha(3000);
-                }
-                else
-                {
-                    printf("Comandos del joystick: Obstaculo hacia la derecha. \r \n");
-                    Reproducir_Buzzer_Proximidad();
-                }
-                thread_sleep_for(500);
+                //thread_sleep_for(500);
             }
             thread_sleep_for(5);
         }
@@ -446,78 +486,72 @@
             
             while(PC.readable()) { // Para saber si hay algo que recibir en el puerto serial. 
                 c = PC.getc();
-                printf("Datos recividos: %c \r\n",c);
+                printf("Datos recibidos: %c \r\n",c);
             }
             
-            
-            //thread_sleep_for(5); // Retraso necesario para que el compilador se dé cuenta del orden correcto de ejecución.
-            int m = Modo3.read();
-            //printf("Estado del modo 3 (Comandos de Voz): %d \r \n",m);
-            if(m == 1)
+            if(c == 'w')
             {
-                if(c == 'w')
+                printf("Distance1 - %d \r \n",Distance1);
+                if(Distance1 > DistanceLimit)
+                 {
+                    printf("  Comandos de voz: Hacia adelante. \r \n");
+                    Mover_Hacia_Adelante(3000);
+                 }
+                else
                 {
-                    //printf("Distance1 - %d \r \n",Distance1);
-                    if(Distance1 > DistanceLimit)
-                    {
-                        printf("Comandos de voz: Hacia adelante. \r \n");
-                        Mover_Hacia_Adelante(3000);
-                    }
-                    else
-                    {
-                        printf("Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");
-                        Reproducir_Buzzer_Proximidad();
-                    }
-                    thread_sleep_for(1000);
+                    printf("Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");
+                    Reproducir_Buzzer_Proximidad();
                 }
-                if(c == 's')
+                //thread_sleep_for(1000);
+            }
+            if(c == 's')
+            {
+                printf("Distance2 - %d \r \n",Distance2);
+                if(Distance2 > DistanceLimit)
+                {
+                    printf("  Comandos de voz: Hacia atras. \r \n");
+                    Mover_Hacia_Atras(3000);
+                }
+                else
                 {
-                    //printf("Distance2 - %d \r \n",Distance2);
-                    if(Distance2 > DistanceLimit)
-                    {
-                        printf("Comandos de voz: Hacia atras. \r \n");
-                        Mover_Hacia_Atras(3000);
-                    }
-                    else
-                    {
-                        printf("Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");
-                        Reproducir_Buzzer_Proximidad();
-                    }
-                    thread_sleep_for(1000);
+                    printf("Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");
+                    Reproducir_Buzzer_Proximidad();
                 }
-                if(c == 'a')
+                    
+            }
+            if(c == 'a')
+            {
+                printf("Distance3 - %d \r \n",Distance3);
+                if(Distance3 > DistanceLimit)
+                {
+                    printf("  Comandos de voz: Hacia la izquierda. \r \n");
+                    Mover_Hacia_Izquierda(3000);
+                }
+                else
                 {
-                    //printf("Distance3 - %d \r \n",Distance3);
-                    if(Distance3 > DistanceLimit)
-                    {
-                        printf("Comandos de voz: Hacia la izquierda. \r \n");
-                        Mover_Hacia_Izquierda(3000);
-                    }
-                    else
-                    {
-                        printf("Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n");
-                        Reproducir_Buzzer_Proximidad();
-                    }
-                    thread_sleep_for(1000);
+                    printf("Comandos de voz: Obstaculo! No se puede ir hacia la izquierda. \r \n");
+                    //Reproducir_Buzzer_Proximidad();
                 }
-                if(c == 'd')
+                
+            }
+            if(c == 'd')
+            {
+                printf("Distance4 - %d \r \n",Distance4);
+                if(Distance4 > DistanceLimit)
                 {
-                    //printf("Distance4 - %d \r \n",Distance4);
-                    if(Distance4 > DistanceLimit)
-                    {
-                        printf("Comandos de voz: Hacia la derecha. \r \n");
-                        Mover_Hacia_Derecha(3000);
-                    }
-                    else
-                    {
-                        printf("Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n");
-                        Reproducir_Buzzer_Proximidad();
-                    }
-                    thread_sleep_for(1000);
+                    printf("  Comandos de voz: Hacia la derecha. \r \n");
+                    Mover_Hacia_Derecha(3000);
                 }
+                else
+                {
+                    printf("Comandos de voz: Obstaculo! No se puede ir hacia la derecha. \r \n");
+                    //Reproducir_Buzzer_Proximidad();
+                }
+                    //thread_sleep_for(1000);
             }
+            
             c = ' ';
-            thread_sleep_for(5);
+            thread_sleep_for(50);
         }
     }
 }
@@ -529,8 +563,37 @@
     bool EstadoModo2 = false;
     bool EstadoModo3 = false;
     bool EstadoModo4 = false;
+    
+    
     while(true)
     {
+        
+        if(modo1 > 0.15f) {
+            Modo1 = 1;
+        } else {
+            Modo1 = 0;
+        }
+        
+        
+        if(modo2 > 0.15f) {
+            Modo2 = 1;
+        } else {
+            Modo2 = 0;
+        }
+        
+        if(modo3 > 0.15f) {
+            Modo3 = 1;
+        } else {
+            Modo3 = 0;
+        }
+        
+        if(modo4 > 0.15f) {
+            Modo4 = 1;
+        } else {
+            Modo4 = 0;
+        }
+        
+        
         if(Modo1 && !Modo2 && !Modo3 && !Modo4 && !EstadoModo1)
         {
             printf("Operando: Modo manual. \r \n");
@@ -563,6 +626,9 @@
             EstadoModo3 = false;
             EstadoModo4 = true;
         }
+        
+        
+        //thread_sleep_for(1000);
     }
 }
 
@@ -577,12 +643,12 @@
     thread_sleep_for(200);
     Thread4.start(Thread4_HCSR04);
     thread_sleep_for(200);
+    Thread7.start(Thread7_IndicarModo);
+    thread_sleep_for(200);
     Thread5.start(Thread5_Joystick);
     thread_sleep_for(200);
     Thread6.start(Thread6_ComandosVoz);
     thread_sleep_for(200);
-    Thread7.start(Thread7_IndicarModo);
-    thread_sleep_for(200);
-    Thread8.start(Thread8_MPU6050);
-    thread_sleep_for(200);
+    //Thread8.start(Thread8_MPU6050);
+    //thread_sleep_for(200);
 }