Diff: main.cpp

Revision:

4:7be1da65326d

Parent:

3:4f35942aee8e

Child:

5:164c27f5399b

--- a/main.cpp	Thu Apr 28 20:25:37 2022 +0000
+++ b/main.cpp	Fri Apr 29 07:52:34 2022 +0000
@@ -5,29 +5,31 @@
 #include "SwerveDrive.h"
 
 #define CTRL_CNT 3   // number of 4 byte packages sent over bluetooth
+#define MAX_SPEED 0.25
+
 Timer t;
 
 
 // UART
 Serial  pc(USBTX, USBRX);
-RawSerial   bluetooth(p13,p14);
+RawSerial   esp8266(p13,p14);
 
 
 // Steer Motor Pins
-DigitalOut bl_sdir(p9);
-DigitalOut bl_step(p10);
-DigitalOut br_sdir(p11);
-DigitalOut br_step(p12);
-DigitalOut fl_sdir(p7);
-DigitalOut fl_step(p8);
-DigitalOut fr_sdir(p5);
-DigitalOut fr_step(p6);
+DigitalOut bl_sdir(p10);
+DigitalOut bl_step(p9);
+DigitalOut br_sdir(p12);
+DigitalOut br_step(p11);
+DigitalOut fl_sdir(p8);
+DigitalOut fl_step(p7);
+DigitalOut fr_sdir(p6);
+DigitalOut fr_step(p5);
 
 // Drive Motor Pins
-DigitalOut bl_ddir(p20);
-PwmOut bl_pwm(p21);
-DigitalOut br_ddir(p19);
-PwmOut br_pwm(p22);
+DigitalOut br_ddir(p20);
+PwmOut br_pwm(p21);
+DigitalOut bl_ddir(p19);
+PwmOut bl_pwm(p22);
 DigitalOut fl_ddir(p18);
 PwmOut fl_pwm(p23);
 DigitalOut fr_ddir(p17);
@@ -57,59 +59,69 @@
 PwmOut myled4(LED4);
 
 uint32_t curr_time = 0; // In microseconds
+uint32_t prev_time = 0; // Also in microseconds
+uint8_t ind = 0;
+uint8_t byte_index = 0;
+float f;
+char b[4];
 
 //l_joy_x, l_joy_y, r_joy_x
 float controller[CTRL_CNT] = {0};
  
 
-int main() {
+int main() {   
     t.start();
     swerve.begin();
+    swerve.kill();
+    
+    fr_drive.set_multiplier(1.00 * MAX_SPEED);
+    fl_drive.set_multiplier(0.68 * MAX_SPEED);
+    bl_drive.set_multiplier(0.56 * MAX_SPEED);
+    br_drive.set_multiplier(0.77 * MAX_SPEED);
+    
     myled1 = 0;
     myled2 = 0;
     myled3 = 0;
     myled4 = 0;
     pc.baud(9600);
-    bluetooth.baud(9600);
+    esp8266.baud(9600);
     
-       
-
-    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);
-        if ((curr_time - prev_time) > interval) {
-            myled4 = !myled4;
-            prev_time = curr_time;
-        }
-        if (pc.readable()) {
-            while (pc.getc() != 'a') {
-                }
-            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(); 
-                }
+        
+        swerve.update(curr_time);
+        swerve.drive(controller[1], controller[0], controller[2]);
+        
+        if (esp8266.readable()) {
+            char temp = esp8266.getc();
+            if (byte_index > 3) { 
                 memcpy(&f, &b, sizeof(f));
                 controller[ind] = f;
+                byte_index = 0;
+                ind = ind == 2 ? 0 : ind + 1;
+            }
+            if (temp == 'a'){
+                prev_time = t.read_us();
+                byte_index = 0;
+                ind = 0;
+            } else {
+                b[byte_index] = temp;
+                byte_index++;
+            }
+        } else {
+            if (curr_time - prev_time > 100000){
+                myled4 = 0;
+                for (int ind = 0; ind < CTRL_CNT; ind++) {
+                    controller[ind] = 0;
+                }
+                swerve.kill();
             }
         }
-
  
         myled1 = abs(controller[0]);
         myled2 = abs(controller[1]);
         myled3 = abs(controller[2]);
     }
-    
 }
 
  
\ No newline at end of file