Working code for ESP32 with balancing

TorturedEngineersDept · Jun 15, 2024 · b8cce65 · b8cce65
1 parent 2a809c7
commit b8cce65
Show file tree

Hide file tree

Showing 9 changed files with 186 additions and 83 deletions.
diff --git a/.github/workflows/workflow.yml b/.github/workflows/workflow.yml
@@ -1,22 +1,23 @@
-name: Integration test
-on:
-  push:
-    branches:
-      - "*"
-  pull_request:
-
-jobs:
-  test:
-    runs-on: [self-hosted, linux]
-    steps:
-        - uses: actions/checkout@v2
-
-        - name: Create Passwords.h
-          run: |
-            echo "#define g_ssid \"${{ secrets.WIFI_SSID }}\"" > esp32/lib/WifiSetup/Passwords.h
-            echo "#define g_password \"${{ secrets.WIFI_PASSWORD }}\"" >> esp32/lib/WifiSetup/Passwords.h
-
-        - name: Run PlatformIO tests 
-          run: |
-            cd esp32
-            pio test
+name: Integration test
+on:
+  push:
+    branches:
+      - "main"
+  pull_request:
+
+jobs:
+  test:
+    runs-on: [self-hosted, linux]
+    steps:
+        - uses: actions/checkout@v4
+
+        - name: Create Passwords.h
+          run: |
+            echo "#define g_ssid \"${{ secrets.WIFI_SSID }}\"" > esp32/lib/WifiSetup/Passwords.h
+            echo "#define g_password \"${{ secrets.WIFI_PASSWORD }}\"" >> esp32/lib/WifiSetup/Passwords.h
+
+        - name: Run PlatformIO tests 
+          shell: bash -leo pipefail {0}
+          run: |
+            cd esp32
+            pio test
diff --git a/esp32/lib/PidController/MPU6050.cpp b/esp32/lib/PidController/MPU6050.cpp
@@ -89,7 +89,6 @@ bool MPU6050::begin(unsigned long timeout, IWifi &wifi)
         break;
     }
 
-    // wifi.println("");
     return true;
 }
 

diff --git a/esp32/lib/PidController/PidController.cpp b/esp32/lib/PidController/PidController.cpp
@@ -18,6 +18,8 @@ double PidController::gyroYoffset = 0;
 double PidController::angle_setpoint = 0;
 double PidController::filtered_value = 0;
 double PidController::rotation_correction = 0;
+double PidController::speed_setpoint = 0;
+double PidController::rotation_setpoint = 0;
 
 // Initialise PID parameters using known values
 PidParams PidController::params(0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00);
@@ -48,15 +50,8 @@ bool PidController::setup(IWifi &wifi, unsigned long timeout)
         }
     }
 
-    if (start >= timeout)
-    {
-        return false;
-    }
-
     calibrate();
 
-    wifi.println("Initialised Interrupt for Stepper");
-
     // Set motor acceleration values
     step1.setAccelerationRad(MOTOR_ACCEL_RAD);
     step2.setAccelerationRad(MOTOR_ACCEL_RAD);
@@ -65,16 +60,60 @@ bool PidController::setup(IWifi &wifi, unsigned long timeout)
     pinMode(STEPPER_EN, OUTPUT);
     digitalWrite(STEPPER_EN, false);
 
