Diff: main.cpp

Revision:

1:5fc064b4c942

Parent:

0:25cb75cafbb8

Child:

2:a73f6db7efef

--- a/main.cpp	Sun May 27 20:33:47 2018 +0000
+++ b/main.cpp	Sun May 27 22:06:26 2018 +0000
@@ -6,10 +6,61 @@
 Serial dev(p28,p27);
 DigitalOut led1(LED1);
 
+// default serial buffer length is 8 bytes
 const int BUFF_LENGTH = 8;
 
 uint8_t rx_buf[BUFF_LENGTH];
 char str_buf[BUFF_LENGTH];
+char battery[BUFF_LENGTH] = "";
+char str[BUFF_LENGTH] = "";
+
+/**
+ * Receive commands in the form of a one-character leading command,
+ * followed by two optional wheel speeds, if driving forwards or back.
+ * Wheel speeds are unsigned integers in the range of 0 (stop) to 255.
+ */
+void process_cmd() {
+    char cmd = (char)rx_buf[0];
+    int right = (int)rx_buf[1];
+    int left = (int)rx_buf[2];
+    
+    switch(cmd) {
+        case 'f': {
+            // forward
+            m3pi.left_motor(left);
+            m3pi.right_motor(right);
+            break;
+        } case 'r': {
+            // reverse
+            m3pi.left_motor(-left);
+            m3pi.right_motor(-right);
+            break;
+        } case 'b': {
+            // return battery level as 8-character string repr of float
+            sprintf(battery, "%f", m3pi.battery());
+            int len = strlen(battery);
+            for (int i = 0; i < len; i++) {
+                dev.putc(battery[i]);
+            }
+            break;
+        } case 's': {
+            m3pi.stop();
+            m3pi.printf("stop");
+            break;
+        } case 'z': {
+            // spin left
+            m3pi.left(0.2);
+            break;
+        } case 'y': {
+            // spin right
+            m3pi.right(0.2);
+            break;
+        } default: {
+            m3pi.stop();
+            m3pi.printf("?");
+        }
+    }
+}
 
 void dev_recv() {
     
@@ -40,16 +91,20 @@
     // print the buffer contents as ASCII on the first line
     m3pi.print(str_buf, i);
     
+    m3pi.locate(0, 1);
+    
+    /*
     // print the buffer length on the second line
-    m3pi.locate(0, 1);
-    char str[BUFF_LENGTH] = "";
     sprintf(str, "%d", i);
     m3pi.printf(str);
-    
+    */
+    process_cmd();
 }
 
 int main() {
     dev.baud(9600);
+    // use CTS-only flow control
+    dev.set_flow_control(Serial::CTS, p26);
     dev.attach(&dev_recv, Serial::RxIrq);
     m3pi.locate(0, 1);
     m3pi.printf("BT LE");