Diff: main.cpp

Revision:

3:3d54fd4109c0

Parent:

2:4f5a0c64d9cd

Child:

4:60e3365da280

--- a/main.cpp	Fri Jun 25 00:34:17 2021 +0000
+++ b/main.cpp	Wed Jul 21 19:16:11 2021 +0000
@@ -26,6 +26,15 @@
 
 Serial pc(USBTX, USBRX); //Para recibir los comandos de voz.
 
+
+//motor driver
+DigitalOut direccion1(PC_6);
+PwmOut PWM_velocidad1(PB_15);
+
+DigitalOut direccion2(D14);
+PwmOut PWM_velocidad2(D15);
+
+
 void flushSerialBuffer(void)
 {
     char char1 = 0;
@@ -36,6 +45,83 @@
     return;
 }
 
+
+void mover_hacia_adelante(int tiempo){
+    direccion1 = 0; // En direccion de las manecillas del reloj.
+    direccion2 = 0; // En direccion de las manecillas del reloj.
+    
+    
+    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad1.write(0.15f); //%25
+    
+    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad2.write(0.15f); //25%
+    
+    
+    thread_sleep_for(tiempo); 
+    
+    PWM_velocidad1.write(0.0f);
+    PWM_velocidad2.write(0.0f);
+}
+
+void mover_hacia_atras(int tiempo){
+    direccion1 = 1; // En direccion de las manecillas del reloj.
+    direccion2 = 1; // En direccion de las manecillas del reloj.
+    
+    
+    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad1.write(0.15f); //%25
+    
+    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad2.write(0.15f); //25%
+    
+    
+    thread_sleep_for(tiempo); 
+    
+    PWM_velocidad1.write(0.0f);
+    PWM_velocidad2.write(0.0f);
+}
+
+
+void mover_hacia_izquierda(int tiempo){
+    direccion1 = 1; // En direccion de las manecillas del reloj.
+    direccion2 = 0; // En direccion de las manecillas del reloj.
+    
+    
+    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad1.write(0.15f); //%25
+    
+    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad2.write(0.15f); //25%
+    
+    
+    thread_sleep_for(tiempo); 
+    
+    PWM_velocidad1.write(0.0f);
+    PWM_velocidad2.write(0.0f);
+}
+
+
+void mover_hacia_derecha(int tiempo){
+    direccion1 = 0; // En direccion de las manecillas del reloj.
+    direccion2 = 1; // En direccion de las manecillas del reloj.
+    
+    
+    PWM_velocidad1.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad1.write(0.15f); //%25
+    
+    PWM_velocidad2.period(0.0001f); //Declaramos el periodo
+    PWM_velocidad2.write(0.15f); //25%
+    
+    
+    thread_sleep_for(tiempo); 
+    
+    PWM_velocidad1.write(0.0f);
+    PWM_velocidad2.write(0.0f);
+}
+
+
+
 void thread1_HCSR04()
 {
     DigitalOut trigger(D0);
@@ -258,6 +344,7 @@
                 if(distance1 > distanceLimit)
                 {
                     printf("Comandos joystick: Hacia adelante \r \n"); 
+                    mover_hacia_adelante(1000); // un segundo 
                 }
                 else
                 {
@@ -270,6 +357,7 @@
                 if(distance2 > distanceLimit)
                 {
                     printf("Comandos joystick: Hacia atras \r \n"); 
+                    mover_hacia_atras(1000); // un segundo
                 }
                 else
                 {
@@ -282,6 +370,7 @@
                 if(distance3 > distanceLimit)
                 {
                     printf("Comandos joystick: Hacia izquierda \r \n"); 
+                    mover_hacia_izquierda(1000); //un segundo
                 }
                 else
                 {
@@ -294,6 +383,7 @@
                 if(distance4 > distanceLimit)
                 {
                     printf("Comandos joystick: Hacia derecha \r \n"); 
+                    mover_hacia_derecha(1000); // segundo
                 }
                 else
                 {
@@ -314,60 +404,72 @@
         {
             flushSerialBuffer();
             char c = pc.getc();
-            if(c == 'w')
-            {
-                //printf("Distance1 - %d \r \n",distance1);
-                if(distance1 > distanceLimit)
-                {
-                    pc.printf(" Comandos de voz: Hacia adelante \r \n");
-                }
-                else
+            
+            thread_sleep_for(5); //timer necesario para que el compilar se de cuenta el orden correcto de ejecucion
+            
+            int m = modo3.read();
+            printf("estado modo 3 (comandos de voz): %d \r \n",m);
+        
+            if(m == 1){     
+                if(c == 'w')
                 {
-                    printf(" Comandos de voz: Obstaculo! No se puede ir hacia adelante. \r \n");      
-                }
-                thread_sleep_for(1000);
-            }
-            if(c == 's')
-            {
-                //printf("Distance2 - %d \r \n",distance2);
-                if(distance2 > distanceLimit)
-                {
-                    pc.printf(" Comandos de voz: Hacia atras \r \n");
-                }
-                else
-                {
-                    printf(" Comandos de voz: Obstaculo! No se puede ir hacia atras. \r \n");      
+                    //printf("Distance1 - %d \r \n",distance1);
+                    if(distance1 > distanceLimit)
+                    {
+                        pc.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");      
+                    }
+                    thread_sleep_for(1000);
                 }
-                thread_sleep_for(1000);
-            }                           
-            if(c == 'a')
-            {
-                //printf("Distance3 - %d \r \n",distance3);
-                if(distance3 > distanceLimit)
+                if(c == 's')
                 {
-                    pc.printf(" Comandos de voz: Hacia la izquierda  \r \n");
-                }
-                else
+                    //printf("Distance2 - %d \r \n",distance2);
+                    if(distance2 > distanceLimit)
+                    {
+                        pc.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");      
+                    }
+                    thread_sleep_for(1000);
+                }                           
+                if(c == 'a')
                 {
-                    printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la izquierda. \r \n");      
+                    //printf("Distance3 - %d \r \n",distance3);
+                    if(distance3 > distanceLimit)
+                    {
+                        pc.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");      
+                    }
+                    thread_sleep_for(1000);
                 }
-                thread_sleep_for(1000);
-            }
-            if(c == 'd')
-            { 
-                //printf("Distance4 - %d \r \n",distance4);
-                if(distance4 > distanceLimit)
-                {
-                    pc.printf(" Comandos de voz: Hacia la derecha \r \n");
-                } 
-                else
-                {
-                    printf(" Comandos de voz:  Obstaculo! No se puede ir hacia la derecha. \r \n");      
+                if(c == 'd')
+                { 
+                    //printf("Distance4 - %d \r \n",distance4);
+                    if(distance4 > distanceLimit)
+                    {
+                        pc.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");      
+                    }
+                    thread_sleep_for(1000);
                 }
-                thread_sleep_for(1000);
             }
             c = ' ';
-            thread_sleep_for(5);
+            thread_sleep_for(5); 
         }
     }
 }