Merge branch 'SketchStructure'

.gitignore

@@ -0,0 +1 @@
+arduino_secrets.h

Balancebot.ino

@@ -3,6 +3,7 @@
 #include <Wire.h>
 #include <MatrixMath.h>
 #include <Ps3Controller.h>
+#include "arduino_secrets.h"
 
 //Declare library objects
 GY_85 IMU;
@@ -28,12 +29,7 @@ int dT = 0;
 float dT_s = 0.0;
 
 
-//Motor variables
-const int PWM_CYCLE = 12000;
-const byte PWM_RES = 12;
-
-
-//Encoders variables
+//Encoder variables
 long int m1Raw, m1RawLast;
 long int m2Raw, m2RawLast;
 volatile bool M1_A_state, M1_B_state;
@@ -44,6 +40,66 @@ volatile bool M2_A_state, M2_B_state;
 const char* _ps3Address = "18:5e:0f:92:00:6c";
 
 
+//MotorControl Constants
+const int PWM_CYCLE = 12000;
+const byte PWM_RES = 12;
+const int MOTOR_SATURATION = round(pow(2, PWM_RES));
+const float BASE_WIDTH = 0.1837;
+const float WHEEL_DIAMETER = 0.0677;
+const float PULSES_PER_TURN = 1320.0;
+const float BALANCE_POINT = 0.05;
+const float SPEED_REF = 0.00;
+const float TURN_SPEED_REF = 0.00;
+const float DEADBAND_M1_POS = 90.0;
+const float DEADBAND_M1_NEG = 90.0;
+const float DEADBAND_M2_POS = 90.0;
+const float DEADBAND_M2_NEG = 90.0;
+
+
+//MotorControl Tuning
+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
+
+
+//MotorControl Help variables
+int M1_Speed_CMD, M2_Speed_CMD;
+float rem_speed_ref, rem_turn_speed_ref;
+float SC_cont_out;
+float TC_cont_out;
+float OL_cont_out;
+float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
+float speedCmd1, speedCmd2;
+bool balancingOn = false;
+
+
+//MotorControl Matrices
+mtx_type motor_ang_vel[2][1];
+mtx_type vel_Matrix[2][1];
+mtx_type inv_Kin[2][2];
+
+
+//IMU CONSTANTS
+const float alpha = 0.95;
+const int acc_overflow_value = 65535;
+const int gyro_overflow_value = 4558;  // 4096+512-50=4558 ?
+
+
+//IMU VARIABLES
+int ax, ay, az;
+int cx, cy, cz;
+int gx, gy, gz;
+float gt;
+float acc_pitch;
+float pitch_rate;
+float pitch = 0;
+float pitch_prev = 0;
+
+
 //UDP variables
 uint8_t data[30 * 4];
 
