1 files changed, 28 insertions, 4 deletions
diff --git a/src/main.cpp b/src/main.cpp
index 2e9aa7c..980029f 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -20,10 +20,7 @@ RoboEyes<Adafruit_SSD1306> roboEyes(display);
 RGB rgb;
 MOTORS motors; 
 
-TaskHandle_t TaskOled;
-TaskHandle_t TaskRgb;
-TaskHandle_t TaskMotors;
-TaskHandle_t TaskWiFi;
+TaskHandle_t TaskOled, TaskRgb, TaskMotors, TaskWiFi, TaskLogic;
 
 enum RobotEmotion {
   MANUAL,
@@ -35,6 +32,10 @@ enum RobotEmotion {
 
 volatile RobotEmotion currentState = E_NEUTRAL;
 
+
+volatile int fatigueLevel = 0;
+volatile bool isTired = false;
+
 volatile char manualMotors = '0';
 volatile char manualRGB = '0';
 
@@ -104,6 +105,27 @@ void wifiTask(void * pvParameters) {
   }
 }
 
+/*================================= LOGIC =================================*/
+
+void robotLogicTask(void * pvParameters) {
+  for(;;) {
+
+  if (currentState == MANUAL && manualMotors != '0') {
+      fatigueLevel++;
+    } else {
+      if (fatigueLevel > 0) fatigueLevel--;
+    }
+
+    if (fatigueLevel >= 100 ) {
+      isTired = true;
+    } else if (fatigueLevel == 0) {
+       isTired = false;
+    }
+
+    vTaskDelay(100 / portTICK_PERIOD_MS); 
+  }
+}
+
 /*================================= OLED =================================*/
 void oledSetup(){
   Wire.begin(SDA_PIN, SCL_PIN);
@@ -148,6 +170,7 @@ void oledTask(void * pvParameters) {
           break;
       }
     }
+    roboEyes.setSweat(isTired);
     roboEyes.update(); 
     vTaskDelay(20 / portTICK_PERIOD_MS); 
   }
@@ -253,6 +276,7 @@ void setup() {
   xTaskCreate(rgbTask, "RGB Task", 2048, NULL, 1, &TaskRgb);
   xTaskCreate(motorsTask, "Motors Task", 2048, NULL, 1, &TaskMotors);
   xTaskCreate(wifiTask, "WiFi Task", 4096, NULL, 1, &TaskWiFi);
+  xTaskCreate(robotLogicTask, "Logic Task", 2048, NULL, 1, &TaskLogic);
   
   currentState = MANUAL; 
   vTaskDelay(5000 / portTICK_PERIOD_MS);