#include <Arduino.h>
const int TRIG_PIN = 5;
const int ECHO_PIN = 18;
QueueHandle_t xSensorQueue
QueueHandle_t xRichtingQueue;
int lastDirection = 1;
enum Richting {
LINKS,
VOOR,
RECHTS,
STUCK
};
struct SensorData {
long links;
long voor;
long rechts;
};
long meetAfstand() {
// 30% kans op een obstakel dichtbij
if (random(100) < 30) {
return random(5, 20); // obstakel dichtbij
}
return random(30, 150); // vrije baan
}
void sensorTask(void *pvParameters) {
while (true) {
SensorData data;
data.voor = meetAfstand(); // of random
// draai hoofd links
data.links = meetAfstand() ;
// draai hoofd rechts
data.rechts = meetAfstand();
xQueueSend(xSensorQueue, &data, 0);
vTaskDelay(pdMS_TO_TICKS(200));
}
}
int findMax(SensorData data) {
if (data.voor > data.links && data.voor > data.rechts) return 1;
if (data.links > data.voor && data.links > data.rechts) return 0;
if (data.rechts > data.voor && data.rechts > data.links) return 2;
return -1; // gelijkspel — breakTie nodig
}
int breakTie(SensorData data, int lastDirection) {
if (data.voor == data.links) {
if (lastDirection == 1) return 1; // was voor, blijf voor
if (lastDirection == 0) return 0; // was links, blijf links
return 1; // standaard voor
}
if (data.voor == data.rechts) {
if (lastDirection == 1) return 1;
if (lastDirection == 2) return 2;
return 1;
}
if (data.links == data.rechts) {
if (lastDirection == 0) return 0;
if (lastDirection == 2) return 2;
return 0; // standaard links
}
return lastDirection;
}
void beslisTask(void *pvParameters) {
SensorData ontvangen;
while (true) {
if (xQueueReceive(xSensorQueue, &ontvangen, portMAX_DELAY) == pdPASS) {
int max = -1;
Richting winner = VOOR; // standaard
if (ontvangen.voor < 5 && ontvangen.links < 5 && ontvangen.rechts < 5) {
winner = STUCK;
} else {
max = findMax(ontvangen);
if (max == -1) max = breakTie(ontvangen, lastDirection);
lastDirection = max;
switch (max) {
case 0: winner = LINKS; break;
case 1: winner = VOOR; break;
case 2: winner = RECHTS; break;
}
}
xQueueSend(xRichtingQueue, &winner, 0);
}
}
}
void bestuurTask(void *pvParameters) {
while (true) {
Richting richting;
if (xQueueReceive(xRichtingQueue, &richting, portMAX_DELAY) == pdPASS) {
switch (richting) {
case VOOR :
Serial.println("ik ga rechtdoor");
break;
case LINKS:
Serial.println("ik ga linksaf");
break;
case RECHTS:
Serial.println("ik ga rechtsaf");
break;
case STUCK:
Serial.println("ik ga achteruit");
break;
}
}
}
}
void setup() {
Serial.begin(115200);
pinMode(TRIG_PIN, OUTPUT); // trig stuurt signaal UIT
pinMode(ECHO_PIN, INPUT); // echo ontvangt signaal IN
xSensorQueue = xQueueCreate(10, sizeof(SensorData));
xQueue2 = xQueueCreate(10, sizeof(Richting));
xTaskCreate(sensorTask, "sensorTask", 2048, NULL, 1, NULL);
xTaskCreate(beslisTask, "beslisTask", 2048, NULL, 1, NULL);
xTaskCreate(bestuurTask, "bestuurTask", 2048, NULL, 1, NULL);
}
void loop() {}