@@ -106,7 +162,7 @@ void loop() {
   motors();
 
   // Plot
-  plot();
+  SerialPlot();
 
   //Udp
   UdpLoop();

IMU.ino

@@ -1,20 +1,3 @@
-//CONSTANTS
-const float alpha = 0.95;
-const int acc_overflow_value = 65535;
-const int gyro_overflow_value = 4558;  // 4096+512-50=4558 ?
-
-
-//IMU VARIABLES
-int ax, ay, az;
-int cx, cy, cz;
-int gx, gy, gz;
-float gt;
-float acc_pitch;
-float pitch_rate;
-float pitch = 0;
-float pitch_prev = 0;
-
-
 void readIMU() {
   //Acceletometer
   ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);

README.md

@@ -8,16 +8,4 @@ These settings allow upload:
 
 
 ## Dependencies
-### Boards Manager
-[esp32 v1.0.4](https://github.com/espressif/arduino-esp32)
-#### Boards libraries
-- Wire @1.0.1
-- Wifi @1.0
-- ESP32 Async UDP @1.0.0
-
-### Libraries
-[Ps3Controller.h @v1.1.0](https://github.com/jvpernis/esp32-ps3)
-
-[MatrixMath.h @v1.0](https://github.com/eecharlie/MatrixMath)
-
-[GY_85.h @commit af33c9f791618cec6ae218fe73d039276448ff4f](https://github.com/sqrtmo/GY-85-arduino/tree/master)
+See sketch.yaml

UDP.ino

@@ -1,5 +1,5 @@
-const char* ssid = "CaveBot";
-const char* password = "&nHM%D2!$]Qg[VUv";
+const char* ssid = SECRET_SSID;
+const char* password = SECRET_PASSWORD;
 
 #include "WiFi.h"
 #include "AsyncUDP.h"
@@ -15,6 +15,7 @@ void UdpInit() {
 }
 
 void UdpLoop() {
+  PackUdpData();
   udp.writeTo(data, sizeof(data), multicastIP, port);
 }
 
@@ -28,3 +29,62 @@ void ConnectToWiFi() {
     }
   }
 }
+
+void PackUdpData() {
+  int i = 0;
+  data[i] = watchdog++;
+  data[i += 1] = balancingOn << 1;
+  i = PackInt(i += 1, M1_Speed_CMD);
+  i = PackInt(i, M2_Speed_CMD);
+  i = PackFloat(i, acc_pitch);
+  i = PackFloat(i, pitch);
+  i = PackFloat(i, pitch_rate);
+  i = PackFloat(i, rem_speed_ref);
+  i = PackFloat(i, rem_turn_speed_ref);
+  i = PackFloat(i, SC_cont_out);
+  i = PackFloat(i, TC_cont_out);
+  i = PackFloat(i, OL_cont_out);
+  i = PackFloat(i, ref_IL);
+  i = PackFloat(i, act_IL);
+  i = PackFloat(i, error_IL);
+  i = PackFloat(i, IL_cont_out);
+  i = PackFloat(i, iError_IL);
+  i = PackFloat(i, IL_anti_windup);
+  i = PackFloat(i, speedCmd1);
+  i = PackFloat(i, speedCmd2);
+  i = PackFloat(i, vel_Matrix[0][0]);
+  i = PackFloat(i, vel_Matrix[1][0]);
+  i = PackFloat(i, motor_ang_vel[0][0]);
+  i = PackFloat(i, motor_ang_vel[1][0]);
+  i = PackLong(i, m1Raw);
+  i = PackLong(i, m2Raw);
+}
+
+
+int PackInt(int _i, int value) {
+  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];
+};
+
+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;
+}

motorControl.ino

@@ -1,43 +1,3 @@
-//Constants
-const int MOTOR_SATURATION = round(pow(2, PWM_RES));
-const float BASE_WIDTH = 0.1837;
-const float WHEEL_DIAMETER = 0.0677;
-const float PULSES_PER_TURN = 1320.0;
-const float BALANCE_POINT = 0.05;
-const float SPEED_REF = 0.00;
-const float TURN_SPEED_REF = 0.00;
-const float DEADBAND_M1_POS = 90.0;
-const float DEADBAND_M1_NEG = 90.0;
-const float DEADBAND_M2_POS = 90.0;
-const float DEADBAND_M2_NEG = 90.0;
-
-
-//Tuning
-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;
-float rem_speed_ref, rem_turn_speed_ref;
-float SC_cont_out;
-float TC_cont_out;
-float OL_cont_out;
-float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
-float speedCmd1, speedCmd2;
-
-bool balancingOn = false;
-
-//Matrices
-mtx_type motor_ang_vel[2][1];
-mtx_type vel_Matrix[2][1];
-mtx_type inv_Kin[2][2];
-
-
 void initMotors() {
   // Inverse Kinematic matrix of differential drive robot
   inv_Kin[0][0] = WHEEL_DIAMETER / 4;

plot.ino

@@ -1,4 +1,4 @@
-void plot() {
+void SerialPlot() {
   // Time
   // Serial.print("dT:");
   // Serial.println(dT);
@@ -8,13 +8,10 @@ void plot() {
   // Serial.print(" ");
 
   // IMU
-
   // Serial.print("RollRate:");
   // Serial.println(pitch_rate);
-
   // Serial.print("Accelerometer_Pitch:");
   // Serial.println(acc_pitch);
-
   // Serial.print("Pitch:");
   // Serial.println(pitch);
   // Serial.print  ( "," );
@@ -39,29 +36,21 @@ void plot() {
   // Encoders
   // Serial.print("m1Raw:");
   // Serial.println(m1Raw);
-
   // Serial.print("m2Raw:");
   // Serial.println(m2Raw);
-
   // // Motors
   // Serial.print("SpeedControllerOut:");
   // Serial.println(SC_cont_out);
-
   // Serial.print("BalanceOLControllerOut:");
   // Serial.println(OL_cont_out);
-
   // Serial.print("BalanceILControllerOut:");
   // Serial.println(IL_cont_out);
-
   // Serial.print("TurnControllerOut:");
   // Serial.println(TC_cont_out);
-
   // Serial.print("M1_CMD:");
   // Serial.println(M1_Speed_CMD);
-
   // Serial.print("M2_CMD:");
   // Serial.println(M2_Speed_CMD);
-
   // Serial.print("M1_Ang_Vel:");
   // Serial.print(motor_ang_vel[0][0]);
   // Serial.print(" ");
@@ -88,70 +77,4 @@ void plot() {
   //   Serial.print("ry:");
   //   Serial.println(Ps3.data.analog.stick.ry);
   // }
-
-  int i = 0;
-  data[i] = watchdog++;
-  data[i += 1] = balancingOn << 1;
-  i = PackInt(i += 1, M1_Speed_CMD);
-  i = PackInt(i, M2_Speed_CMD);
-  i = PackFloat(i, acc_pitch);
-  i = PackFloat(i, pitch);
-  i = PackFloat(i, pitch_rate);
-  i = PackFloat(i, rem_speed_ref);
-  i = PackFloat(i, rem_turn_speed_ref);
-  i = PackFloat(i, SC_cont_out);
-  i = PackFloat(i, TC_cont_out);
-  i = PackFloat(i, OL_cont_out);
-  i = PackFloat(i, ref_IL);
-  i = PackFloat(i, act_IL);
-  i = PackFloat(i, error_IL);
-  i = PackFloat(i, IL_cont_out);
-  i = PackFloat(i, iError_IL);
-  i = PackFloat(i, IL_anti_windup);
-  i = PackFloat(i, speedCmd1);
-  i = PackFloat(i, speedCmd2);
-  i = PackFloat(i, vel_Matrix[0][0]);
-  i = PackFloat(i, vel_Matrix[1][0]);
-  i = PackFloat(i, motor_ang_vel[0][0]);
-  i = PackFloat(i, motor_ang_vel[1][0]);
-  i = PackLong(i, m1Raw);
-  i = PackLong(i, m2Raw);
 }
-
-int PackInt(int _i, int value) {
-  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];
-};
-
-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;
-}
-
-
-
-// int PackFloat(int _i, float value) {
-//   data[_i] = (value & 0x000000FF) ;
-//   data[_i + 2] = (value & 0x0000FF00)>>8;
-//   data[_i + 3] = (value & 0x00FF0000)>>16;
-//   data[_i + 4] = (value & 0xFF000000)>>24;
-//   return _i + 4;
-// }

sketch.yaml

@@ -0,0 +1,13 @@
+profiles:
+  Esp32BalanceBot:
+    fqbn: arduino:esp32:esp32:firebeetle32
+    platforms:
+      - platform: arduino:esp32 (1.0.4)
+        platform_index_url: https://dl.espressif.com/dl/package_esp32_index.json
+    libraries:
+      - GY85 (1.0)
+      - Wire (1.0.1)
+      - MatrixMath (1.0)
+      - PS3 Controller Host (1.1.0)
+      - WiFi (1.0)
+      - ESP32 Async UDP (1.0.0)