diff --git a/MiniSumo/MiniSumo.ino b/MiniSumo/MiniSumo.ino index 9133369..e505c59 100644 --- a/MiniSumo/MiniSumo.ino +++ b/MiniSumo/MiniSumo.ino @@ -1,26 +1,32 @@ #include #include // 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 ); +}