+    Serial.println("Finished setting up PidController");
+
     return true;
 }
 
 void PidController::loop()
 {
     // Time of the next control update
+    static unsigned long loop0Timer = 0; // control
     static unsigned long loop1Timer = 0; // inner
     static unsigned long loop2Timer = 0; // outer
     static unsigned long loop3Timer = 0; // rotation loop
 
+    const float SPEED_SETPOINT = 1.5;
+    const float ROTATION_SPEED_SETPOINT = 0.5;
+
+    // Control the robot
+    if (millis() > loop0Timer)
+    {
+        PidDirection currentDirection = PidController::getDirection();
+        KeyDirection key_dir = currentDirection.key_dir;
+
+        switch (key_dir)
+        {
+        case KeyDirection::RIGHT:
+            speed_setpoint = 0;
+            rotation_setpoint = -ROTATION_SPEED_SETPOINT;
+            // Serial.println("RIGHT");
+            break;
+        case KeyDirection::FORWARD:
+            speed_setpoint = SPEED_SETPOINT;
+            rotation_setpoint = 0;
+            // Serial.println("FORWARD");
+            break;
+        case KeyDirection::LEFT:
+            speed_setpoint = 0;
+            rotation_setpoint = ROTATION_SPEED_SETPOINT;
+            // Serial.println("LEFT");
+            break;
+        case KeyDirection::BACKWARD:
+            speed_setpoint = -SPEED_SETPOINT;
+            rotation_setpoint = 0;
+            // Serial.println("BACKWARDS");
+            break;
+        case KeyDirection::STOP:
+        default:
+            speed_setpoint = 0;
+            rotation_setpoint = 0;
+            // Serial.println("STOP");
+        }
+
+        loop0Timer += LOOP0_INTERVAL;
+    }
+
     // Run the control loop every LOOP_INTERVAL ms
     if (millis() > loop1Timer)
     {
@@ -104,13 +143,14 @@ void PidController::stabilizedLoop()
     PidDirection currentDirection = PidController::getDirection();
     KeyDirection key_dir = currentDirection.key_dir;
 
-    const float SPEED = 10;
+    const float SPEED = 10.0;
+    const float ROTATION_SPEED = 5.0;
 
     switch (key_dir)
     {
     case KeyDirection::RIGHT:
-        step1.setTargetSpeedRad(SPEED);
-        step2.setTargetSpeedRad(SPEED);
+        step1.setTargetSpeedRad(ROTATION_SPEED);
+        step2.setTargetSpeedRad(ROTATION_SPEED);
         // Serial.println("RIGHT");
         break;
     case KeyDirection::FORWARD:
@@ -119,8 +159,8 @@ void PidController::stabilizedLoop()
         // Serial.println("FORWARD");
         break;
     case KeyDirection::LEFT:
-        step1.setTargetSpeedRad(-SPEED);
-        step2.setTargetSpeedRad(-SPEED);
+        step1.setTargetSpeedRad(-ROTATION_SPEED);
+        step2.setTargetSpeedRad(-ROTATION_SPEED);
         // Serial.println("LEFT");
         break;
     case KeyDirection::BACKWARD:
@@ -190,6 +230,7 @@ void PidController::innerLoop()
     // betweeen subsequent calls to this function
     static double previous_angle_error = 0;
     static double robot_angle = 0.0;
+    static double integral_angle = 0;
 
     const double COMP_FILTER = 0.98;  // complementary filter coefficient
     const double LOOP1_INTERVAL = 10; // inner loop
@@ -217,13 +258,14 @@ void PidController::innerLoop()
 
     double gyro_angle = g.gyro.y - gyroYoffset;
 
-    robot_angle = (1 - COMP_FILTER) * (tiltx) + COMP_FILTER;
-    robot_angle *= ((gyro_angle * (LOOP1_INTERVAL / 1000)) + robot_angle);
+    robot_angle = (1 - COMP_FILTER) * (tiltx) +
+                  COMP_FILTER *
+                      ((gyro_angle * (LOOP1_INTERVAL / 1000)) + robot_angle);
 
     // PIDeez Nuts
     double angle_error = angle_setpoint - robot_angle * 57.32;
     double Pout_a = kp_i * angle_error;
-    double integral_angle = integral_angle + ((angle_error * LOOP1_INTERVAL) / 1000);
+    integral_angle += ((angle_error * LOOP1_INTERVAL) / 1000);
     double Iout_a = ki_i * integral_angle;
     double Dout_a = kd_i * ((angle_error - previous_angle_error) / LOOP1_INTERVAL) * 1000;
     double motor_out = Pout_a + Iout_a + Dout_a;
@@ -256,7 +298,6 @@ void PidController::outerLoop()
     static double integral_speed = 0;
     static double derivative_speed = 0;
     static double current_speed_setpoint = 0;
-    static double speed_setpoint = 0; // outer loop setpoint
 
     const double TIME_CONSTANT = 2000;
     const double BETA = 1 - exp(-LOOP2_INTERVAL / TIME_CONSTANT);
@@ -271,7 +312,6 @@ void PidController::outerLoop()
 
     current_speed_setpoint += BETA * (speed_setpoint - current_speed_setpoint);
 
-    // filtered_value = (step1.getSpeedRad()-step2.getSpeedRad())/2;
     speed_error = -(current_speed_setpoint - filtered_value);
     double Pout_s = kp_o * speed_error;
     integral_speed = integral_speed + speed_error * (LOOP2_INTERVAL / 1000);
@@ -318,7 +358,6 @@ void PidController::rotationLoop()
 {
     static double rotation_angle = 0;
     static double rotation_target_angle = 0;
-    static double rotation_setpoint = 0;
     static double previous_rotation_error;
 
     const float KP_ROTATION = 3.00;

diff --git a/esp32/lib/PidController/PidController.h b/esp32/lib/PidController/PidController.h
@@ -23,8 +23,9 @@
 
 struct PidParams
 {
-    PidParams(float ki_i, float kp_i, float kd_i, float tilt_setpoint,
-              float ki_o, float kp_o, float kd_o, float velocity_setpoint, float rotation_setpoint)
+    PidParams(float kp_i, float ki_i, float kd_i, float tilt_setpoint,
+              float kp_o, float ki_o, float kd_o, float velocity_setpoint,
+              float rotation_setpoint)
         : kp_i(kp_i), ki_i(ki_i), kd_i(kd_i), tilt_setpoint(tilt_setpoint),
           kp_o(kp_o), ki_o(ki_o), kd_o(kd_o), velocity_setpoint(velocity_setpoint),
           rotation_setpoint(rotation_setpoint) {}
@@ -122,6 +123,7 @@ class PidController
     // Class constants
     static constexpr int STEPPER_INTERVAL_US = 10;
     static constexpr float MOTOR_ACCEL_RAD = 30.0;
+    static constexpr double LOOP0_INTERVAL = 20;  // control
     static constexpr double LOOP1_INTERVAL = 10;  // inner
     static constexpr double LOOP2_INTERVAL = 200; // outer
     static constexpr double LOOP3_INTERVAL = 20;  // rotation
@@ -135,4 +137,6 @@ class PidController
     static double angle_setpoint; // inner loop setpoint
     static double filtered_value;
     static double rotation_correction;
+    static double speed_setpoint;
+    static double rotation_setpoint;
 };
diff --git a/esp32/lib/WifiSetup/MqttSetup.cpp b/esp32/lib/WifiSetup/MqttSetup.cpp
@@ -1,5 +1,6 @@
 #include "MqttSetup.h"
 #include "BatteryModule.h"
+#include "PidController.h"
 
 float x = 0;
 float y = 0;
@@ -46,6 +47,9 @@ void MqttSetup::loop()
     // Ensure the client remains connected
     if (!isConnected())
     {
+        // Safety mechanism
+        PidController::setDirection(PidDirection(0, KeyDirection::STOP));
+
         Serial.println("MQTT not connected, attempting to reconnect...");
         connect();
     }

diff --git a/esp32/lib/WifiSetup/WifiSetup.cpp b/esp32/lib/WifiSetup/WifiSetup.cpp
@@ -220,6 +220,8 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length)
         }
         else if (strcmp(topic, "user/pid") == 0)
         {
+            Serial.println("Setting PID coefficients");
+
             // Get previous coefficients
             PidParams params = PidController::getParams();
 
@@ -254,13 +256,23 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length)
             if (message == "/auto" || message == "/a")
             {
                 // Release the mutex so the Raspberry Pi can take control
-                xSemaphoreGive(PidController::controlMutex);
+                if (xSemaphoreTake(
+                        PidController::controlMutex,
+                        (TickType_t)0) == pdTRUE)
+                {
+                    xSemaphoreGive(PidController::controlMutex);
+                }
             }
             else if (message == "/manual" || message == "/m")
             {
                 // Take the mutex so the Wifi can take control
                 xSemaphoreTake(PidController::controlMutex, (TickType_t)0);
             }
+            else if (message == "/reset" || message == "/r")
+            {
+                // Raise an exception, so the ESP32 resets
+                throw std::runtime_error("Resetting ESP32");
+            }
 
             Serial.println("Received command from CLI: " + message);
         }
-Original file line number
+Diff line change
@@ Expand Up / @@ -89,7 +89,6 @@ bool MPU6050::begin(unsigned long timeout, IWifi &wifi) @@
             break;
         }
-        // wifi.println("");
         return true;
     }
@@ Expand Down @@