Browse Source

Se agregan las funciones de evasion y deteccion de enemigo

master
parent
commit
f53e25af85
1 changed files with 88 additions and 67 deletions
  1. +88
    -67
      MiniSumo/MiniSumo.ino

+ 88
- 67
MiniSumo/MiniSumo.ino View File

@ -1,26 +1,32 @@
#include <Arduino_FreeRTOS.h> #include <Arduino_FreeRTOS.h>
#include <semphr.h> // add the FreeRTOS functions for Semaphores (or Flags). #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; SemaphoreHandle_t xSerialSemaphore;
void EncontrarEnemigo( void *pvParameters ); void EncontrarEnemigo( void *pvParameters );
void SensorTras( void *pvParameters ); void SensorTras( void *pvParameters );
void SensoresLateral( void *pvParameters ); void SensoresLateral( void *pvParameters );
void MotoresDelante();
void MotoresGirar();
void EsquivarAtras();
void setup() { void setup() {
pinMode(LedEvasion,OUTPUT);
pinMode(LedEvasionLat,OUTPUT);
pinMode(LedUltra,OUTPUT);
pinMode(Trigger, OUTPUT); //pin como salida pinMode(Trigger, OUTPUT); //pin como salida
pinMode(Echo, INPUT); //pin como entrada pinMode(Echo, INPUT); //pin como entrada
@ -30,6 +36,14 @@ void setup() {
// initialize serial communication at 9600 bits per second: // initialize serial communication at 9600 bits per second:
Serial.begin(9600); Serial.begin(9600);
pinMode(M1Delante,OUTPUT);
pinMode(M1Atras,OUTPUT);
pinMode(M2Delante,OUTPUT);
pinMode(M2Atras,OUTPUT);
pinMode(SensorTrasero,INPUT);
while (!Serial) { while (!Serial) {
; // wait for serial port to connect. Needed for native USB, on LEONARDO, MICRO, YUN, and other 32u4 based boards. ; // 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. xSemaphoreGive( ( xSerialSemaphore ) ); // Make the Serial Port available for use, by "Giving" the Semaphore.
} }
// Now set up two Tasks to run independently. // Now set up two Tasks to run independently.
xTaskCreate( xTaskCreate(
EncontrarEnemigo EncontrarEnemigo
@ -50,7 +66,7 @@ void setup() {
, 128 // This stack size can be checked & adjusted by reading the Stack Highwater , 128 // This stack size can be checked & adjusted by reading the Stack Highwater
, NULL //Parameters for the task , NULL //Parameters for the task
, 2 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest. , 2 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
, NULL ); //Task Handle
, &xd ); //Task Handle
xTaskCreate( xTaskCreate(
SensorTras SensorTras
@ -103,11 +119,22 @@ void EncontrarEnemigo( void *pvParameters __attribute__((unused)) ) // This is
t = pulseIn(Echo, HIGH); //obtenemos el ancho del pulso t = pulseIn(Echo, HIGH); //obtenemos el ancho del pulso
d = t/59; //escalamos el tiempo a una distancia en cm d = t/59; //escalamos el tiempo a una distancia en cm
if(d<70)
{
MotoresDelante();
}
else
{
MotoresGirar();
}
if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE ) if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
{ {
Serial.println(d);
xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others. 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. void SensorTras( void *pvParameters __attribute__((unused)) ) // This is a Task.
{ {
for (;;) 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 ) if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
{ {
Serial.println(linea);
Serial.println(estado);
xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others. 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 (;;) 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 ) if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
{ {
Serial.println(lineaIzq);
Serial.println(lineaDer);
xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others. 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 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 );
}

Loading…
Cancel
Save