From 26955c1c123894b9cb03a061a6278128b25e03bc Mon Sep 17 00:00:00 2001
From: Stedd <toanpa+git@pm.me>
Date: Sun, 22 Oct 2023 02:09:46 +0200
Subject: [PATCH] Formatting

---
 motorControl.ino | 15 +++++++--------
 plot.ino         | 30 ++++++++++++++++++++----------
 2 files changed, 27 insertions(+), 18 deletions(-)

diff --git a/motorControl.ino b/motorControl.ino
index ac66cee..6b24aca 100644
--- a/motorControl.ino
+++ b/motorControl.ino
@@ -13,13 +13,13 @@ const float DEADBAND_M2_NEG = 90.0;
 
 
 //Tuning
-const float K_SC = 18.5*gainScale;         //Speed controller gain
-const float K_TC = 90.0*gainScale;         //Turn controller gain
-const float K_OL = 13.0*gainScale;         //Outer loop balance controller gain
-const float K_IL = 72.0*gainScale;         //Inner loop balance controller gain
-const float I_IL = 80.0*gainScale;         //Inner loop balance controller Igain
-const float filter_gain = 16.0;  //Motor speed LPF gain
 const float gainScale = 1;
+const float K_SC = 18.5 * gainScale;  //Speed controller gain
+const float K_TC = 90.0 * gainScale;  //Turn controller gain
+const float K_OL = 13.0 * gainScale;  //Outer loop balance controller gain
+const float K_IL = 72.0 * gainScale;  //Inner loop balance controller gain
+const float I_IL = 80.0 * gainScale;  //Inner loop balance controller Igain
+const float filter_gain = 16.0;       //Motor speed LPF gain
 
 //Help variables
 int M1_Speed_CMD, M2_Speed_CMD;
@@ -62,7 +62,7 @@ void motors() {
   }
 
   if (Ps3.data.button.square) {
-      IMU.init();
+    IMU.init();
   }
 
 
@@ -82,7 +82,6 @@ void motors() {
     // Speed Controller
     SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC);
 
-
     // Balance controller
     // Outer loop
     OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL);
diff --git a/plot.ino b/plot.ino
index 2587668..a1d051f 100644
--- a/plot.ino
+++ b/plot.ino
@@ -119,25 +119,35 @@ void plot() {
 }
 
 int PackInt(int _i, int value) {
-  data[_i] = (value & 0x00FF) ;
-  data[_i + 1] = (value & 0xFF00)>>8;  
+  data[_i] = (value & 0x00FF);
+  data[_i + 1] = (value & 0xFF00) >> 8;
   return _i + 2;
 }
 
+int PackLong(int _i, long value) {
+  data[_i] = (value & 0x000000FF);
+  data[_i + 1] = (value & 0x0000FF00) >> 8;
+  data[_i + 2] = (value & 0x00FF0000) >> 16;
+  data[_i + 3] = (value & 0xFF000000) >> 24;
+  return _i + 4;
+}
+
 union FloatToBytes {
-    float value;
-    byte bytes[4];
+  float value;
+  byte bytes[4];
 };
 
 int PackFloat(int _i, float value) {
-    FloatToBytes converter;
-    converter.value = value;
-    for(int j = 0; j < 4; j++) {
-        data[_i + j] = converter.bytes[j];
-    }
-    return _i + 4;
+  FloatToBytes converter;
+  converter.value = value;
+  for (int j = 0; j < 4; j++) {
+    data[_i + j] = converter.bytes[j];
+  }
+  return _i + 4;
 }
 
+
+
 // int PackFloat(int _i, float value) {
 //   data[_i] = (value & 0x000000FF) ;
 //   data[_i + 2] = (value & 0x0000FF00)>>8;