Diff: main.cpp

Revision:

7:8af26d50c006

Parent:

6:0bff4e260db2

Child:

8:2332c4a79276

--- a/main.cpp	Thu Apr 09 20:43:23 2020 +0000
+++ b/main.cpp	Sat Apr 18 19:37:27 2020 +0000
@@ -2,12 +2,96 @@
 
 DigitalOut myled(LED2);
 
-int main() {
-    while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
+Ticker bluetooth;
+Serial pc(USBTX, USBRX);
+
+
+//Control system variables
+float rp = 50;
+float rd = 51;
+float ri = 50;
+float desired_angle = 0;
+Serial blue(p28,p27);
+
+void bluetooth_update() {
+    char bnum=0;
+    char bhit=0;
+    if (blue.getc()=='!') {
+        if (blue.getc()=='B') { //button data packet
+            bnum = blue.getc(); //button number
+            bhit = blue.getc(); //1=hit, 0=release
+            if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+                myled = bnum - '0'; //current button number will appear on LEDs
+                switch (bnum) {
+                    case '1': //number button 1
+                        if (bhit=='1') {
+                            rd += 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '2': //number button 2
+                        if (bhit=='1') {
+                            ri += 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '3': //number button 3
+                        if (bhit=='1') {
+                            rd -= 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '4': //number button 4
+                        if (bhit=='1') {
+                            ri -= 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '5': //button 5 up arrow
+                        if (bhit=='1') {
+                            rp += 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '6': //button 6 down arrow
+                        if (bhit=='1') {
+                            rp -= 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '7': //button 7 left arrow
+                        if (bhit=='1') {
+                            desired_angle -= 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    case '8': //button 8 right arrow
+                        if (bhit=='1') {
+                            desired_angle += 1;
+                        } else {
+                            //add release code here
+                        }
+                        break;
+                    default:
+                        break;
+                }
+            }
+        }
     }
 }
-//hi
+
+int main() {
+    pc.printf("this is running");
+    bluetooth.attach(&bluetooth_update, 0.1);
+    while(1) {
+        pc.printf("rp: %f, rd: %f, ri: %f, desired_angle: %f", rp, rd, ri, desired_angle);
+    }
+}
+