Diff: main.cpp

Revision:

3:4f35942aee8e

Parent:

2:69d290bd4b7d

Child:

4:7be1da65326d

--- a/main.cpp	Thu Apr 28 04:45:50 2022 +0000
+++ b/main.cpp	Thu Apr 28 20:25:37 2022 +0000
@@ -63,6 +63,8 @@
  
 
 int main() {
+    t.start();
+    swerve.begin();
     myled1 = 0;
     myled2 = 0;
     myled3 = 0;
@@ -72,55 +74,36 @@
     
        
 
-    //attach interrupt function for each new Bluetooth serial character
-//    bluetooth.attach(&parse_message,Serial::RxIrq);
-    
     if (pc.writeable()) {
         pc.putc('s'); 
     }
     
+    uint32_t prev_time = 0;
+    uint32_t interval = .1*1000000;
     
     int counter = 0;
     while(1) {
+        curr_time = t.read_us();
 //        swerve.update(curr_time);
 //        swerve.drive(0.1f, 0.1f, 0.1f);
-        myled4 = bluetooth.readable();
-        if (bluetooth.readable()) {
-            if (bluetooth.getc() == 'a') {
-                counter++;
-                //receive float (4 bytes)
-                
-                for (int ind = 0; ind < CTRL_CNT; ind++) {
-                    float f;
-                    char b[4];
-                    for (int byte_index = 0; byte_index < 4; byte_index++) {
-                        b[byte_index] = bluetooth.getc(); 
-                        counter++;
-                        
-                    }
-                    memcpy(&f, &b, sizeof(f));
-                    controller[ind] = f;
+        if ((curr_time - prev_time) > interval) {
+            myled4 = !myled4;
+            prev_time = curr_time;
+        }
+        if (pc.readable()) {
+            while (pc.getc() != 'a') {
                 }
-            }
-//            while (bluetooth.readable()) {
-//                bluetooth.getc();
-//            }
-            else {
-                for (int ind = 0; ind < CTRL_CNT; ind++) {
-                    controller[ind] = 0.0;
+            for (int ind = 0; ind < CTRL_CNT; ind++) {
+                float f;
+                char b[4];
+                for (int byte_index = 0; byte_index < 4; byte_index++) {
+                    b[byte_index] = pc.getc(); 
                 }
+                memcpy(&f, &b, sizeof(f));
+                controller[ind] = f;
             }
         }
 
-
-        pc.printf("%i\r\n", counter);
-//        if (abs(controller[0]) > 0.0)
-//            myled1 = 1;
-//        if (abs(controller[1]) > 0.0)
-//            myled2 = 1;
-//        if (abs(controller[2]) > 0.0)
-//            myled3 = 1;     
-
  
         myled1 = abs(controller[0]);
         myled2 = abs(controller[1]);