Se agregan las funciones de evasion y deteccion de enemigo

MiniSumo/MiniSumo.ino

@@ -1,26 +1,32 @@
 #include <Arduino_FreeRTOS.h>
 #include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
 
-const int Trigger = 5;   //Pin digital 2 para el Trigger del sensor
-const int Echo = 6;   //Pin digital 3 para el Echo del sensor
+const int Trigger = 6;   //Pin digital 2 para el Trigger del sensor
+const int Echo = 7;   //Pin digital 3 para el Echo del sensor
 
+#define M1Delante 2
+#define M1Atras 3
 
-#define LedEvasion 2
-#define LedEvasionLat 3
-#define LedUltra 4
+#define M2Delante 4
+#define M2Atras 5
 
+#define SensorTrasero 8
 
+
+TaskHandle_t xd;
 SemaphoreHandle_t xSerialSemaphore;
 
 void EncontrarEnemigo( void *pvParameters );
 void SensorTras( void *pvParameters );
 void SensoresLateral( void *pvParameters );
 
+void MotoresDelante();
+void MotoresGirar();
+void EsquivarAtras();
+
 void setup() {
 
-  pinMode(LedEvasion,OUTPUT);
-  pinMode(LedEvasionLat,OUTPUT);
-  pinMode(LedUltra,OUTPUT);
+  
 
   pinMode(Trigger, OUTPUT); //pin como salida
   pinMode(Echo, INPUT);  //pin como entrada
@@ -30,6 +36,14 @@ void setup() {
 
   // initialize serial communication at 9600 bits per second:
   Serial.begin(9600);
+
+  pinMode(M1Delante,OUTPUT);
+  pinMode(M1Atras,OUTPUT);
+
+  pinMode(M2Delante,OUTPUT);
+  pinMode(M2Atras,OUTPUT);
+
+  pinMode(SensorTrasero,INPUT);
   
   while (!Serial) {
     ; // wait for serial port to connect. Needed for native USB, on LEONARDO, MICRO, YUN, and other 32u4 based boards.
@@ -43,6 +57,8 @@ void setup() {
       xSemaphoreGive( ( xSerialSemaphore ) );  // Make the Serial Port available for use, by "Giving" the Semaphore.
   }
 
+  
+
   // Now set up two Tasks to run independently.
   xTaskCreate(
     EncontrarEnemigo
@@ -50,7 +66,7 @@ void setup() {
     ,  128  // This stack size can be checked & adjusted by reading the Stack Highwater
     ,  NULL //Parameters for the task
     ,  2  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
-    ,  NULL ); //Task Handle
+    ,  &xd ); //Task Handle
 
   xTaskCreate(
     SensorTras
@@ -103,11 +119,22 @@ void EncontrarEnemigo( void *pvParameters __attribute__((unused)) )  // This is
     
     t = pulseIn(Echo, HIGH); //obtenemos el ancho del pulso
     d = t/59;             //escalamos el tiempo a una distancia en cm
+
+    if(d<70)
+    {
+      MotoresDelante();
+    }
+    else
+    {
+      MotoresGirar();
+    }
+
+    
     
     if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
     {
       
-      Serial.println(d);
+     
 
       xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
     }
@@ -119,31 +146,23 @@ void EncontrarEnemigo( void *pvParameters __attribute__((unused)) )  // This is
 void SensorTras( void *pvParameters __attribute__((unused)) )  // This is a Task.
 {
 
+
   for (;;)
   {
-    // read the input on analog pin 0:
-    int sensorValue = analogRead(A0);
-    String linea="";
-    if(sensorValue>512)
-    {
-      linea="Linea detectada";
 
-      //evadir
-      digitalWrite(LedEvasion,HIGH);
-      vTaskDelay(500);
-      
-    }
-    else
+String estado="Normal";
+    if(digitalRead(SensorTrasero))
     {
-      linea="Linea no detectada";
-      digitalWrite(LedEvasion,LOW);
-    }
 
-    // See if we can obtain or "Take" the Serial Semaphore.
-    // If the semaphore is not available, wait 5 ticks of the Scheduler to see if it becomes free.
+      EsquivarAtras();
+  
+      estado="Esquivando";
+    }
+   
     if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
     {
-      Serial.println(linea);
+
+      Serial.println(estado);
 
       xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
     }
@@ -157,47 +176,10 @@ void SensoresLateral( void *pvParameters __attribute__((unused)) )  // This is a
 
   for (;;)
   {
-    // read the input on analog pin 0:
-    int sensorValueIzq = analogRead(A1);
-    int sensorValueDer = analogRead(A2);
-    String lineaIzq="";
-    String lineaDer="";
-   /* if(sensorValueIzq>512)
-    {
-      lineaIzq="Linea Izquierda detectada";
-
-      //evadir
-      digitalWrite(LedEvasion,HIGH);
-      vTaskDelay(50);
-      
-    }
-    else
-    {
-      lineaIzq="Linea Izquierda no detectada";
-      digitalWrite(LedEvasion,LOW);
-    }
-
-    if(sensorValueDer>512)
-    {
-      lineaDer="Linea Derecha detectada";
-
-      //evadir
-      digitalWrite(LedEvasion,HIGH);
-      vTaskDelay(50);
-      
-    }
-    else
-    {
-      lineaDer="Linea Derecha no detectada";
-      digitalWrite(LedEvasion,LOW);
-    }*/
-
-    // See if we can obtain or "Take" the Serial Semaphore.
-    // If the semaphore is not available, wait 5 ticks of the Scheduler to see if it becomes free.
+    
     if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
     {
-      Serial.println(lineaIzq);
-      Serial.println(lineaDer);
+      
 
       xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
     }
@@ -205,3 +187,42 @@ void SensoresLateral( void *pvParameters __attribute__((unused)) )  // This is a
     vTaskDelay(1);  // one tick delay (15ms) in between reads for stability
   }
 }
+
+void MotoresDelante()
+{
+  digitalWrite(M1Delante,HIGH);
+  digitalWrite(M1Atras,LOW);
+
+  digitalWrite(M2Delante,HIGH);
+  digitalWrite(M2Atras,LOW);
+  
+}
+
+void MotoresGirar()
+{
+  digitalWrite(M1Delante,LOW);
+  digitalWrite(M1Atras,HIGH);
+
+  digitalWrite(M2Delante,HIGH);
+  digitalWrite(M2Atras,LOW);
+  
+}
+
+void EsquivarAtras()
+{
+  vTaskSuspend( xd );
+  digitalWrite(M1Delante,HIGH);
+  digitalWrite(M1Atras,LOW);
+  
+  digitalWrite(M2Delante,LOW);
+  digitalWrite(M2Atras,HIGH);
+
+  delay(500);
+
+  digitalWrite(M1Delante,LOW);
+  digitalWrite(M1Atras,LOW);
+
+  delay(500);
+
+  vTaskResume( xd );
+}