Diff: main.cpp

Revision:

2:69d290bd4b7d

Parent:

0:c6a3095b5843

Child:

3:4f35942aee8e

diff -r 4f003fe5ffd7 -r 69d290bd4b7d main.cpp
--- a/main.cpp	Sat Apr 23 23:58:30 2022 +0000
+++ b/main.cpp	Thu Apr 28 04:45:50 2022 +0000
@@ -4,8 +4,15 @@
 #include "SwerveModule.h"
 #include "SwerveDrive.h"
 
+#define CTRL_CNT 3   // number of 4 byte packages sent over bluetooth
 Timer t;
 
+
+// UART
+Serial  pc(USBTX, USBRX);
+RawSerial   bluetooth(p13,p14);
+
+
 // Steer Motor Pins
 DigitalOut bl_sdir(p9);
 DigitalOut bl_step(p10);
@@ -44,19 +51,82 @@
 
 SwerveDrive swerve(&fl_module, &fr_module, &bl_module, &br_module);
 
-DigitalOut myled(LED1);
+PwmOut myled1(LED1);
+PwmOut myled2(LED2);
+PwmOut myled3(LED3);
+PwmOut myled4(LED4);
 
 uint32_t curr_time = 0; // In microseconds
 
+//l_joy_x, l_joy_y, r_joy_x
+float controller[CTRL_CNT] = {0};
+ 
+
 int main() {
-    t.start();
-    swerve.begin();
+    myled1 = 0;
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0;
+    pc.baud(9600);
+    bluetooth.baud(9600);
+    
+       
 
+    //attach interrupt function for each new Bluetooth serial character
+//    bluetooth.attach(&parse_message,Serial::RxIrq);
     
+    if (pc.writeable()) {
+        pc.putc('s'); 
+    }
+    
+    
+    int counter = 0;
     while(1) {
-        curr_time = t.read_us();
-        swerve.update(curr_time);
-        swerve.drive(0.1f, 0.1f, 0.1f);
-        myled = 1;
+//        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;
+                }
+            }
+//            while (bluetooth.readable()) {
+//                bluetooth.getc();
+//            }
+            else {
+                for (int ind = 0; ind < CTRL_CNT; ind++) {
+                    controller[ind] = 0.0;
+                }
+            }
+        }
+
+
+        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]);
+        myled3 = abs(controller[2]);
     }
+    
 }
+
+ 
\ No newline at end of file