Diff: main.cpp

Revision:

7:6ef9b0aab972

Parent:

6:235548599e79

Child:

8:e90d9ce42b33

--- a/main.cpp	Sat Apr 21 21:22:43 2018 +0000
+++ b/main.cpp	Mon Apr 30 20:01:22 2018 +0000
@@ -1,67 +1,27 @@
 #include "mbed.h"
 #include "motordriver.h"
 #include "HALLFX_ENCODER.h"
+#include "Servo.h"
 
-#include <string>
+#include <string.h>
 
 #define SPEED 0.2
 #define TICKSPERREV 390
-#define DISTPERREV 8.25     // 8.25 inches per revolution
+#define DISTPERREV 8.25         // 8.25 inches per revolution
 #define TicksPerDeg 2.756
-
-using namespace std;
  
-Motor right(p23,p6,p5, 1); // pwm, fwd, rev
-Motor left(p21, p7, p8, 1); // pwm, fwd, rev
+Motor right(p23, p6, p5, 1);    // pwm, fwd, rev
+Motor left(p21, p7, p8, 1);     // pwm, fwd, rev
 
 HALLFX_ENCODER leftEnc(p15);
 HALLFX_ENCODER rightEnc(p16);
 
-RawSerial dev(p13,p14);
-RawSerial pc(USBTX, USBRX);
+Serial blue(p13,p14);
+Serial pc(USBTX, USBRX);
 DigitalOut led1(LED1);
 DigitalOut led4(LED4);
 
-int count = 0;
-
-void dev_recv()
-{
-    count++;
-    led1 = !led1;
-    while(dev.readable()) {
-        pc.putc(dev.getc());
-    }
-    pc.printf("%d", count);
-}
- 
-void pc_recv()
-{
-    led4 = !led4;
-    while(pc.readable()) {
-        dev.putc(pc.getc());
-    }
-}
-
-string command;
-
-void blue_recv()
-{
-    led1 = !led1;
-    while(dev.readable()) {
-        char letter = (char) dev.getc();
-        command += letter;
-        //strcat(command, &letter);
-        //pc.putc(dev.getc());
-    }
-    pc.printf("%s", command);
-    
-    string circle = "circle";
-    if (circle.compare(0,6,command) == 0) {
-        dev.puts("Recognized word circle");
-    } else {
-        dev.puts("Did not recognize command");
-    }
-}
+Servo pen(p24);
 
 void stop() {
     right.speed(0.0);
@@ -126,16 +86,77 @@
     }
 }
 
+// command character array for bluetooth parsing
+char command[100];
+
+void blue_interrupt() {
+    // bluetooth receiver interrupt
+    char letter = (char) blue.getc();
+    command[strlen(command)] = letter;
+}
+
+void get_command() {
+    memset(command, '\0', sizeof(command)); // resetting command buffer
+    while (!strlen(command)) {  // waiting for full command to be read
+        led4 = !led4;
+        wait(1);
+    }
+}
+
 int main() {
-    pc.baud(9600);
-    dev.baud(9600);
- 
-    pc.attach(&pc_recv, Serial::RxIrq);
-    dev.attach(&blue_recv, Serial::RxIrq);
-
+    
+    blue.baud(9600);
+    blue.attach(&blue_interrupt);
+    
+    while (1) {
+        get_command();
+        blue.printf("%s, len = %d\n", command, strlen(command));
+        
+        const char *delimiter = " ";    // space character to separate arguments
+        char *arg1;
+        char *arg2;
+        
+        arg1 = strtok(command, delimiter);
+        arg2 = strtok(NULL, delimiter);
+        
+        if (strcmp(arg1, "draw") == 0) {
+            // first argument is draw
+            blue.printf("  First argument is draw\n");
+            
+            if (strcmp(arg2, "square") == 0) {
+                // second argument is square
+                blue.printf("  Second argument is square\n");
+            } else if (strcmp(arg2, "circle") == 0) {
+                // second argument is circle
+                blue.printf("  Second argument is circle\n");
+            } else {
+                // second argument is not recognized
+                blue.printf("  Second argument is not recognized (must be square or circle)\n");
+            }
+            
+        } else if (strcmp(arg1, "write") == 0) {
+            // first argument is write
+            blue.printf("  First argument is write\n");
+            blue.printf("  Second argument is %s\n", arg2);
+        } else {
+            // first argument is not recognized
+            blue.printf("  First argument is not recognized (must be draw or write)\n");
+        }
+        
+        blue.printf("\n");
+    }
+    
+    /*
     while(1) {
-        sleep();
+        pen.calibrate(0.0005, 180.0);
+        for(float p=0; p<1.0; p += 0.1) {
+            pen.write(p);
+            wait(2);
+        }
     }
+    */
+    
+    //forward(5.0);
     //forward(5.0);
     //turnLeft(90);
     //Should make a triangle