Diff: main.cpp

Revision:

4:bb9c8df5bef9

Parent:

3:add27c286be9

--- a/main.cpp	Mon May 28 18:26:42 2018 +0000
+++ b/main.cpp	Tue May 29 14:29:12 2018 +0000
@@ -8,12 +8,24 @@
 
 // default serial buffer length is 8 bytes
 const int BUFF_LENGTH = 8;
+const int MAX_SPEED = 255;
+const int MIN_SPEED = -255;
 
 uint8_t rx_buf[BUFF_LENGTH];
 char str_buf[BUFF_LENGTH];
 char battery[BUFF_LENGTH] = "";
 char str[BUFF_LENGTH] = "";
 
+void drive(int left, int right) {
+    // bounds check speeds
+    if (right > MIN_SPEED && right < MAX_SPEED &&
+        left > MIN_SPEED && left < MAX_SPEED) {
+                
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+    }
+}    
+
 /**
  * Receive commands in the form of a one-character leading command,
  * followed by two optional wheel speeds, if driving forwards or back.
@@ -27,13 +39,11 @@
     switch(cmd) {
         case 'f': {
             // forward
-            m3pi.left_motor(left);
-            m3pi.right_motor(right);
+            drive(left, right);
             break;
         } case 'r': {
             // reverse
-            m3pi.left_motor(-left);
-            m3pi.right_motor(-right);
+            drive(-left, -right);
             break;
         } case 'b': {
             // return battery level as 8-character string repr of float