Diff: main.cpp

Revision:

11:8d4b7702ac06

Parent:

9:6649141315be

Child:

12:4fec6ed886ab

--- a/main.cpp	Mon Dec 10 02:56:30 2018 +0000
+++ b/main.cpp	Mon Dec 10 04:53:16 2018 +0000
@@ -3,6 +3,11 @@
 #include "rgbled.h"
 #include "rgbSensor.h"
 
+// Define threads and mutexes
+Thread thread1;
+Thread thread2;
+Mutex motors_lock;
+
 // Define devices
 Motor left(p22, p16, p15); // pwm, fwd, rev
 Motor right(p23, p19, p20); // pwm, fwd, rev
@@ -10,14 +15,18 @@
 RGBLed myRGBled(p26, p25, p24); // red, green, blue
 rgbSensor rgbsensor(p28, p27);
 Serial blue(p13, p14); // serial tx, serial rx
+enum speed_state { accelerating, braking, coasting };
 
 //Game state variables
 int progress = 0;
-int current_item;
-int speed;
-int max_speed;
-int acceleration;
+int current_item = 0;
+float speed_cmd = 0.0;
+float max_speed = 1.0;
+float acceleration_rate = 0.04;
+float brake_rate = 0.1;
+float coast_rate = 0.02;
 int cstate;
+speed_state sstate = coasting;
 bool collide = false;
 
 // Global game actions
@@ -37,7 +46,6 @@
         }
     }
 }
-
 void game_paused() {
     // Cycle through LEDs
     Thread pause_thread;
@@ -57,7 +65,52 @@
         }
         ThisThread::sleep_for(500);
     }
-}  
+}
+
+// Thread to control speed
+void speed_control() {
+    // The kart has to be in one of three states at all times
+    while(1) {
+        while(sstate == coasting) {
+            if (speed_cmd > 0.0) {
+                speed_cmd -= coast_rate;
+                left.speed(speed_cmd);
+                right.speed(speed_cmd);
+            }
+            ThisThread::sleep_for(200);
+        }
+        while(sstate == accelerating) {
+            if (speed_cmd < 1.0) {
+                speed_cmd += acceleration_rate;
+                left.speed(speed_cmd);
+                right.speed(speed_cmd);
+            }
+            ThisThread::sleep_for(200);
+        }
+        while(sstate == braking) {
+            if (speed_cmd > 0.0) {
+                speed_cmd -= brake_rate;
+                left.speed(speed_cmd);
+                right.speed(speed_cmd);
+            }
+            ThisThread::sleep_for(200);
+        }
+    }
+}
+
+// Thread to brake
+void brake() {
+    motors_lock.lock();
+    myled = 8;
+    while(1) {
+        if (speed_cmd > 0.0) {
+            speed_cmd -= brake_rate;
+            left.speed(speed_cmd);
+            right.speed(speed_cmd);
+        }
+        ThisThread::sleep_for(200);
+    }
+}
 
 // Thread for checking rgb sensor values and updating game variables
 // Change later depending on behavior of RGB sensor and colors used
@@ -92,12 +145,10 @@
     }
 }
 
-// Define threads
-Thread thread1;
-
 int main() {
     // Start threads
     thread1.start(check_RGB);
+    thread2.start(speed_control); // Since we're stopped, this won't do anything
     
     // Bluetooth controller code
     char bnum=0;
@@ -118,12 +169,9 @@
                             break;
                         case '2': //number button 2, accelerate
                             if (bhit=='1') {
-                                myled = bnum - '0'; //current button number will appear on LEDs
-                                left.speed(1.0);
-                                right.speed(1.0);
+                                sstate = accelerating;
                             } else {
-                                left.speed(0.0);
-                                right.speed(0.0);
+                                sstate = coasting;
                             }
                             break;
                         case '3': //number button 3
@@ -137,9 +185,9 @@
                         case '4': //number button 4, brakes
                             if (bhit=='1') {
                                 //myled = bnum - '0'; //current button number will appear on LEDs
-                                //add hit code here
+                                sstate = braking;
                             } else {
-                                //add release code here
+                                sstate = coasting;
                             }
                             break;
                         case '5': //button 5 up arrow
@@ -153,7 +201,7 @@
                             break;
                         case '6': //button 6 down arrow
                             if (bhit=='1') {
-                                myled = bnum - '0'; //current button number will appear on LEDs
+                                //myled = bnum - '0'; //current button number will appear on LEDs
                                 left.speed(-1.0);
                                 right.speed(-1.0);
                             } else {