Minisumo Estable V1

MiniSumo/MiniSumo.ino

@@ -1,8 +1,12 @@
 #include <Arduino_FreeRTOS.h>
 #include <semphr.h>  // add the FreeRTOS functions for Semaphores (or Flags).
 
-const int Trigger = 6;   //Pin digital 2 para el Trigger del sensor
-const int Echo = 7;   //Pin digital 3 para el Echo del sensor
+bool GO = false;
+
+
+bool Derecha=false;
+bool Izquierda=0;
+
 
 #define M1Delante 2
 #define M1Atras 3
@@ -12,6 +16,10 @@ const int Echo = 7;   //Pin digital 3 para el Echo del sensor
 
 #define SensorTrasero 8
 
+#define SensorDerecho 9
+#define SensorIzquierdo 10
+
+
 
 TaskHandle_t xd;
 SemaphoreHandle_t xSerialSemaphore;
@@ -21,16 +29,15 @@ void SensorTras( void *pvParameters );
 void SensoresLateral( void *pvParameters );
 
 void MotoresDelante();
-void MotoresGirar();
+void MotoresGirarIzquierda();
+void MotoresGirarDerecha();
 void EsquivarAtras();
 
 void setup() {
 
   
 
-  pinMode(Trigger, OUTPUT); //pin como salida
-  pinMode(Echo, INPUT);  //pin como entrada
-  digitalWrite(Trigger, LOW);//Inicializamos el pin con 0
+  
 
    
 
@@ -44,6 +51,12 @@ void setup() {
   pinMode(M2Atras,OUTPUT);
 
   pinMode(SensorTrasero,INPUT);
+  pinMode(SensorDerecho,INPUT);
+  pinMode(SensorIzquierdo,INPUT);
+
+  pinMode(7,INPUT);
+
+
   
   while (!Serial) {
     ; // wait for serial port to connect. Needed for native USB, on LEONARDO, MICRO, YUN, and other 32u4 based boards.
@@ -88,7 +101,7 @@ void setup() {
 
 void loop()
 {
-  // Empty. Things are done in Tasks.
+  
 }
 
 /*--------------------------------------------------*/
@@ -108,25 +121,44 @@ void EncontrarEnemigo( void *pvParameters __attribute__((unused)) )  // This is
 
   for (;;) // A Task shall never return or exit.
   {
-    
 
-    long t; //timepo que demora en llegar el eco
-    long d; //distancia en centimetros
-  
-    digitalWrite(Trigger, HIGH);
-    delayMicroseconds(10);          //Enviamos un pulso de 10us
-    digitalWrite(Trigger, LOW);
-    
-    t = pulseIn(Echo, HIGH); //obtenemos el ancho del pulso
-    d = t/59;             //escalamos el tiempo a una distancia en cm
 
-    if(d<70)
+float suma = 0;
+
+if(GO==false)
+    {
+      bool arranque = digitalRead(7);
+      
+      if(arranque)
+      {
+        Izquierda=true;
+        
+        GO=true;
+      }
+    }
+
+    for(int i=0;i<50;i++)
+  {
+    suma=suma+analogRead(A0);
+  } 
+
+    float adc=suma/50;
+  float d = 17569.7 * pow(adc, -1.2062);
+
+    if(d<70&&GO==true)
     {
       MotoresDelante();
     }
     else
     {
-      MotoresGirar();
+      if(Derecha)
+      {
+        MotoresGirarDerecha();
+      }
+      else if(Izquierda)
+      {
+        MotoresGirarIzquierda(); 
+      }
     }
 
     
@@ -134,7 +166,7 @@ void EncontrarEnemigo( void *pvParameters __attribute__((unused)) )  // This is
     if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
     {
       
-     
+     Serial.println(d);
 
       xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
     }
@@ -151,12 +183,17 @@ void SensorTras( void *pvParameters __attribute__((unused)) )  // This is a Task
   {
 
 String estado="Normal";
+
+  
+
     if(digitalRead(SensorTrasero))
     {
-
+  
       EsquivarAtras();
   
       estado="Esquivando";
+      Derecha=true;
+      Izquierda=false;
     }
    
     if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
@@ -176,6 +213,17 @@ void SensoresLateral( void *pvParameters __attribute__((unused)) )  // This is a
 
   for (;;)
   {
+
+    if(digitalRead(SensorIzquierdo))
+    {
+      Derecha=true;
+      Izquierda=false;
+    }
+    else if(digitalRead(SensorDerecho))
+    {
+      Derecha=false;
+      Izquierda=true;
+    }
     
     if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
     {
@@ -198,7 +246,7 @@ void MotoresDelante()
   
 }
 
-void MotoresGirar()
+void MotoresGirarIzquierda()
 {
   digitalWrite(M1Delante,LOW);
   digitalWrite(M1Atras,HIGH);
@@ -226,3 +274,13 @@ void EsquivarAtras()
 
   vTaskResume( xd );
 }
+
+void MotoresGirarDerecha()
+{
+  digitalWrite(M1Delante,HIGH);
+  digitalWrite(M1Atras,LOW);
+
+  digitalWrite(M2Delante,LOW);
+  digitalWrite(M2Atras,HIGH);
+  
+}