From ecc20050e8c8b636218a6851ac2bd796c624313e Mon Sep 17 00:00:00 2001 From: saturn691 Date: Thu, 13 Jun 2024 15:36:03 +0100 Subject: [PATCH] Integrate circuits and control onto ESP32 --- .../single_stepper_motor/platformio.ini | 40 +- esp32-tests/single_stepper_motor/src/main.cpp | 486 ++++++++++++++++-- esp32-tests/single_stepper_motor/src/step.h | 174 +++++++ esp32/include/README | 39 -- esp32/include/SerialLoop.h | 71 +-- esp32/lib/BatteryModule/BatteryModule.cpp | 381 ++++++++++++++ esp32/lib/BatteryModule/BatteryModule.h | 64 +++ esp32/lib/PidController/PidController.cpp | 259 +++++++--- esp32/lib/PidController/PidController.h | 38 +- esp32/lib/WifiSetup/MqttSetup.cpp | 16 +- esp32/lib/WifiSetup/MqttSetup.h | 2 - esp32/lib/WifiSetup/WifiSetup.cpp | 25 +- esp32/platformio.ini | 5 +- esp32/src/main.cpp | 32 +- esp32/test/test_battery/test_battery.cpp | 34 ++ esp32/test/test_mpu6050/test_mpu6050.cpp | 2 + esp32/test/test_wifi/test_wifi.cpp | 22 +- 17 files changed, 1466 insertions(+), 224 deletions(-) create mode 100644 esp32-tests/single_stepper_motor/src/step.h delete mode 100644 esp32/include/README create mode 100644 esp32/lib/BatteryModule/BatteryModule.cpp create mode 100644 esp32/lib/BatteryModule/BatteryModule.h create mode 100644 esp32/test/test_battery/test_battery.cpp diff --git a/esp32-tests/single_stepper_motor/platformio.ini b/esp32-tests/single_stepper_motor/platformio.ini index 6cd0995..61172c8 100644 --- a/esp32-tests/single_stepper_motor/platformio.ini +++ b/esp32-tests/single_stepper_motor/platformio.ini @@ -1,15 +1,25 @@ -; PlatformIO Project Configuration File -; -; Build options: build flags, source filter -; Upload options: custom upload port, speed and extra flags -; Library options: dependencies, extra library storages -; Advanced options: extra scripting -; -; Please visit documentation for the other options and examples -; https://docs.platformio.org/page/projectconf.html - -[env:esp-wrover-kit] -platform = espressif32 -board = esp-wrover-kit -framework = arduino -monitor_speed = 115200 \ No newline at end of file +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp-wrover-kit] +platform = espressif32 +board = esp-wrover-kit +framework = arduino +monitor_speed = 115200 +lib_deps = + knolleary/PubSubClient + bblanchon/ArduinoJson + khoih-prog/TimerInterrupt_Generic@^1.13.0 + adafruit/Adafruit MPU6050 + arduino-libraries/NTPClient @ ^3.2.1 + adafruit/Adafruit Unified Sensor + adafruit/Adafruit BusIO + Wire + SPI diff --git a/esp32-tests/single_stepper_motor/src/main.cpp b/esp32-tests/single_stepper_motor/src/main.cpp index 2df91e1..ad2619d 100644 --- a/esp32-tests/single_stepper_motor/src/main.cpp +++ b/esp32-tests/single_stepper_motor/src/main.cpp @@ -1,55 +1,465 @@ -// Single Stepper Motor Test -// Author: Sanjit Raman -// Date: 21 May 2024 -// Version: 1.0.0 -// Description: -// This code is meant to work with an A4988 motor driver connected to the -// microcontroller with the pinouts as described above. The expected result: -// - the stepper motor spins FORWARDS for 1 second -// - waits for 1 second -// - the stepper motor spins BACKWARDS for 1 second -// - at the end the motor should return to the same position it started -// at. - #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// MQTT +#define server "18.130.87.186" +#define server_port 1883 +#define timeout 5000 + +// The Stepper pins +#define STEPPER1_DIR_PIN 16 // Arduino D9 +#define STEPPER1_STEP_PIN 17 // Arduino D8 +#define STEPPER2_DIR_PIN 4 // Arduino D11 +#define STEPPER2_STEP_PIN 14 // Arduino D10 +#define STEPPER_EN 15 // Arduino D12 + +// Diagnostic pin for oscilloscope +#define TOGGLE_PIN 32 // Arduino A4 + +// now all the variables lol +const int PRINT_INTERVAL = 500; +const double LOOP1_INTERVAL = 10; // inner loop +const double LOOP2_INTERVAL = 200; // outer loop +const double LOOP3_INTERVAL = 20; // loop to stop/control rotations (work in progress) +const int STEPPER_INTERVAL_US = 20; + +double receivedNumber = 0; // number received from webpage +double angle_setpoint = 0; // inner loop setpoint +double speed_setpoint = 0; // outer loop setpoint +double rotation_setpoint = 0; +double rotation_target_angle = 0; +double current_speed_setpoint = 0; +double balancing_setpoint = 0; // setpoint to balance the bot in place +double kp_i; // inner +double kd_i = 0; // inner +double ki_i = 0; // inner +double kp_o = 0; // outer +double kd_o = 0; // outer +double ki_o = 0; // outer +double comp_filter = 0.98; // complementary filter coefficient +sensors_event_t a, g, temp; // MPU sensor +double accXoffset_sum = 0; +double accYoffset_sum = 0; +double accZoffset_sum = 0; +double gyroXoffset_sum = 0; +double gyroYoffset_sum = 0; +double accXoffset; +double accYoffset; +double accZoffset; +double gyroXoffset; +double gyroYoffset; + +// MQTT Client +WiFiClient espClient; +PubSubClient client(espClient); +void callback(char *topic, byte *payload, unsigned int length); + +// Global objects +ESP32Timer ITimer(3); +Adafruit_MPU6050 mpu; // Default pins for I2C are SCL: IO22/Arduino D3, SDA: IO21/Arduino D4 + +step step1(STEPPER_INTERVAL_US, STEPPER1_STEP_PIN, STEPPER1_DIR_PIN); +step step2(STEPPER_INTERVAL_US, STEPPER2_STEP_PIN, STEPPER2_DIR_PIN); + +// all the webserver stuff +const char *ssid = "hi"; +const char *password = "password"; + +// Interrupt Service Routine for motor update +// Note: ESP32 doesn't support floating point calculations in an ISR +bool TimerHandler(void *timerNo) +{ + static bool toggle = false; + + // Update the stepper motors + step1.runStepper(); + step2.runStepper(); -// Define pin connections & motor's steps per revolution -const int dirPin = 2; -const int stepPin = 3; -const int stepsPerRevolution = 200; + // Indicate that the ISR is running + digitalWrite(TOGGLE_PIN, toggle); + toggle = !toggle; + return true; +} void setup() { - // Declare pins as Outputs - pinMode(stepPin, OUTPUT); - pinMode(dirPin, OUTPUT); + Serial.begin(115200); + delay(1000); + pinMode(TOGGLE_PIN, OUTPUT); + + // Try to initialize Accelerometer/Gyroscope + if (!mpu.begin()) + { + Serial.println("Failed to find MPU6050 chip"); + while (1) + { + delay(10); + } + } + Serial.println("MPU6050 Found!"); + + mpu.setAccelerometerRange(MPU6050_RANGE_2_G); + mpu.setGyroRange(MPU6050_RANGE_250_DEG); + mpu.setFilterBandwidth(MPU6050_BAND_44_HZ); + + for (int i = 0; i < 200; i++) + { + mpu.getEvent(&a, &g, &temp); + accXoffset_sum += a.acceleration.x; + accYoffset_sum += a.acceleration.y; + accZoffset_sum += a.acceleration.z; + gyroXoffset_sum += g.gyro.x; + gyroYoffset_sum += g.gyro.y; + } + + accXoffset = accXoffset_sum / 200 - 9.81; + accYoffset = accYoffset_sum / 200; + accZoffset = accZoffset_sum / 200; + gyroXoffset = gyroXoffset_sum / 200; + gyroYoffset = gyroYoffset_sum / 200; + + // Attach motor update ISR to timer to run every STEPPER_INTERVAL_US μs + if (!ITimer.attachInterruptInterval(STEPPER_INTERVAL_US, TimerHandler)) + { + Serial.println("Failed to start stepper interrupt"); + while (1) + delay(10); + } + Serial.println("Initialised Interrupt for Stepper"); + + // Set motor acceleration values (initial values, dont really do anything though) + step1.setAccelerationRad(30); + step2.setAccelerationRad(30); + // Enable the stepper motor drivers + pinMode(STEPPER_EN, OUTPUT); + digitalWrite(STEPPER_EN, false); + + // START WIFI + WiFi.begin(ssid, password); + Serial.println("wifi.begun"); + while (WiFi.status() != WL_CONNECTED) + { + delay(1000); + Serial.println("Connecting to WiFi..."); + } + + delay(100); + + // GET WIFI Strength + Serial.print("WiFi Strength:"); + Serial.println(WiFi.RSSI()); + Serial.println("Check WiFi strength if no connection is established to Broker (<-55)"); + + // MQTT SETUP + Serial.println("Connected to WiFi"); + Serial.print("IP address: "); + Serial.println(WiFi.localIP()); + + // CONNECT TO MQTT BROKER + client.setServer(server, server_port); + client.setCallback(callback); + + unsigned long start = 0; + Serial.print("IP of MQTT Broker: "); + Serial.println(server); + Serial.println("Check this IP in case no connection is established to Broker"); + while (!client.connected() && start < timeout) + { + Serial.print("Attempting MQTT connection..."); + if (client.connect("Varun")) + { + Serial.println("connected"); + client.subscribe("user/pid"); + } + else + { + Serial.print("failed, rc="); + Serial.print(client.state()); + Serial.println(" try again in 1 second"); + delay(1000); + } + } } void loop() { - // Set motor direction clockwise - digitalWrite(dirPin, HIGH); - // Spin motor slowly - for (int x = 0; x < stepsPerRevolution; x++) + // Maintain MQTT connection + if (!client.connected()) + { + unsigned long start = millis(); + while (!client.connected() && millis() - start < timeout) + { + Serial.print("Attempting MQTT reconnection..."); + if (client.connect("ESP32Client")) + { + Serial.println("connected"); + client.subscribe("user/pid"); + } + else + { + Serial.print("failed, rc="); + Serial.print(client.state()); + Serial.println(" try again in 0.5 seconds"); + delay(500); + } + } + } + client.loop(); + + // Static variables are initialised once and then the value is remembered betweeen subsequent calls to this function + static unsigned long printTimer = 500; // time of the next print + static unsigned long loop1Timer = 0; // inner + static unsigned long loop2Timer = 0; // outer + static unsigned long loop3Timer = 0; // rotation loop + static unsigned long milliseconds = 0; + static double acceleration_angle = 0.0; + static double gyro_angle = 0.0; // current tilt angle + static double robot_angle = 0.0; + static double motor_speed1 = 17; + static double motor_speed2 = 17; + + // variables for rotation loop (work in progress) + static double rotation = 0, rotation_error = 0, motor_out = 0, rotation_correction = 0, rotation_angle, previous_rotation_error, Pout_r, Dout_r, Iout_r; + + double kp_rotation = 3; + double kd_rotation = 5; + double ki_rotation = 0; + + if (millis() > loop3Timer) + { + mpu.getEvent(&a, &g, &temp); + loop3Timer += LOOP3_INTERVAL; + motor_speed1 = 17; + motor_speed2 = 17; + + rotation = g.gyro.x - gyroXoffset; + + rotation_angle = rotation * (LOOP3_INTERVAL / 1000) + rotation_angle; + + rotation_target_angle = rotation_target_angle + (rotation_setpoint * 0.4 * LOOP3_INTERVAL) / 1000; + + rotation_error = -(rotation_target_angle - rotation_angle); + + Pout_r = kp_rotation * rotation_error; + Dout_r = kd_rotation * ((rotation_error - previous_rotation_error) / LOOP3_INTERVAL) * 1000; + + previous_rotation_error = rotation_error; + + rotation_correction = Pout_r + Dout_r; + } + + // variables for inner loop + static double angle_error, previous_angle_error = 0, integral_angle = 0, Pout_a = 0, Iout_a = 0, Dout_a = 0, filtered_value = 0; + + double alpha = 0.05, accX, accY, accZ; + + if (millis() > loop1Timer) + { + loop1Timer += LOOP1_INTERVAL; + mpu.getEvent(&a, &g, &temp); + accX = a.acceleration.x - accXoffset; + accY = a.acceleration.y - accYoffset; + accZ = a.acceleration.z - accZoffset; + + // Calculate Tilt using accelerometer and sin x = x approximation for a small tilt angle + // acceleration_angle = (accZ) / (9.67); + acceleration_angle = (accZ) / sqrt(pow(accY, 2) + pow(accX, 2)); + + gyro_angle = g.gyro.y - gyroYoffset; + + robot_angle = (1 - comp_filter) * (acceleration_angle) + comp_filter * ((gyro_angle * (LOOP1_INTERVAL / 1000)) + robot_angle); + + // PIDeez Nuts + + angle_error = angle_setpoint - robot_angle * 57.32; + + Pout_a = kp_i * angle_error; + + integral_angle = integral_angle + ((angle_error * LOOP1_INTERVAL) / 1000); + Iout_a = ki_i * integral_angle; + + Dout_a = kd_i * ((angle_error - previous_angle_error) / LOOP1_INTERVAL) * 1000; + + motor_out = Pout_a + Iout_a + Dout_a; + + previous_angle_error = angle_error; + + if (motor_out >= 0) + { + step1.setTargetSpeedRad(motor_speed1); + step2.setTargetSpeedRad(-motor_speed2); + } + else + { + step1.setTargetSpeedRad(-motor_speed1); + step2.setTargetSpeedRad(motor_speed2); + } + + step1.setAccelerationRad(motor_out + rotation_correction); // uses absolute values + step2.setAccelerationRad(motor_out - rotation_correction); + + filtered_value = alpha * ((step1.getSpeedRad() - step2.getSpeedRad()) / 2) + (1 - alpha) * filtered_value; + } + + // variables for outer loop + static double avgspeed = 0, avg_temp = 0, speed_error = 0, previous_speed_error = 0, integral_speed = 0, derivative_speed = 0, Pout_s = 0, Iout_s = 0, Dout_s = 0, ramp_rate = 0; + + double max_ramp_rate = 0.5; + double time_constant = 2000; + double beta = 1 - exp(-LOOP2_INTERVAL / time_constant); + + static int speed_size = 0; + + if (millis() > loop2Timer) + { + loop2Timer += LOOP2_INTERVAL; + + // ramp_rate = max_ramp_rate * fabs(speed_error); // Variable ramp rate proportional to the error + + // // Clamp ramp_rate to max_ramp_rate + // if (ramp_rate > max_ramp_rate) { + // ramp_rate = max_ramp_rate; + // } + + // if (fabs(speed_error) < ramp_rate) { + // // If the error is smaller than the ramp rate, set it directly + // current_speed_setpoint = speed_setpoint; + // } else if (current_speed_setpoint < speed_setpoint) { + // // Increase the setpoint gradually + // current_speed_setpoint += ramp_rate; + // } else if (current_speed_setpoint > speed_setpoint) { + // // Decrease the setpoint gradually + // current_speed_setpoint -= ramp_rate; + // } + + current_speed_setpoint += beta * (speed_setpoint - current_speed_setpoint); + + // filtered_value = (step1.getSpeedRad()-step2.getSpeedRad())/2; + speed_error = -(current_speed_setpoint - filtered_value); + + Pout_s = kp_o * speed_error; + + integral_speed = integral_speed + speed_error * (LOOP2_INTERVAL / 1000); + + if (ki_o == 0) + { + integral_speed = 0; + } + + Iout_s = ki_o * integral_speed; + + derivative_speed = ((speed_error - previous_speed_error) / LOOP2_INTERVAL) * 1000; + + Dout_s = kd_o * derivative_speed; + + angle_setpoint = Pout_s + Iout_s + Dout_s + balancing_setpoint; + + if (speed_setpoint > 0) + { + if (angle_setpoint > balancing_setpoint + 1) + { + angle_setpoint = balancing_setpoint + 1; + } + else if (angle_setpoint < balancing_setpoint - 5) + { + angle_setpoint = balancing_setpoint - 5; + } + } + + else if (speed_setpoint < 0) + { + if (angle_setpoint < balancing_setpoint - 1) + { + angle_setpoint = balancing_setpoint - 1; + } + else if (angle_setpoint > balancing_setpoint + 5) + { + angle_setpoint = balancing_setpoint + 5; + } + } + + previous_speed_error = speed_error; + } + + if (millis() > printTimer) { - digitalWrite(stepPin, HIGH); - delayMicroseconds(1000); - digitalWrite(stepPin, LOW); - delayMicroseconds(1000); + printTimer += PRINT_INTERVAL; + + Serial.print(rotation_setpoint * 57.32); + Serial.print(' '); + Serial.println(rotation_angle * 57.32); + // Serial.print(' '); + // Serial.println(filtered_value); + + // Publish the data to the MQTT broker + StaticJsonDocument<200> doc; + doc["timestamp"] = millis(); + doc["tilt"] = rotation_angle; + // doc["velocity"] = filtered_value; + + char buffer[512]; + serializeJson(doc, buffer); + // client.publish("tuning/graphing", buffer); } - delay(1000); // Wait a second +} + +void callback(char *topic, byte *payload, unsigned int length) +{ + Serial.print("Message arrived ["); + Serial.print(topic); + Serial.print("] "); + + // Create a buffer to store the payload + char payloadStr[length + 1]; + memcpy(payloadStr, payload, length); + payloadStr[length] = '\0'; // Null-terminate the string - // Set motor direction counterclockwise - digitalWrite(dirPin, LOW); + Serial.println(payloadStr); - // Spin motor quickly - for (int x = 0; x < stepsPerRevolution; x++) + // Parse the JSON + StaticJsonDocument<200> doc; + DeserializationError error = deserializeJson(doc, payloadStr); + + if (error) + { + Serial.print("Failed to parse JSON: "); + Serial.println(error.c_str()); + return; + } + + if (strcmp(topic, "user/pid") == 0) + { + // Extract values from the JSON document + kp_i = doc["kp_i"].as(); + ki_i = doc["ki_i"].as(); + kd_i = doc["kd_i"].as(); + balancing_setpoint = doc["setpoint_i"].as(); + kp_o = doc["kp_o"].as(); + ki_o = doc["ki_o"].as(); + kd_o = doc["kd_o"].as(); + speed_setpoint = doc["setpoint_o"].as(); + rotation_setpoint = doc["rotation_setpoint"].as(); + Serial.println("kp_i: " + String(kp_i) + + ", ki_i: " + String(ki_i) + + ", kd_i: " + String(kd_i) + + ", Balancing Setpoint: " + String(balancing_setpoint) + + ", kp_o: " + String(kp_o) + + ", ki_o: " + String(ki_o) + + ", kd_o: " + String(kd_o) + + ", Speed Setpoint: " + String(speed_setpoint)); + } + else { - digitalWrite(stepPin, HIGH); - delayMicroseconds(1000); - digitalWrite(stepPin, LOW); - delayMicroseconds(1000); + Serial.println("Unsupported topic"); } - delay(1000); // Wait a second } diff --git a/esp32-tests/single_stepper_motor/src/step.h b/esp32-tests/single_stepper_motor/src/step.h new file mode 100644 index 0000000..35be960 --- /dev/null +++ b/esp32-tests/single_stepper_motor/src/step.h @@ -0,0 +1,174 @@ +#include + +class step +{ + +public: + const int MAX_SPEED = 10000; // Maximum motor speed (steps/s) + const int MAX_SPEED_INTERVAL_US = 1000; // Maximum interval between speed updates (μs) + const int SPEED_SCALE = 2000; // Integer speed units are in steps per SPEED_SCALE seconds + const int MICROSTEPS = 16; // Number of microsteps per physical step + const int STEPS = 200; // Number of physical steps per revolution + const float STEP_ANGLE = (2.0 * PI) / (STEPS * MICROSTEPS); // Angle per microstep (rad) + int32_t accel = 0; // current acceleration (steps/s) + int32_t tSpeed = 0; // current speed (steps/(SPEED_SCALE * s)) + + // Initialise the stepper with interval and pin numbers + step(int i, int8_t sp, int8_t dp) : interval(i), stepPin(sp), dirPin(dp) + { + pinMode(stepPin, OUTPUT); + pinMode(dirPin, OUTPUT); + } + + // Update the stepper motor, performing a step and updating the speed as necessary. Call every interval μs + void runStepper() + { + // Note: ESP32 doesn't support floating point calculations in an ISR, so this function only uses integer operations + + // Increment speed calculation interval timer + speedTimer += interval; + + // Check for stepping active + if (step_period != 0) + { + + // Increment step timer + stepTimer += interval; + + // Check for step period elapsed + if (stepTimer > step_period) + { + + // Start pulse + digitalWrite(stepPin, HIGH); + + // Roll back step timer by one period + stepTimer -= step_period; + + // Recaculate step interval + updateSpeed(); + + // Set step direction for next step + digitalWrite(dirPin, speed > 0); + + // Increment/decrement position counter + position += (speed > 0) ? 1 : -1; + + // End pulse + digitalWrite(stepPin, LOW); + } + } + else + { + // Do nothing if stepping inactive + stepTimer = 0; + } + + // Recalculate step interval if the last update was longer ago than MAX_SPEED_INTERVAL_US + if (speedTimer > MAX_SPEED_INTERVAL_US) + updateSpeed(); + } + + // Set acceleration in rad/s/s. Do not call from ISR + void setAccelerationRad(float accelRad) + { + accel = static_cast(accelRad / STEP_ANGLE); + } + + // Set acceleration in microsteps/s/s + void setAcceleration(int newAccel) + { + accel = newAccel; + } + + // Set target speed in rad/s. Do not call from ISR + void setTargetSpeedRad(float speedRad) + { + tSpeed = static_cast(speedRad * SPEED_SCALE / STEP_ANGLE); + } + + // Set target speed in microsteps/(SPEED_SCALE * s) + void setTargetSpeed(int speed) + { + tSpeed = speed; + } + + // Get position in microsteps + int getPosition() + { + return position; + } + + // Get position in rads. Do not call from ISR + float getPositionRad() + { + return static_cast(position) * STEP_ANGLE; + } + + // Get current speed in microsteps/(SPEED_SCALE * s) + float getSpeed() + { + return speed; + } + + // Get current speed in rad/s. Do not call from ISR + float getSpeedRad() + { + return static_cast(speed) * STEP_ANGLE / SPEED_SCALE; + } + +private: + int32_t stepTimer = 0; // time since last step (μs) + int32_t speedTimer = 0; // time since last speed update (μs) + int32_t step_period = 0; // current time between steps (μs) + int32_t position = 0; // current accumulated steps (steps) + int8_t stepPin; // output pin number for step + int8_t dirPin; // output pin number for direction + int32_t speed = 0; // current steps per SPEED_SCALE seconds (steps) + int32_t interval; // interval between calls to runStepper (μs) + + // Update the motor speed and step interval + void updateSpeed() + { + + if (accel < 0) + accel = -accel; + + // Calculate change to speed + if (speed < tSpeed) + { + speed += accel * speedTimer / (1000000 / SPEED_SCALE); + if (speed > tSpeed) + { + speed = tSpeed; + } + if (speed > MAX_SPEED * SPEED_SCALE) + { + speed = MAX_SPEED * SPEED_SCALE; + } + } + else + { + speed -= accel * speedTimer / (1000000 / SPEED_SCALE); + if (speed < tSpeed) + { + speed = tSpeed; + } + if (speed < -MAX_SPEED * SPEED_SCALE) + { + speed = -MAX_SPEED * SPEED_SCALE; + } + } + + // Reset speed calculation timer + speedTimer = 0; + + // Calculate step period + if (speed == 0) + step_period = 0; + else if (speed > 0) + step_period = 1000000 * SPEED_SCALE / speed; + else + step_period = -1000000 * SPEED_SCALE / speed; + } +}; diff --git a/esp32/include/README b/esp32/include/README deleted file mode 100644 index 194dcd4..0000000 --- a/esp32/include/README +++ /dev/null @@ -1,39 +0,0 @@ - -This directory is intended for project header files. - -A header file is a file containing C declarations and macro definitions -to be shared between several project source files. You request the use of a -header file in your project source file (C, C++, etc) located in `src` folder -by including it, with the C preprocessing directive `#include'. - -```src/main.c - -#include "header.h" - -int main (void) -{ - ... -} -``` - -Including a header file produces the same results as copying the header file -into each source file that needs it. Such copying would be time-consuming -and error-prone. With a header file, the related declarations appear -in only one place. If they need to be changed, they can be changed in one -place, and programs that include the header file will automatically use the -new version when next recompiled. The header file eliminates the labor of -finding and changing all the copies as well as the risk that a failure to -find one copy will result in inconsistencies within a program. - -In C, the usual convention is to give header files names that end with `.h'. -It is most portable to use only letters, digits, dashes, and underscores in -header file names, and at most one dot. - -Read more about using header files in official GCC documentation: - -* Include Syntax -* Include Operation -* Once-Only Headers -* Computed Includes - -https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/esp32/include/SerialLoop.h b/esp32/include/SerialLoop.h index d1ac2d9..1bbee00 100644 --- a/esp32/include/SerialLoop.h +++ b/esp32/include/SerialLoop.h @@ -19,44 +19,51 @@ void decode(uint8_t message, KeyDirection &direction, uint8_t &speed) #include #include "PidController.h" -/** - * Allows the Raspberry Pi to communicate with the ESP32 over serial. - * Decodes as a 8-bit ASCII - */ -void serialLoop() +namespace SerialLoop { - // Discard all buffer data - while (Serial.available() > 1) + /** + * Allows the Raspberry Pi to communicate with the ESP32 over serial. + * Decodes as a 8-bit ASCII + */ + void loop() { - Serial.read(); - } + // Discard all buffer data + while (Serial.available() > 1) + { + Serial.read(); + } - char message = Serial.read(); + char message = Serial.read(); - KeyDirection direction; - uint8_t speed; - decode(message, direction, speed); + KeyDirection direction; + uint8_t speed; + decode(message, direction, speed); - switch (direction) - { - case KeyDirection::FORWARD: - Serial.println("[rpi] FORWARD"); - break; - case KeyDirection::BACKWARD: - Serial.println("[rpi] BACKWARD"); - break; - case KeyDirection::LEFT: - Serial.println("[rpi] LEFT"); - break; - case KeyDirection::RIGHT: - Serial.println("[rpi] RIGHT"); - break; - case KeyDirection::STOP: - Serial.println("[rpi] STOP"); - break; - } + switch (direction) + { + case KeyDirection::FORWARD: + Serial.println("[rpi] FORWARD"); + break; + case KeyDirection::BACKWARD: + Serial.println("[rpi] BACKWARD"); + break; + case KeyDirection::LEFT: + Serial.println("[rpi] LEFT"); + break; + case KeyDirection::RIGHT: + Serial.println("[rpi] RIGHT"); + break; + case KeyDirection::STOP: + Serial.println("[rpi] STOP"); + break; + } - PidController::setDirection(PidDirection(speed, direction)); + if (xSemaphoreTake(PidController::controlMutex, (TickType_t)0) == pdTRUE) + { + PidController::setDirection(PidDirection(speed, direction)); + xSemaphoreGive(PidController::controlMutex); + } + } } #endif diff --git a/esp32/lib/BatteryModule/BatteryModule.cpp b/esp32/lib/BatteryModule/BatteryModule.cpp new file mode 100644 index 0000000..18f78b8 --- /dev/null +++ b/esp32/lib/BatteryModule/BatteryModule.cpp @@ -0,0 +1,381 @@ +#include "BatteryModule.h" + +std::map BatteryModule::battery_data = { + {16.19, 100}, + {16.18, 100}, + {16.17, 100}, + {16.16, 100}, + {16.15, 100}, + {16.14, 100}, + {16.13, 100}, + {16.12, 100}, + {16.11, 100}, + {16.1, 100}, + {16.09, 100}, + {16.08, 100}, + {16.07, 99}, + {16.06, 99}, + {16.05, 99}, + {16.04, 99}, + {16.03, 99}, + {16.02, 99}, + {16.01, 99}, + {16, 99}, + {15.99, 99}, + {15.98, 99}, + {15.97, 99}, + {15.96, 99}, + {15.95, 99}, + {15.94, 99}, + {15.93, 99}, + {15.92, 99}, + {15.91, 98}, + {15.9, 98}, + {15.89, 98}, + {15.88, 98}, + {15.87, 98}, + {15.86, 98}, + {15.85, 98}, + {15.84, 98}, + {15.83, 98}, + {15.82, 98}, + {15.81, 97}, + {15.8, 97}, + {15.79, 97}, + {15.78, 97}, + {15.77, 97}, + {15.76, 97}, + {15.75, 97}, + {15.74, 97}, + {15.73, 97}, + {15.72, 97}, + {15.71, 97}, + {15.7, 96}, + {15.69, 96}, + {15.68, 96}, + {15.67, 96}, + {15.66, 96}, + {15.65, 96}, + {15.64, 96}, + {15.63, 96}, + {15.62, 95}, + {15.61, 95}, + {15.6, 95}, + {15.59, 95}, + {15.58, 95}, + {15.57, 95}, + {15.56, 95}, + {15.55, 95}, + {15.54, 94}, + {15.53, 94}, + {15.52, 94}, + {15.51, 94}, + {15.5, 94}, + {15.49, 94}, + {15.48, 94}, + {15.47, 93}, + {15.46, 93}, + {15.45, 93}, + {15.44, 93}, + {15.43, 93}, + {15.42, 93}, + {15.41, 92}, + {15.4, 92}, + {15.39, 92}, + {15.38, 92}, + {15.37, 92}, + {15.36, 91}, + {15.35, 91}, + {15.34, 91}, + {15.33, 91}, + {15.32, 90}, + {15.31, 90}, + {15.3, 90}, + {15.29, 90}, + {15.28, 90}, + {15.27, 89}, + {15.26, 89}, + {15.25, 89}, + {15.24, 89}, + {15.23, 88}, + {15.22, 88}, + {15.21, 88}, + {15.2, 87}, + {15.19, 87}, + {15.18, 87}, + {15.17, 86}, + {15.16, 86}, + {15.15, 85}, + {15.14, 85}, + {15.13, 85}, + {15.12, 84}, + {15.11, 83}, + {15.1, 83}, + {15.09, 82}, + {15.08, 81}, + {15.07, 80}, + {15.06, 79}, + {15.05, 77}, + {15.04, 75}, + {15.03, 74}, + {15.02, 70}, + {15.01, 66}, + {15, 63}, + {14.99, 61}, + {14.98, 59}, + {14.97, 57}, + {14.96, 55}, + {14.95, 54}, + {14.94, 52}, + {14.93, 51}, + {14.92, 48}, + {14.91, 47}, + {14.9, 46}, + {14.89, 45}, + {14.88, 44}, + {14.87, 43}, + {14.86, 42}, + {14.85, 42}, + {14.84, 41}, + {14.83, 40}, + {14.82, 39}, + {14.81, 38}, + {14.8, 38}, + {14.79, 37}, + {14.78, 36}, + {14.77, 36}, + {14.76, 35}, + {14.75, 34}, + {14.74, 34}, + {14.73, 33}, + {14.72, 33}, + {14.71, 32}, + {14.7, 31}, + {14.69, 31}, + {14.68, 30}, + {14.67, 30}, + {14.66, 29}, + {14.65, 29}, + {14.64, 28}, + {14.63, 28}, + {14.62, 27}, + {14.61, 27}, + {14.6, 26}, + {14.59, 26}, + {14.58, 25}, + {14.57, 25}, + {14.56, 24}, + {14.55, 24}, + {14.54, 24}, + {14.53, 23}, + {14.52, 23}, + {14.51, 22}, + {14.5, 22}, + {14.49, 21}, + {14.48, 21}, + {14.47, 21}, + {14.46, 20}, + {14.45, 20}, + {14.44, 19}, + {14.43, 19}, + {14.42, 19}, + {14.41, 18}, + {14.4, 18}, + {14.39, 18}, + {14.38, 18}, + {14.37, 17}, + {14.36, 17}, + {14.35, 17}, + {14.34, 16}, + {14.33, 16}, + {14.32, 16}, + {14.31, 15}, + {14.3, 15}, + {14.29, 15}, + {14.28, 14}, + {14.27, 14}, + {14.26, 14}, + {14.25, 14}, + {14.24, 13}, + {14.23, 13}, + {14.22, 13}, + {14.21, 12}, + {14.2, 12}, + {14.19, 12}, + {14.18, 12}, + {14.17, 12}, + {14.16, 11}, + {14.15, 11}, + {14.14, 11}, + {14.13, 11}, + {14.12, 10}, + {14.11, 10}, + {14.1, 10}, + {14.09, 10}, + {14.08, 9}, + {14.07, 9}, + {14.06, 9}, + {14.05, 9}, + {14.04, 9}, + {14.03, 8}, + {14.02, 8}, + {14.01, 8}, + {14, 8}, + {13.99, 8}, + {13.98, 7}, + {13.97, 7}, + {13.96, 7}, + {13.95, 7}, + {13.94, 7}, + {13.93, 6}, + {13.92, 6}, + {13.91, 6}, + {13.9, 6}, + {13.89, 6}, + {13.88, 6}, + {13.87, 5}, + {13.86, 5}, + {13.85, 5}, + {13.84, 5}, + {13.83, 5}, + {13.82, 4}, + {13.81, 4}, + {13.8, 4}, + {13.79, 4}, + {13.78, 4}, + {13.77, 4}, + {13.76, 4}, + {13.75, 3}, + {13.74, 3}, + {13.73, 3}, + {13.72, 3}, + {13.71, 3}, + {13.7, 3}, + {13.69, 2}, + {13.68, 2}, + {13.67, 2}, + {13.66, 2}, + {13.65, 2}, + {13.64, 2}, + {13.63, 2}, + {13.62, 1}, + {13.61, 1}, + {13.6, 1}, + {13.59, 1}, + {13.58, 1}, + {13.57, 1}, + {13.56, 1}, + {13.55, 1}, + {13.54, 0}, + {13.53, 0}, + {13.52, 0}, + {13.51, 0}, + {13.5, 0}}; + +int BatteryModule::calcBatteryPercentage(float voltage) +{ + int percentage; + + if (voltage > 16.19) + { + percentage = 100; + } + else if (voltage < 13.5) + { + percentage = 0; + } + else + { + percentage = battery_data[voltage]; + } + + return percentage; +} + +#ifndef NATIVE + +// Initialize the battery voltage and percentage +SemaphoreHandle_t BatteryModule::battery_mutex = xSemaphoreCreateMutex(); +SemaphoreHandle_t BatteryModule::power_mutex = xSemaphoreCreateMutex(); +float BatteryModule::filtered_battery_voltage = 17.0; +int BatteryModule::filtered_battery_percentage = 100; +float BatteryModule::total_power_consumption = 0.0; +unsigned long BatteryModule::start_time = millis(); + +void BatteryModule::setup() +{ + + float battery_setup = 0; + for (int i = 0; i < 200; i++) + { + battery_setup += ((analogRead(VB_PIN) / ADC_RESOLUTION) * ADC_MAX_VOLTAGE) / VOLTAGE_DIVIDER_RATIO; + } + filtered_battery_voltage = battery_setup / 200; +} + +void BatteryModule::loop() +{ + // Read raw ADC values + int vb_raw = analogRead(VB_PIN); + int i5_raw = analogRead(I5_PIN); + int im_raw = analogRead(IM_PIN); + + // Convert raw ADC values to voltages + float vb_voltage = (vb_raw / ADC_RESOLUTION) * ADC_MAX_VOLTAGE; + float i5_voltage = ((i5_raw / ADC_RESOLUTION) * ADC_MAX_VOLTAGE); + float im_voltage = ((im_raw / ADC_RESOLUTION) * ADC_MAX_VOLTAGE); + + /// Calculate actual battery voltage and currents + float battery_voltage = vb_voltage / VOLTAGE_DIVIDER_RATIO; + filtered_battery_voltage = 0.01 * (battery_voltage) + (1 - 0.01) * filtered_battery_voltage; + filtered_battery_voltage = std::round(filtered_battery_voltage * 100.0) / 100.0; + int battery_percentage = calcBatteryPercentage(filtered_battery_voltage); + setBatteryPercentage(0.05 * (battery_percentage) + (1 - 0.05) * getBatteryPercentage()); + + /// Calculate power consumption + float current_5v = i5_voltage / (CURRENT_SENSE_RESISTOR * USB_RATIO); + float current_motor = im_voltage / (CURRENT_SENSE_RESISTOR * IM_RATIO); + // Power consumption in the 5V rail (in W) + float power_consumption_5v = 5.25 * current_5v; + // Power consumption in the motor (in W) + float power_consumption_motor = battery_voltage * current_motor; + // Total power consumption (in W) + setPowerConsumption(power_consumption_5v + power_consumption_motor); + + // Delay before next reading + delay(1000); +} + +int BatteryModule::getBatteryPercentage() +{ + xSemaphoreTake(battery_mutex, portMAX_DELAY); + int percentage = filtered_battery_percentage; + xSemaphoreGive(battery_mutex); + + return percentage; +} + +float BatteryModule::getPowerConsumption() +{ + xSemaphoreTake(power_mutex, portMAX_DELAY); + float power = total_power_consumption; + xSemaphoreGive(power_mutex); + + return power; +} + +void BatteryModule::setBatteryPercentage(int percentage) +{ + xSemaphoreTake(battery_mutex, portMAX_DELAY); + filtered_battery_percentage = percentage; + xSemaphoreGive(battery_mutex); +} + +void BatteryModule::setPowerConsumption(float power) +{ + xSemaphoreTake(power_mutex, portMAX_DELAY); + total_power_consumption = power; + xSemaphoreGive(power_mutex); +} + +#endif diff --git a/esp32/lib/BatteryModule/BatteryModule.h b/esp32/lib/BatteryModule/BatteryModule.h new file mode 100644 index 0000000..ad3acf4 --- /dev/null +++ b/esp32/lib/BatteryModule/BatteryModule.h @@ -0,0 +1,64 @@ +#pragma once + +#ifndef NATIVE +#include +#include +#endif +#include + +// Define the pins connected to the PCB +// ADC2 (GPIO 25-27) is used for Wi-Fi, so we can't use it +#define VB_PIN 36 // Analog pin for battery voltage - D0 +#define I5_PIN 34 // Analog pin for 5V current sense - D4 +#define IM_PIN 33 // Analog pin for motor current sense - D5 + +/** + * BatteryModule is responsible for monitoring the battery level. + */ +class BatteryModule +{ +public: + BatteryModule() = delete; + + static int calcBatteryPercentage(float voltage); + static void setup(); + static void loop(); + + // Interface + static int getBatteryPercentage(); + static float getPowerConsumption(); + + static void setBatteryPercentage(int percentage); + static void setPowerConsumption(float power); + + // Public to allow testing + static std::map battery_data; + +private: + static float filtered_battery_voltage; + static int filtered_battery_percentage; + + // Variables to keep track of battery usage + static float total_power_consumption; // in mW + static unsigned long start_time; + + /// Constants for voltage dividers and calibration + + // Example ratio based on 100k and 20k resistors + static constexpr float VOLTAGE_DIVIDER_RATIO = 0.090909; + // Example value in ohms + static constexpr float CURRENT_SENSE_RESISTOR = 0.01; + // Maximum voltage for the ADC (Arduino Uno) + static constexpr float ADC_MAX_VOLTAGE = 3.3; + // 10-bit ADC resolution + static constexpr float ADC_RESOLUTION = 3880.0; + static constexpr float USB_RATIO = 330; + static constexpr float IM_RATIO = 200; + // Battery capacity in mAh + static constexpr float BATTERY_CAPACITY_MAH = 2000.0; + +#ifndef NATIVE + static SemaphoreHandle_t battery_mutex; + static SemaphoreHandle_t power_mutex; +#endif +}; diff --git a/esp32/lib/PidController/PidController.cpp b/esp32/lib/PidController/PidController.cpp index 9b9be73..e9f0b81 100644 --- a/esp32/lib/PidController/PidController.cpp +++ b/esp32/lib/PidController/PidController.cpp @@ -8,6 +8,16 @@ Step PidController::step1(STEPPER_INTERVAL_US, STEPPER1_STEP_PIN, STEPPER1_DIR_P Step PidController::step2(STEPPER_INTERVAL_US, STEPPER2_STEP_PIN, STEPPER2_DIR_PIN); SemaphoreHandle_t PidController::paramsMutex = xSemaphoreCreateMutex(); SemaphoreHandle_t PidController::directionMutex = xSemaphoreCreateMutex(); +SemaphoreHandle_t PidController::controlMutex = xSemaphoreCreateMutex(); + +double PidController::accXoffset = 0; +double PidController::accYoffset = 0; +double PidController::accZoffset = 0; +double PidController::gyroXoffset = 0; +double PidController::gyroYoffset = 0; +double PidController::angle_setpoint = 0; +double PidController::filtered_value = 0; +double PidController::rotation_correction = 0; // Initialise PID parameters using known values PidParams PidController::params(1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00); @@ -43,6 +53,8 @@ bool PidController::setup(IWifi &wifi, unsigned long timeout) return false; } + calibrate(); + wifi.println("Initialised Interrupt for Stepper"); // Set motor acceleration values @@ -58,68 +70,22 @@ bool PidController::setup(IWifi &wifi, unsigned long timeout) void PidController::loop() { - // Static variables are initialised once and then the value is remembered - // betweeen subsequent calls to this function - static unsigned long printTimer = 0; // time of the next print - static unsigned long loopTimer = 0; // time of the next control update - static double tiltx = 0.0; - static double gyrox = 0.0; // current tilt angle - static double theta_n = 0.0; - - // Fetch PID parameters - take mutex and wait forever until it is available - xSemaphoreTake(paramsMutex, portMAX_DELAY); - float kp_i = params.kp_i; - float ki_i = params.ki_i; - float kd_i = params.kd_i; - float tilt_setpoint = params.tilt_setpoint; - xSemaphoreGive(paramsMutex); - - double error; - double previous_error = 0; - double integral = 0; - double derivative = 0; - double previous_derivative = 0; - double Pout, Iout, Dout, motor_out; + // Time of the next control update + static unsigned long loop1Timer = 0; // inner + static unsigned long loop2Timer = 0; // outer + static unsigned long loop3Timer = 0; // rotation loop // Run the control loop every LOOP_INTERVAL ms - if (millis() > loopTimer) + if (millis() > loop1Timer) { - loopTimer += LOOP_INTERVAL; - - // Fetch data from MPU6050 - sensors_event_t a, g, temp; - mpu.getEvent(&a, &g, &temp); - - const float ACCELOREMETER_OFFSET = 0.03; - const float GYRO_OFFSET = -0.05; - const float COMP_FILTER_COEFF = 0.98; - - // Calculate Tilt using accelerometer and sin x = x approximation for - // a small tilt angle - tiltx = a.acceleration.z / 9.81 - ACCELOREMETER_OFFSET; - - gyrox = g.gyro.y - GYRO_OFFSET; - - theta_n = (1 - COMP_FILTER_COEFF) * (tiltx * 100) + COMP_FILTER_COEFF * ((gyrox * LOOP_INTERVAL) / 10 + theta_n); - - // PIDeez Nuts - error = tilt_setpoint - theta_n; - Pout = kp_i * error; - - integral = integral + error * (LOOP_INTERVAL / 1000); - Iout = ki_i * integral; - - derivative = ((error - previous_error) / LOOP_INTERVAL) * 1000; - derivative = previous_derivative * (derivative - previous_derivative); - previous_derivative = derivative; - - Dout = kd_i * derivative; - motor_out = Pout + Iout + Dout; - previous_error = error; + innerLoop(); + loop1Timer += LOOP_INTERVAL; + } - // Set target motor speed - step1.setTargetSpeedRad(motor_out); - step2.setTargetSpeedRad(-motor_out); + if (millis() > loop2Timer) + { + outerLoop(); + loop2Timer += LOOP2_INTERVAL; } } @@ -129,20 +95,8 @@ void PidController::stabilizedLoop() // Just get the robot moving // Maybe someone can be bothered to research the maths here - // Static variable to store the last direction - static PidDirection lastDirection = PidController::getDirection(); - - // Get the current direction PidDirection currentDirection = PidController::getDirection(); - - // Update last direction only if current direction is different - if (currentDirection.key_dir != lastDirection.key_dir) - { - lastDirection = currentDirection; - } - - float speed = lastDirection.speed; - KeyDirection key_dir = lastDirection.key_dir; + KeyDirection key_dir = currentDirection.key_dir; const float SPEED = 10; @@ -223,3 +177,166 @@ bool PidController::timerHandler(void *args) toggle = !toggle; return true; } + +void PidController::innerLoop() +{ + // Static variables are initialised once and then the value is remembered + // betweeen subsequent calls to this function + static double previous_angle_error = 0; + static double robot_angle = 0.0; + + const double COMP_FILTER = 0.98; // complementary filter coefficient + const double LOOP1_INTERVAL = 10; // inner loop + const double ALPHA = 0.05; // filter coefficient + const double MOTOR_SPEED = 17; + + // Fetch PID parameters - take mutex and wait forever until it is available + xSemaphoreTake(paramsMutex, portMAX_DELAY); + float kp_i = params.kp_i; + float ki_i = params.ki_i; + float kd_i = params.kd_i; + float tilt_setpoint = params.tilt_setpoint; + xSemaphoreGive(paramsMutex); + + // Fetch data from MPU6050 + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); + + double accX = a.acceleration.x - accXoffset; + double accY = a.acceleration.y - accYoffset; + double accZ = a.acceleration.z - accZoffset; + + // Calculate Tilt using accelerometer and sin x = x approximation for a + // small tilt angle - tiltx = (accZ) / (9.67); + double tiltx = (accZ) / sqrt(pow(accY, 2) + pow(accX, 2)); + + double gyro_angle = g.gyro.y - gyroYoffset; + + robot_angle = (1 - COMP_FILTER) * (tiltx) + COMP_FILTER; + robot_angle *= ((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); + 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; + + previous_angle_error = angle_error; + + if (motor_out >= 0) + { + step1.setTargetSpeedRad(MOTOR_SPEED); + step2.setTargetSpeedRad(-MOTOR_SPEED); + } + else + { + step1.setTargetSpeedRad(-MOTOR_SPEED); + step2.setTargetSpeedRad(MOTOR_SPEED); + } + + // uses absolute values + step1.setAccelerationRad(motor_out + rotation_correction); + step2.setAccelerationRad(motor_out - rotation_correction); + + double avgSpeed = (step1.getSpeedRad() - step2.getSpeedRad()) / 2; + filtered_value = ALPHA * avgSpeed + (1 - ALPHA) * filtered_value; +} + +void PidController::outerLoop() +{ + static double avgspeed = 0, avg_temp = 0, speed_error = 0, previous_speed_error = 0, integral_speed = 0, derivative_speed = 0, Pout_s = 0, Iout_s = 0, Dout_s = 0, ramp_rate = 0; + static double current_speed_setpoint = 0; + static double speed_setpoint = 0; // outer loop setpoint + static int speed_size = 0; + + double max_ramp_rate = 0.5; + double time_constant = 2000; + double beta = 1 - exp(-LOOP2_INTERVAL / time_constant); + + // Fetch PID parameters - take mutex and wait forever until it is available + xSemaphoreTake(paramsMutex, portMAX_DELAY); + float kp_o = params.kp_o; + float ki_o = params.ki_o; + float kd_o = params.kd_o; + float balancing_setpoint = params.velocity_setpoint; + + current_speed_setpoint += beta * (speed_setpoint - current_speed_setpoint); + + // filtered_value = (step1.getSpeedRad()-step2.getSpeedRad())/2; + speed_error = -(current_speed_setpoint - filtered_value); + + Pout_s = kp_o * speed_error; + + integral_speed = integral_speed + speed_error * (LOOP2_INTERVAL / 1000); + + if (ki_o == 0) + { + integral_speed = 0; + } + + Iout_s = ki_o * integral_speed; + + derivative_speed = ((speed_error - previous_speed_error) / LOOP2_INTERVAL) * 1000; + + Dout_s = kd_o * derivative_speed; + + angle_setpoint = Pout_s + Iout_s + Dout_s + balancing_setpoint; + + if (speed_setpoint > 0) + { + if (angle_setpoint > balancing_setpoint + 1) + { + angle_setpoint = balancing_setpoint + 1; + } + else if (angle_setpoint < balancing_setpoint - 5) + { + angle_setpoint = balancing_setpoint - 5; + } + } + + else if (speed_setpoint < 0) + { + if (angle_setpoint < balancing_setpoint - 1) + { + angle_setpoint = balancing_setpoint - 1; + } + else if (angle_setpoint > balancing_setpoint + 5) + { + angle_setpoint = balancing_setpoint + 5; + } + } + + previous_speed_error = speed_error; +} + +void PidController::rotationLoop() +{ +} + +void PidController::calibrate() +{ + sensors_event_t a, g, temp; + double accXoffset_sum = 0; + double accYoffset_sum = 0; + double accZoffset_sum = 0; + double gyroXoffset_sum = 0; + double gyroYoffset_sum = 0; + + for (int i = 0; i < 200; i++) + { + mpu.getEvent(&a, &g, &temp); + accXoffset_sum += a.acceleration.x; + accYoffset_sum += a.acceleration.y; + accZoffset_sum += a.acceleration.z; + gyroXoffset_sum += g.gyro.x; + gyroYoffset_sum += g.gyro.y; + } + + accXoffset = accXoffset_sum / 200 - 9.81; + accYoffset = accYoffset_sum / 200; + accZoffset = accZoffset_sum / 200; + gyroXoffset = gyroXoffset_sum / 200; + gyroYoffset = gyroYoffset_sum / 200; +} diff --git a/esp32/lib/PidController/PidController.h b/esp32/lib/PidController/PidController.h index dd503a7..e3eab7e 100644 --- a/esp32/lib/PidController/PidController.h +++ b/esp32/lib/PidController/PidController.h @@ -81,9 +81,34 @@ class PidController static PidDirection getDirection(); + // Public members + static SemaphoreHandle_t controlMutex; + private: static bool timerHandler(void *args); + /** + * Controls tilt. + */ + static void innerLoop(); + + /** + * Controls velocity. + */ + static void outerLoop(); + + /** + * Controls rotation. + */ + static void rotationLoop(); + + /** + * Calibrates the MPU6050. + * Outputs values into accXoffset etc. + */ + static void calibrate(); + + // Class members static IWifi *wifi; static MPU6050 mpu; static ESP32Timer ITimer; @@ -95,7 +120,18 @@ class PidController static SemaphoreHandle_t directionMutex; // Class constants - static constexpr double LOOP_INTERVAL = 5; static constexpr int STEPPER_INTERVAL_US = 10; static constexpr float MOTOR_ACCEL_RAD = 30.0; + static constexpr double LOOP_INTERVAL = 10; + static constexpr double LOOP2_INTERVAL = 200; + + // Class variables + static double accXoffset; + static double accYoffset; + static double accZoffset; + static double gyroXoffset; + static double gyroYoffset; + static double angle_setpoint; // inner loop setpoint + static double filtered_value; + static double rotation_correction; }; diff --git a/esp32/lib/WifiSetup/MqttSetup.cpp b/esp32/lib/WifiSetup/MqttSetup.cpp index b2c6141..9e1d7d9 100644 --- a/esp32/lib/WifiSetup/MqttSetup.cpp +++ b/esp32/lib/WifiSetup/MqttSetup.cpp @@ -1,4 +1,5 @@ #include "MqttSetup.h" +#include "BatteryModule.h" float x = 0; float y = 0; @@ -24,6 +25,7 @@ void MqttSetup::connect(unsigned long timeout) Serial.println("connected"); client.subscribe("user/joystick"); client.subscribe("user/pid"); + client.subscribe("esp32/cli"); } else { @@ -56,11 +58,7 @@ void MqttSetup::loop() lastBatteryMessageSent = millis(); // Update battery status and publish message - batteryLevel -= 7; - if (batteryLevel < 0) - { - batteryLevel = 100; - } + int batteryLevel = BatteryModule::getBatteryPercentage(); BatteryMessage batteryMessage(batteryLevel, getEpochTime()); publishMessage(batteryMessage); } @@ -71,11 +69,7 @@ void MqttSetup::loop() lastPowerMessageSent = millis(); // Update battery status and publish message - powerLevel -= 123; - if (powerLevel < 0) - { - powerLevel = 1000; - } + float powerLevel = BatteryModule::getPowerConsumption(); PowerMessage powerMessage(powerLevel, getEpochTime()); publishMessage(powerMessage); @@ -113,4 +107,4 @@ unsigned long MqttSetup::getEpochTime() timeClient.update(); unsigned long epochTime = timeClient.getEpochTime(); return epochTime; -} \ No newline at end of file +} diff --git a/esp32/lib/WifiSetup/MqttSetup.h b/esp32/lib/WifiSetup/MqttSetup.h index a4c803a..34477fb 100644 --- a/esp32/lib/WifiSetup/MqttSetup.h +++ b/esp32/lib/WifiSetup/MqttSetup.h @@ -43,11 +43,9 @@ class MqttSetup int lastBatteryMessageSent = 0; int delayBatterySent = 2000; - int batteryLevel = 100; int lastPowerMessageSent = 0; int delayPowerSent = 1000; - int powerLevel = 1000; char *client_id; }; diff --git a/esp32/lib/WifiSetup/WifiSetup.cpp b/esp32/lib/WifiSetup/WifiSetup.cpp index ff8eeb1..25462d6 100644 --- a/esp32/lib/WifiSetup/WifiSetup.cpp +++ b/esp32/lib/WifiSetup/WifiSetup.cpp @@ -59,6 +59,12 @@ void WifiSetup::connect(unsigned long timeout) start += d; } + if (WiFi.status() != WL_CONNECTED) + { + Serial.println("Failed to connect to WiFi"); + return; + } + Serial.println(""); Serial.println("WiFi connected"); Serial.println("IP address: "); @@ -183,7 +189,6 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length) if (doc["run_id"] == RunID) { // Check the topic and process accordingly - Serial.println("Processing message..."); if (strcmp(topic, "user/joystick") == 0) { // Extract values from the JSON document @@ -242,6 +247,22 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length) // Use the callback given in the static class PidController::setParams(PidParams(kp_i, ki_i, kd_i, tilt_setpoint, kp_o, ki_o, kd_o, velocity_setpoint, rotation_setpoint)); } + else if (strcmp(topic, "esp32/cli") == 0) + { + String message = doc["message"]; + if (message == "/auto" || message == "/a") + { + // Release the mutex so the Raspberry Pi can take control + xSemaphoreGive(PidController::controlMutex); + } + else if (message == "/manual" || message == "/m") + { + // Take the mutex so the Wifi can take control + xSemaphoreTake(PidController::controlMutex, (TickType_t)0); + } + + Serial.println("Received command from CLI: " + message); + } else { Serial.println("Unsupported topic"); @@ -284,4 +305,4 @@ void WifiSetup::resolveId() NTPClient &WifiSetup::getNTPClient() { return timeClient; -} \ No newline at end of file +} diff --git a/esp32/platformio.ini b/esp32/platformio.ini index f0a709f..be1b886 100644 --- a/esp32/platformio.ini +++ b/esp32/platformio.ini @@ -16,6 +16,7 @@ build_flags = -I include -D CONFIG_ARDUINO_LOOP_STACK_SIZE=8192 -Wno-deprecated-declarations + -Wunused-variable lib_deps = knolleary/PubSubClient @@ -32,7 +33,9 @@ lib_deps = monitor_filters = esp32_exception_decoder monitor_speed = 115200 upload_speed = 921600 -test_ignore = test_serial +test_ignore = + test_serial + test_battery [env:native] platform = native diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index d3051af..f2dd54f 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -1,12 +1,14 @@ // ESP32 Guide: https://RandomNerdTutorials.com/esp32-mpu-6050-accelerometer-gyroscope-arduino/ #include +#include "BatteryModule.h" #include "WifiSetup.h" +#include "BatteryModule.h" #include "SerialLoop.h" #include "PidController.h" -WifiSetup wifi(g_ssid, g_password, MQTT_SERVER, MQTT_PORT); -// WifiSetup wifi(i_ssid, i_email, i_password, MQTT_SERVER, MQTT_PORT); +// 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) @@ -17,6 +19,7 @@ int BotID, RunID; void pidLoop(void *pvParameters); void serialLoop(void *pvParameters); +void batteryLoop(void *pvParameters); void setup() { @@ -24,6 +27,7 @@ void setup() // Setup modules wifi.connect(timeout); + BatteryModule::setup(); if (PidController::setup(wifi, timeout)) { @@ -31,14 +35,14 @@ void setup() xTaskCreatePinnedToCore( pidLoop, /* Task function. */ "pidLoop", /* name of task. */ - 10000, /* Stack size of task = 40 KB */ + 15000, /* Stack size of task = 60 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 */ } - // Setup the serialLoop0 + // Setup the serialLoop xTaskCreatePinnedToCore( serialLoop, /* Task function. */ "serialLoop", /* name of task. */ @@ -47,6 +51,16 @@ void setup() 1, /* priority of the task */ NULL, /* Task handle to keep track of created task */ 1); /* pin task to core 1 */ + + // Setup the batteryLoop + // xTaskCreatePinnedToCore( + // batteryLoop, /* Task function. */ + // "batteryLoop", /* name of task. */ + // 15000, /* Stack size of task = 60 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() @@ -67,6 +81,14 @@ void serialLoop(void *pvParameters) { while (true) { - serialLoop(); + SerialLoop::loop(); + } +} + +void batteryLoop(void *pvParameters) +{ + while (true) + { + BatteryModule::loop(); } } diff --git a/esp32/test/test_battery/test_battery.cpp b/esp32/test/test_battery/test_battery.cpp new file mode 100644 index 0000000..586f54a --- /dev/null +++ b/esp32/test/test_battery/test_battery.cpp @@ -0,0 +1,34 @@ +#include "BatteryModule.h" +#include + +void setUp(void) +{ +} + +void tearDown(void) +{ +} + +void test_battery(void) +{ + // Over the table + TEST_ASSERT_EQUAL_INT(100, BatteryModule::calcBatteryPercentage(20)); + + // In the table + TEST_ASSERT_EQUAL_INT(100, BatteryModule::calcBatteryPercentage(16.19)); + TEST_ASSERT_EQUAL_INT(100, BatteryModule::calcBatteryPercentage(16.18)); + TEST_ASSERT_EQUAL_INT(4, BatteryModule::calcBatteryPercentage(13.8)); + TEST_ASSERT_EQUAL_INT(0, BatteryModule::calcBatteryPercentage(13.5)); + + // Under the table + TEST_ASSERT_EQUAL_INT(0, BatteryModule::calcBatteryPercentage(13)); +} + +int main() +{ + UNITY_BEGIN(); + RUN_TEST(test_battery); + UNITY_END(); + + return 0; +} diff --git a/esp32/test/test_mpu6050/test_mpu6050.cpp b/esp32/test/test_mpu6050/test_mpu6050.cpp index 65d8759..6090828 100644 --- a/esp32/test/test_mpu6050/test_mpu6050.cpp +++ b/esp32/test/test_mpu6050/test_mpu6050.cpp @@ -5,6 +5,8 @@ #include MPU6050 mpu; +// Declare variables for RunID and BotID +int BotID, RunID; void setUp(void) { diff --git a/esp32/test/test_wifi/test_wifi.cpp b/esp32/test/test_wifi/test_wifi.cpp index 4f2c5bf..4c33cb2 100644 --- a/esp32/test/test_wifi/test_wifi.cpp +++ b/esp32/test/test_wifi/test_wifi.cpp @@ -9,6 +9,9 @@ #include "WifiSetup.h" #include +// Declare variables for RunID and BotID +int BotID, RunID; + void setUp(void) { } @@ -66,14 +69,15 @@ void test_remote_joystick() // Use the callback char *topic = const_cast("user/joystick"); - char *payload = const_cast(R"({"speed": 0.5, "angle": 30})"); + char *payload = const_cast(R"({"run_id": 121, "direction": 'A'})"); wifi.callback(topic, reinterpret_cast(payload), strlen(payload)); - PidDirection expectedDirection = PidDirection(0.5, 30); + // Default speed = 100 + PidDirection expectedDirection = PidDirection(100, KeyDirection::RIGHT); PidDirection actualDirection = PidController::getDirection(); TEST_ASSERT_EQUAL(expectedDirection.speed, actualDirection.speed); - TEST_ASSERT_EQUAL(expectedDirection.angle, actualDirection.angle); + TEST_ASSERT_EQUAL(expectedDirection.key_dir, actualDirection.key_dir); } void test_remote_pid() @@ -85,20 +89,24 @@ void test_remote_pid() // Use the callback char *topic = const_cast("user/pid"); char *payload = const_cast( - R"({"kp_i": 0.5, "ki_i": 0.1, "kd_i": 0.2, "setpoint_i": 30, - "kp_o": 0.5, "ki_o": 0.1, "kd_o": 0.2, "setpoint_o": 30})"); + R"({"run_id": 121, "kp_i": 0.5, "ki_i": 0.1, "kd_i": 0.2, + "tilt_setpoint": 30, "kp_o": 0.5, "ki_o": 0.1, "kd_o": 0.2, + "velocity_setpoint": 30, "rotation_setpoint": 0.4"})"); wifi.callback(topic, reinterpret_cast(payload), strlen(payload)); - PidParams expectedParams = PidParams(0.5, 0.1, 0.2, 30, 0.4, 0.05, 0.1, 10); + PidParams expectedParams = PidParams( + 0.5, 0.1, 0.2, 30.0, 0.4, 0.05, 0.1, 10.0, 0.4); PidParams actualParams = PidController::getParams(); TEST_ASSERT_EQUAL(expectedParams.kp_i, actualParams.kp_i); TEST_ASSERT_EQUAL(expectedParams.ki_i, actualParams.ki_i); TEST_ASSERT_EQUAL(expectedParams.kd_i, actualParams.kd_i); - TEST_ASSERT_EQUAL(expectedParams.setpoint_i, actualParams.setpoint_i); + TEST_ASSERT_EQUAL(expectedParams.tilt_setpoint, actualParams.tilt_setpoint); TEST_ASSERT_EQUAL(expectedParams.kp_o, actualParams.kp_o); TEST_ASSERT_EQUAL(expectedParams.ki_o, actualParams.ki_o); TEST_ASSERT_EQUAL(expectedParams.kd_o, actualParams.kd_o); + TEST_ASSERT_EQUAL(expectedParams.velocity_setpoint, actualParams.velocity_setpoint); + TEST_ASSERT_EQUAL(expectedParams.rotation_setpoint, actualParams.rotation_setpoint); } void setup()