Diff: main.cpp

Revision:

3:003d2a50231f

Parent:

1:a2a5171ac99f

Parent:

2:672f7335a017

Child:

4:9c2c77d61a15

--- a/main.cpp	Tue Apr 16 22:30:18 2019 +0000
+++ b/main.cpp	Tue Apr 16 22:49:28 2019 +0000
@@ -5,7 +5,6 @@
 #include "ultrasonic.h"
 uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
 DigitalOut speaker(p18);
-ultrasonic mu(p6, p7, .1, 1, &dist); 
 Motor m1(p23, p12, p11); // pwm, fwd, rev
 Motor m2(p24, p12, p11); // pwm, fwd, rev
 
@@ -14,9 +13,13 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-volatile unsigned int state;
-volatile unsigned int energy;
-volatile unsigned int touched;
+enum {ROAM, STOP, SLEEP};
+volatile unsigned int state = ROAM;
+
+enum {NOT_TOUCHED, TOUCHED_RIGHT, TOUCHED_WRONG};
+volatile unsigned int touched = NOT_TOUCHED;
+
+volatile unsigned int energy = 70;
 volatile bool fed;
 volatile bool blocked;
 
@@ -27,11 +30,12 @@
     {
         //turn the bot
     }
-    else if(distance < 10 && state == STOPPED)
+    else if(distance < 10 && state == STOP)
     {
         //increase food status
     }
 }
+ultrasonic mu(p6, p7, .1, 1, &dist);
 
 void sonar(void const *argument) {
     while(1)
@@ -58,8 +62,8 @@
     {
         if(state == ROAM && !blocked)
         {
-            m1.speed(s); 
-            m2.speed(s); 
+            m1.speed(1); 
+            m2.speed(1); 
         }
         else if(state == STOP || state == SLEEP)
         {
@@ -83,8 +87,38 @@
     Thread thread4(sound);
     Thread thread5(wheels);
     
-    while(1) 
-    {
+    while(1) {
+        // Status LEDS
+        led1 = state > 0; // no leds on = ROAM, 1 led on = STOP, both on = SLEEP
+        led2 = state > 1;
+        led3 = touched;
+        led4 = fed;
+        
+        // Update state
+        if (energy < 5) { // sleeps for 10 seconds and wake up with 50 more energy
+            state = SLEEP;
+            Thread::wait(10000);
+            energy += 50;
+        } else if (energy < 30 || touched != NOT_TOUCHED || fed) {
+            state = STOP;
+        } else {
+            state = ROAM;
+        }
         
+        // Update energy
+        switch(state) {
+            ROAM: energy -= 2;
+                break;
+            STOP: energy -= 1;
+                break;
+        }
+        
+        // Make sure energy stays in 0-100
+        if (energy < 0) {
+            energy = 0;
+        } else if (energy > 100) {
+            energy = 100;
+        }
     }
+    Thread::wait(1000); // runs once per second
 }
\ No newline at end of file