Fix MPU6050 test and refactor wifi.println()

.gitignore

@@ -1,8 +1,9 @@
-.pio/
-.vscode/.browse.c_cpp.db*
-.vscode/c_cpp_properties.json
-.vscode/launch.json
-.vscode/ipch
-
-# SSID and Password
-Passwords.h
+.pio/
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
+node_modules
+
+# SSID and Password
+Passwords.h

esp32/lib/PidController/PidController.cpp

@@ -13,23 +13,36 @@ SemaphoreHandle_t PidController::directionMutex = xSemaphoreCreateMutex();
 PidParams PidController::params(3, 0.00, 0.12, -2.5, 0.00, 0.00, 0.00, 0.00);
 PidDirection PidController::direction(0, 0);
 
-void PidController::setup(IWifi &wifi)
+bool PidController::setup(IWifi &wifi, unsigned long timeout)
 {
     // Set the wifi interface
     PidController::wifi = &wifi;
+    unsigned long start = 0;
+
+    // Timeout not entirely accurate, but good enough
+    if (!mpu.begin(timeout, wifi))
+    {
+        return false;
+    }
 
-    mpu.begin(ULONG_MAX, wifi);
     pinMode(TOGGLE_PIN, OUTPUT);
 
     // Attach motor update ISR to timer to run every STEPPER_INTERVAL_US μs
     if (!ITimer.attachInterruptInterval(STEPPER_INTERVAL_US, timerHandler))
     {
         wifi.println("Failed to start stepper interrupt");
-        while (1)
+        while (start < timeout)
         {
             delay(10);
+            start += 10;
         }
     }
+
+    if (start >= timeout)
+    {
+        return false;
+    }
+
     wifi.println("Initialised Interrupt for Stepper");
 
     // Set motor acceleration values
@@ -39,6 +52,8 @@ void PidController::setup(IWifi &wifi)
     // Enable the stepper motor drivers
     pinMode(STEPPER_EN, OUTPUT);
     digitalWrite(STEPPER_EN, false);
+
+    return true;
 }
 
 void PidController::loop()

esp32/lib/PidController/PidController.h

@@ -51,7 +51,11 @@ class PidController
 public:
     PidController() = delete;
 
-    static void setup(IWifi &wifi);
+    /**
+     * Setup the PID controller.
+     * @return true if the setup was successful, false otherwise.
+     */
+    static bool setup(IWifi &wifi, unsigned long timeout = ULONG_MAX);
     static void loop();
 
     /**
@@ -88,7 +92,6 @@ class PidController
     static SemaphoreHandle_t directionMutex;
 
     // Class constants
-    static constexpr int PRINT_INTERVAL = 50;
     static constexpr double LOOP_INTERVAL = 5;
     static constexpr int STEPPER_INTERVAL_US = 10;
     static constexpr float MOTOR_ACCEL_RAD = 30.0;

esp32/lib/WifiSetup/WifiSetup.cpp

@@ -69,9 +69,6 @@ void WifiSetup::connect(unsigned long timeout)
     // Setup MQTT
     mqtt.setCallback(callback);
     mqtt.connect(timeout - start);
-
-    // Done
-    connected = true;
 }
 
 void WifiSetup::getStrength() const
@@ -124,7 +121,7 @@ void WifiSetup::loop()
 
 void WifiSetup::print(const char *message)
 {
-    if (connected)
+    if (mqttConnected())
     {
         DebugMessage debugMessage(message);
         mqtt.publishMessage(debugMessage);
@@ -134,7 +131,7 @@ void WifiSetup::print(const char *message)
 
 void WifiSetup::println(const char *message)
 {
-    if (connected)
+    if (mqttConnected())
     {
         std::string msg = std::string(message) + "\n";
         DebugMessage debugMessage(msg.c_str());

esp32/lib/WifiSetup/WifiSetup.h

@@ -75,5 +75,4 @@ class WifiSetup : public IWifi
     const char *username;
     const char *password;
     MqttSetup mqtt;
-    bool connected = false;
 };

esp32/src/main.cpp

@@ -8,6 +8,9 @@ WifiSetup wifi(g_ssid, g_password, MQTT_SERVER, MQTT_PORT);
 // WifiSetup wifi(i_ssid, i_email, i_password, MQTT_SERVER, MQTT_PORT);
 // PidController is a static class, so we don't need to create an instance of it
 
+// The loop interval in milliseconds (should be ULONG_MAX for normal operation)
+unsigned long timeout = 1000;
+
 void pidLoop(void *pvParameters);
 
 void setup()
@@ -16,17 +19,19 @@ void setup()
 
     // Setup modules
     wifi.connect();
-    PidController::setup(wifi);
-
-    // Setup the pidLoop
-    // xTaskCreatePinnedToCore(
-    //     pidLoop,   /* Task function. */
-    //     "pidLoop", /* name of task. */
-    //     10000,     /* Stack size of task = 40 KB */
-    //     NULL,      /* parameter of the task */
-    //     1,         /* priority of the task */
-    //     NULL,      /* Task handle to keep track of created task */
-    //     0);        /* pin task to core 0 */
+
+    if (PidController::setup(wifi, timeout))
+    {
+        // Setup the pidLoop
+        xTaskCreatePinnedToCore(
+            pidLoop,   /* Task function. */
+            "pidLoop", /* name of task. */
+            10000,     /* Stack size of task = 40 KB */
+            NULL,      /* parameter of the task */
+            1,         /* priority of the task */
+            NULL,      /* Task handle to keep track of created task */
+            1);        /* pin task to core 1 */
+    }
 }
 
 void loop()

esp32/test/test_mpu6050/test_mpu6050.cpp

@@ -1,4 +1,5 @@
 #include "MPU6050.h"
+#include "WifiSetup.h"
 #include <Arduino.h>
 #include <Wire.h>
 #include <unity.h>
@@ -9,7 +10,8 @@ void setUp(void)
 {
     // This will be run before each test
     Wire.begin();
-    TEST_ASSERT_TRUE_MESSAGE(mpu.begin(100), "Failed to initialize MPU6050");
+    WifiSetup wifi("ssid", "password", "mqtt_server", 1883);
+    TEST_ASSERT_TRUE_MESSAGE(mpu.begin(100, wifi), "Failed to initialize MPU6050");
 }
 
 void tearDown(void)