From fb35d065af7c8813a56da9bf66f9d6700a362778 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sat, 23 Nov 2024 22:12:27 +0100
Subject: [PATCH 01/59] Add IMU registers

---
 src/motionDetection/IMU_CMDs.h | 66 ++++++++++++++++++++++++++++++++--
 1 file changed, 64 insertions(+), 2 deletions(-)

diff --git a/src/motionDetection/IMU_CMDs.h b/src/motionDetection/IMU_CMDs.h
index ab420c7..25ac657 100644
--- a/src/motionDetection/IMU_CMDs.h
+++ b/src/motionDetection/IMU_CMDs.h
@@ -6,8 +6,70 @@
 #define ADDR_MASK      0x7F
 
 //Registers
-#define MCLK_RDY    0x00
-
+//User bank 0
+#define MCLK_RDY 0x00
+#define DEVICE_CONFIG 0x01
+#define SIGNAL_PATH_RESET 0x02
+#define DRIVE_CONFIG1 0x03
+#define DRIVE_CONFIG2 0x04
+#define DRIVE_CONFIG3 0x05
+#define INT_CONFIG 0x06
+#define TEMP_DATA1 0x09
+#define TEMP_DATA0 0x0A
+#define ACCEL_DATA_X1 0x0B
+#define ACCEL_DATA_X0 0x0C
+#define ACCEL_DATA_Y1 0x0D
+#define ACCEL_DATA_Y0 0x0E
+#define ACCEL_DATA_Z1 0x0F
+#define ACCEL_DATA_Z0 0x10
+#define GYRO_DATA_X1 0x11
+#define GYRO_DATA_X0 0x12
+#define GYRO_DATA_Y1 0x13
+#define GYRO_DATA_Y0 0x14
+#define GYRO_DATA_Z1 0x15
+#define GYRO_DATA_Z0 0x16
+#define TMST_FSYNCH 0x17
+#define TMST_FSYNCL 0x18
+#define APEX_DATA4 0x1D
+#define APEX_DATA5 0x1E
+#define PWR_MGMT0 0x1F
+#define GYRO_CONFIG0 0x20
+#define ACCEL_CONFIG0 0x21
+#define TEMP_CONFIG0 0x22
+#define GYRO_CONFIG1 0x23
+#define ACCEL_CONFIG1 0x24
+#define APEX_CONFIG0 0x25
+#define APEX_CONFIG1 0x26
+#define WOM_CONFIG 0x27
+#define FIFO_CONFIG1 0x28
+#define FIFO_CONFIG2 0x29
+#define FIFO_CONFIG3 0x2A
+#define INT_SOURCE0 0x2B
+#define INT_SOURCE1 0x2C
+#define INT_SOURCE3 0x2D
+#define INT_SOURCE4 0x2E
+#define FIFO_LOST_PKT0 0x2F
+#define FIFO_LOST_PKT1 0x30
+#define APEX_DATA0 0x31
+#define APEX_DATA1 0x32
+#define APEX_DATA2 0x33
+#define APEX_DATA3 0x34
+#define INTF_CONFIG0 0x35
+#define INTF_CONFIG1 0x36
+#define INT_STATUS_DRDY 0x39
+#define INT_STATUS 0x3A
+#define INT_STATUS2 0x3B
+#define INT_STATUS3 0x3C
+#define FIFO_COUNTH 0x3D
+#define FIFO_COUNTL 0x3E
+#define FIFO_DATA 0x3F
+#define WHO_AM_I 0x75
+#define BLK_SEL_W 0x79
+#define MADDR_W 0x7A
+#define M_W 0x7B
+#define BLK_SEL_R 0x7C
+#define MADDR_R 0x7D
+#define M_R 0x7E
 #define REG_TEMP_LOW   0x0A
 #define REG_TEMP_HIGH  0X09
 

From 8eeb829a9198c94036c744c7f15363dd93b55f83 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 24 Nov 2024 23:09:46 +0100
Subject: [PATCH 02/59] Add existing code: Power consumption test cases and
 Semaphore power scheduler skeleton

---
 .../Power_Measurements/Display/Display.ino    |  26 ++++
 .../ESP32_baseline/ESP32_baseline.ino         | 136 ++++++++++++++++++
 .../advanced/Power_Measurements/IMU/IMU.ino   |  43 ++++++
 .../Power_Measurements/IR_LED/IR_LED.ino      |  23 +++
 .../LightSensorsDaylight.ino                  |  34 +++++
 .../LightSensorsDaylightBottom.ino            |  34 +++++
 .../LightSensorsInfrared.ino                  |  34 +++++
 .../Power_Measurements/Motors/Motors.ino      |  22 +++
 .../Motors_Full/Motors_Full.ino               |  24 ++++
 .../Power_Measurements/RGB_LED/RGB_LED.ino    |  44 ++++++
 .../Power_Measurements/Template/Template.ino  |  32 +++++
 .../Power_Measurements/VEML6040/VEML6040.ino  |  34 +++++
 .../advanced/Power_Measurements/WLAN/WLAN.ino |  36 +++++
 src/power/Power.cpp                           |  40 ++++++
 src/power/Power.h                             |  35 +++++
 15 files changed, 597 insertions(+)
 create mode 100644 example/advanced/Power_Measurements/Display/Display.ino
 create mode 100644 example/advanced/Power_Measurements/ESP32_baseline/ESP32_baseline.ino
 create mode 100644 example/advanced/Power_Measurements/IMU/IMU.ino
 create mode 100644 example/advanced/Power_Measurements/IR_LED/IR_LED.ino
 create mode 100644 example/advanced/Power_Measurements/LightSensorsDaylight/LightSensorsDaylight.ino
 create mode 100644 example/advanced/Power_Measurements/LightSensorsDaylightBottom/LightSensorsDaylightBottom.ino
 create mode 100644 example/advanced/Power_Measurements/LightSensorsInfrared/LightSensorsInfrared.ino
 create mode 100644 example/advanced/Power_Measurements/Motors/Motors.ino
 create mode 100644 example/advanced/Power_Measurements/Motors_Full/Motors_Full.ino
 create mode 100644 example/advanced/Power_Measurements/RGB_LED/RGB_LED.ino
 create mode 100644 example/advanced/Power_Measurements/Template/Template.ino
 create mode 100644 example/advanced/Power_Measurements/VEML6040/VEML6040.ino
 create mode 100644 example/advanced/Power_Measurements/WLAN/WLAN.ino
 create mode 100644 src/power/Power.cpp
 create mode 100644 src/power/Power.h

diff --git a/example/advanced/Power_Measurements/Display/Display.ino b/example/advanced/Power_Measurements/Display/Display.ino
new file mode 100644
index 0000000..c455fc4
--- /dev/null
+++ b/example/advanced/Power_Measurements/Display/Display.ino
@@ -0,0 +1,26 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+
+// How many times to run a command on the display consecutively;
+const uint16_t iterations = 5000;
+
+void setup() {
+  dezibot.begin();
+}
+
+void loop() {
+  //Typical output
+  dezibot.display.println("Typical output");
+  for(uint16_t iter=0; iter<iterations; iter++) {
+    dezibot.display.println(iter);
+  }
+  delay(10);
+  //Lots of pixels turned on
+  dezibot.display.invertColor();
+  dezibot.display.println("Inverted output");
+  for(uint16_t iter=0; iter<iterations; iter++) {
+    dezibot.display.println(iter);
+  }
+  delay(10);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/ESP32_baseline/ESP32_baseline.ino b/example/advanced/Power_Measurements/ESP32_baseline/ESP32_baseline.ino
new file mode 100644
index 0000000..e587853
--- /dev/null
+++ b/example/advanced/Power_Measurements/ESP32_baseline/ESP32_baseline.ino
@@ -0,0 +1,136 @@
+#include "Dezibot.h"
+#include "esp_pm.h"
+#include "esp_task_wdt.h"
+
+Dezibot dezibot;
+
+const uint16_t cycleTime = 2e4;  //20000 ms = 20 s
+
+esp_pm_lock_handle_t cpuFreqLock;
+esp_pm_lock_handle_t apbFreqLock;
+esp_pm_lock_handle_t lightSleepLock;
+static bool pmLocksCreated = false;
+
+
+void stress_task0(void *pvParameters) {
+  // Register ourselves with the task watchdog
+  if (esp_task_wdt_add(NULL) != ESP_OK) {
+    Serial.println("Failed to add task 0 to TWDT");
+  }
+  while (1) {
+    volatile uint32_t x = 0;
+    for (uint32_t i = 0; i < 10000; i++) {
+      x += i;
+    }
+    esp_task_wdt_reset();
+  }
+}
+
+void stress_task1(void *pvParameters) {
+  // Register ourselves with the task watchdog
+  if (esp_task_wdt_add(NULL) != ESP_OK) {
+    Serial.println("Failed to add task 0 to TWDT");
+  }
+  while (1) {
+    volatile uint32_t x = 0;
+    for (uint32_t i = 0; i < 10000; i++) {
+      x += i;
+    }
+    esp_task_wdt_reset();
+  }
+}
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) {
+    ;
+  }
+  uint32_t Freq = getCpuFrequencyMhz();
+  Serial.print("CPU Freq = ");
+  Serial.print(Freq);
+  Serial.println(" MHz");
+  Freq = getXtalFrequencyMhz();
+  Serial.print("XTAL Freq = ");
+  Serial.print(Freq);
+  Serial.println(" MHz");
+  Freq = getApbFrequency();
+  Serial.print("APB Freq = ");
+  Serial.print(Freq);
+  Serial.println(" Hz");
+  // Create lock handles for controlling power management features
+  // Parameter arg is always unused and should thus be set to 0
+  // Ref: https://docs.espressif.com/projects/esp-idf/en/v3.3.6/api-reference/system/power_management.html#enumerations
+  if (!pmLocksCreated) {
+    bool cpuFreqLockSuccess = esp_pm_lock_create(ESP_PM_CPU_FREQ_MAX, 0, "CPU_FREQ_MAX", &cpuFreqLock);
+    bool apbFreqLockSuccess = esp_pm_lock_create(ESP_PM_APB_FREQ_MAX, 0, "APB_FREQ_MAX", &apbFreqLock);
+    bool lightSleepLockSuccess = esp_pm_lock_create(ESP_PM_NO_LIGHT_SLEEP, 0, "NO_LIGHT_SLEEP", &lightSleepLock);
+    if ((cpuFreqLockSuccess && apbFreqLockSuccess && lightSleepLockSuccess)) {
+      pmLocksCreated = true;
+      Serial.println("Sucessfully created handles to control power management features.");
+    } else {
+      Serial.println("Failed to create handles to control power management features!");
+    }
+  } else {
+    Serial.println("Powermanagement lock handles already created, skipping creation!");
+  }
+  // Configure Task Watchdog so it does not interfere
+  esp_task_wdt_config_t twdt_config = {
+    .timeout_ms = 30000,
+    .idle_core_mask = (1 << portNUM_PROCESSORS) - 1,
+    .trigger_panic = true
+  };
+  if (esp_task_wdt_reconfigure(&twdt_config) != ESP_OK) {
+    Serial.println("Failed to reconfigure task watchdog!");
+  }
+  delay(500);
+  Serial.flush();
+  Serial.end();
+}
+
+/*
+ * A function that prints to serial, setting up the serial peripheral beforehand, and shutting it down afterwards.
+ * Do not use this on a regular basis, as it is probably very slow. It is useful for not having the serial peripheral
+ * active at all times.
+ *
+ * @param str The text to print
+ */
+void setupAndCleanupSerialPrint(char *str) {
+  Serial.begin(115200);
+  while (!Serial) {
+    ;
+    ;
+  }
+  delay(10);
+  Serial.print(str);
+  delay(10);
+  Serial.flush();
+  delay(10);
+  Serial.end();
+}
+
+void loop() {
+  // Alternate between 20 Seconds at 100% CPU load, no artificial load but not sleeping, and normal behaviour (light sleep when there is nothing to do).
+  setupAndCleanupSerialPrint("Beginning stress phase\n");
+  TaskHandle_t core0StressTask = NULL;
+  TaskHandle_t core1StressTask = NULL;
+  xTaskCreatePinnedToCore(stress_task0, "CPU0Stress", 4096, NULL, 1, &core0StressTask, 0);
+  xTaskCreatePinnedToCore(stress_task1, "CPU1Stress", 4096, NULL, 1, &core1StressTask, 1);
+  vTaskDelay(pdMS_TO_TICKS(cycleTime));
+  setupAndCleanupSerialPrint("Still alive after waiting cycleTime after setting up stress tasks...\n");
+  esp_task_wdt_delete(core0StressTask);
+  esp_task_wdt_delete(core1StressTask);
+  vTaskDelete(core0StressTask);
+  vTaskDelete(core1StressTask);
+  // Now disable light sleep
+  setupAndCleanupSerialPrint("Beginning idle with power management disabled\n");
+  esp_pm_lock_acquire(cpuFreqLock);
+  esp_pm_lock_acquire(apbFreqLock);
+  esp_pm_lock_acquire(lightSleepLock);
+  vTaskDelay(pdMS_TO_TICKS(cycleTime));
+  // Restore auto light sleep and dynamic frequency scaling
+  setupAndCleanupSerialPrint("Beginning idle with power management reenabled\n");
+  esp_pm_lock_release(cpuFreqLock);
+  esp_pm_lock_release(apbFreqLock);
+  esp_pm_lock_release(lightSleepLock);
+  vTaskDelay(pdMS_TO_TICKS(cycleTime));
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/IMU/IMU.ino b/example/advanced/Power_Measurements/IMU/IMU.ino
new file mode 100644
index 0000000..b9a6925
--- /dev/null
+++ b/example/advanced/Power_Measurements/IMU/IMU.ino
@@ -0,0 +1,43 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+// How many times to run a command on the IMU consecutively;
+const uint16_t iterations = 5000;
+
+void setup() {
+  dezibot.motion.detection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  // Test if IMU is working correctly
+  char imu_whoami = dezibot.motion.detection.getWhoAmI();
+  if (imu_whoami == 0x67) {
+    Serial.println("IMU seems to be working. Starting measurements...");
+  } else {
+    Serial.println("IMU does not seemm to be working correctly.");
+    exit(0);
+  }
+  dezibot.motion.detection.end();
+  Serial.println("Returned IMU to low-power mode. Killing Serial peripheral. Goodbye!");
+  delay(1000);
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // This puts both accelerometer and gyroscope into low noise mode, which is their highest
+  // power consumption state
+  dezibot.motion.detection.begin();
+  for (uint16_t iter = 0; iter < iterations; iter++) {
+    dezibot.motion.detection.getRotation();
+    dezibot.motion.detection.getAcceleration();
+    delay(1);
+  }
+  // Turn everything back off to measure baseline power consumption
+  delay(iterations);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/IR_LED/IR_LED.ino b/example/advanced/Power_Measurements/IR_LED/IR_LED.ino
new file mode 100644
index 0000000..3bffc84
--- /dev/null
+++ b/example/advanced/Power_Measurements/IR_LED/IR_LED.ino
@@ -0,0 +1,23 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+const uint16_t cycleMillis = 2e4;
+
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.infraredLight.begin();
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  // Just turn the lights on alternating each five seconds, to get the consumption
+  dezibot.infraredLight.front.turnOn();
+  delay(cycleMillis);
+  dezibot.infraredLight.front.turnOff();
+  dezibot.infraredLight.bottom.turnOn();
+  delay(cycleMillis);
+  //Turn off and wait a little to measure baseline consumption
+  dezibot.infraredLight.bottom.turnOff();
+  delay(cycleMillis);
+}
diff --git a/example/advanced/Power_Measurements/LightSensorsDaylight/LightSensorsDaylight.ino b/example/advanced/Power_Measurements/LightSensorsDaylight/LightSensorsDaylight.ino
new file mode 100644
index 0000000..8070a3a
--- /dev/null
+++ b/example/advanced/Power_Measurements/LightSensorsDaylight/LightSensorsDaylight.ino
@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if SFH325 (Q3) is working correctly
+  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (light_value != UINT16_MAX) {
+    Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
+  } else {
+    Serial.printf("Light detection does not seem to be working correctly (detected value: %d).\n",light_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(DL_FRONT);
+  delay(10);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/LightSensorsDaylightBottom/LightSensorsDaylightBottom.ino b/example/advanced/Power_Measurements/LightSensorsDaylightBottom/LightSensorsDaylightBottom.ino
new file mode 100644
index 0000000..0c03f58
--- /dev/null
+++ b/example/advanced/Power_Measurements/LightSensorsDaylightBottom/LightSensorsDaylightBottom.ino
@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if SFH320 (Q12) is working correctly
+  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (light_value != UINT16_MAX) {
+    Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
+  } else {
+    Serial.printf("Light detection does not seem to be working correctly (detected value: %d).\n",light_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(DL_BOTTOM);
+  delay(10);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/LightSensorsInfrared/LightSensorsInfrared.ino b/example/advanced/Power_Measurements/LightSensorsInfrared/LightSensorsInfrared.ino
new file mode 100644
index 0000000..176856a
--- /dev/null
+++ b/example/advanced/Power_Measurements/LightSensorsInfrared/LightSensorsInfrared.ino
@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if VEML6040 is working correctly
+  char ir_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (ir_value != UINT16_MAX) {
+    Serial.printf("IR detection seems to be working (detected value: %d). Starting measurements...\r\n", ir_value);
+  } else {
+    Serial.printf("IR detection does not seem to be working correctly (detected value: %d).\n",ir_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(IR_FRONT);
+  delay(10);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/Motors/Motors.ino b/example/advanced/Power_Measurements/Motors/Motors.ino
new file mode 100644
index 0000000..b4f446b
--- /dev/null
+++ b/example/advanced/Power_Measurements/Motors/Motors.ino
@@ -0,0 +1,22 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.motion.begin();
+}
+
+void loop() {
+  // Alternatingly turn on the Motors. Use the commands provided by motion for this, using the provided
+  // functions (with their duty cycle), as it represents typical usage
+  dezibot.motion.rotateClockwise();
+  delay(20000);
+  dezibot.motion.rotateAntiClockwise();
+  delay(20000);
+  // Turn on both motors at the same time
+  dezibot.motion.left.setSpeed(DEFAULT_BASE_VALUE);
+  dezibot.motion.right.setSpeed(DEFAULT_BASE_VALUE);  
+  delay(20000);
+  dezibot.motion.stop();
+  delay(20000);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/Motors_Full/Motors_Full.ino b/example/advanced/Power_Measurements/Motors_Full/Motors_Full.ino
new file mode 100644
index 0000000..97b37d7
--- /dev/null
+++ b/example/advanced/Power_Measurements/Motors_Full/Motors_Full.ino
@@ -0,0 +1,24 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+const uint duty = 8192; //Full tilt
+
+void setup() {
+  dezibot.motion.begin();
+}
+
+void loop() {
+  // Alternatingly turn on the Motors. Use the commands provided by motion for this, using the provided
+  // functions (with their duty cycle), as it represents typical usage
+  dezibot.motion.rotateClockwise(0,duty);
+  delay(20000);
+  dezibot.motion.rotateAntiClockwise(0,duty);
+  delay(20000);
+  // Turn on both motors at the same time
+  dezibot.motion.left.setSpeed(duty);
+  dezibot.motion.right.setSpeed(duty);  
+  delay(20000);
+  dezibot.motion.stop();
+  delay(20000);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/RGB_LED/RGB_LED.ino b/example/advanced/Power_Measurements/RGB_LED/RGB_LED.ino
new file mode 100644
index 0000000..6014e5b
--- /dev/null
+++ b/example/advanced/Power_Measurements/RGB_LED/RGB_LED.ino
@@ -0,0 +1,44 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+
+void setup() {
+  dezibot.multiColorLight.begin();
+}
+
+void loop() {
+  /* Loop through the LEDs at full brightness for measurements, with 20 seconds to measure for each state */
+  for (int state = 0; state < 6; state++) {
+    switch (state) {
+      case 0:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        break;
+      case 1:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(TOP_LEFT,WHITE);
+        break;
+      case 2:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(TOP_RIGHT,WHITE);
+        break;
+      case 3:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(BOTTOM,WHITE);
+        break;
+      case 4:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(TOP,WHITE);
+        break;
+      case 5:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(ALL,WHITE);
+        break;
+    }
+    sleep(20);
+  }
+}
diff --git a/example/advanced/Power_Measurements/Template/Template.ino b/example/advanced/Power_Measurements/Template/Template.ino
new file mode 100644
index 0000000..c65c37d
--- /dev/null
+++ b/example/advanced/Power_Measurements/Template/Template.ino
@@ -0,0 +1,32 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if Part under Test is working correctly
+  if (result = working) {
+    Serial.printf("Part X seems to be working (detected value: %d). Starting measurements...\r\n", result);
+  } else {
+    Serial.printf("Part X does not seem to be working correctly (detected value: %d).\n",result);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  delay(10);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/VEML6040/VEML6040.ino b/example/advanced/Power_Measurements/VEML6040/VEML6040.ino
new file mode 100644
index 0000000..6c821be
--- /dev/null
+++ b/example/advanced/Power_Measurements/VEML6040/VEML6040.ino
@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if VEML6040 is working correctly
+  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (light_value != UINT16_MAX) {
+    Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
+  } else {
+    Serial.printf("Light detection does not seem to be working correctly (detected value: %d).\n",light_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(DL_FRONT);
+  delay(10);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Measurements/WLAN/WLAN.ino b/example/advanced/Power_Measurements/WLAN/WLAN.ino
new file mode 100644
index 0000000..ece11b9
--- /dev/null
+++ b/example/advanced/Power_Measurements/WLAN/WLAN.ino
@@ -0,0 +1,36 @@
+#include "Dezibot.h"
+#define DEBUG
+
+Dezibot dezibot = Dezibot();
+
+void setup() {
+  Serial.begin(112500);
+  while (!Serial) {
+    ; /* Wait for USB-CDC Serial init to complete. */
+  }
+  #ifdef DEBUG
+  dezibot.display.begin();
+  dezibot.display.println("Debug enabled.");
+  Serial.println("Debug enabled.");
+  #endif
+  dezibot.communication.begin();
+  dezibot.communication.setGroupNumber(1);
+  dezibot.communication.sendMessage("Repeated send power consumption test commencing");
+  #ifdef DEBUG
+  dezibot.display.println("Mesh set up");
+  /* Set up receive handler */
+  dezibot.communication.onReceive(handle_receive);
+  dezibot.display.println("Set up receive. Printing incoming messages:");
+  Serial.println("Sending broadcast messages to generate TX power consumption:");
+  #endif
+}
+
+void handle_receive(String &message) {
+  dezibot.display.println(message);
+}
+
+void loop() {
+    /* Continuously send to consume power on TX */
+    dezibot.communication.sendMessage("Power Test Message");
+
+}
\ No newline at end of file
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
new file mode 100644
index 0000000..a22b6bf
--- /dev/null
+++ b/src/power/Power.cpp
@@ -0,0 +1,40 @@
+/**
+ * @file Power.h
+ * @author Phillip Kühne
+ * @brief This component provides utilities for keeping track of power usage
+ * consumption.
+ * @version 0.1
+ * @date 2024-11-23 
+ */
+
+#include "Power.h"
+
+Power::Power()
+{
+    this->free_power_budget = this->total_power_budget;
+}
+
+bool Power::tryAccquirePowerAllowance(uint16_t neededPower)
+{
+    if (this->free_power_budget >= neededPower)
+    {
+        this->free_power_budget -= neededPower;
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void Power::releasePower(uint16_t power)
+{
+    if (this->free_power_budget + power <= this->total_power_budget)
+    {
+        this->free_power_budget += power;
+    }
+    else // TODO: Maybe we should actually throw an error here, since obviouslsy someone used us wrong.
+    {
+        this->free_power_budget = this->total_power_budget;
+    }
+}
diff --git a/src/power/Power.h b/src/power/Power.h
new file mode 100644
index 0000000..b4b64f9
--- /dev/null
+++ b/src/power/Power.h
@@ -0,0 +1,35 @@
+/**
+ * @file Power.h
+ * @author Phillip Kühne
+ * @brief This component provides utilities for keeping track of power usage
+ * consumption.
+ * @version 0.1
+ * @date 2024-11-23 
+ */
+
+#include <Arduino.h>
+
+#ifndef Power_h
+#define Power_h
+
+#define TOTAL_POWER_MILLIWATTS  700
+
+class Power {
+    protected:
+    static const uint16_t total_power_budget = TOTAL_POWER_MILLIWATTS;
+    uint16_t free_power_budget;
+
+    public:
+    Power();
+    uint16_t getFreePowerBudget(void);
+    /// @brief Request an allowance of a certain number of milliwatts from the power scheduler
+    /// @param neededPower the amount of power we want to be accounted for (in mW)
+    /// @return whether the power could be successfully allocated
+    bool tryAccquirePowerAllowance(uint16_t neededPower);
+    /// @brief "Return" a certain amount of power when it is no longer needed
+    /// @param power the amount of power to return (in mW)
+    /// @return whether the power
+    void releasePower(uint16_t power);
+};
+
+#endif //Power
\ No newline at end of file

From bb338d31be6bd5d71db175c6243401b71725b41d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 28 Nov 2024 20:41:54 +0100
Subject: [PATCH 03/59] Phase shift right motor PWM signal by 180 degrees

---
 src/motion/Motion.h  | 14 ++++++++++----
 src/motion/Motor.cpp |  5 +++--
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index 2a7ed8a..84e395b 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -22,11 +22,12 @@
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
 #define CHANNEL_RIGHT      LEDC_CHANNEL_4  
 #define DUTY_RES           LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
+#define PHASE_180_DEG      (1 << (DUTY_RES))/2 // (2**DUTY_RES)/2 Set phase to 180 degrees 
 #define FREQUENCY          (5000) // Frequency in Hertz. Set frequency at 5 kHz
 #define DEFAULT_BASE_VALUE  3900
 class Motor{
     public:
-        Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
+        Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase=0);
         
         /**
          * @brief Initializes the motor 
@@ -52,14 +53,19 @@ class Motor{
         uint8_t pin;
         ledc_timer_t timer;
         ledc_channel_t channel;
-       
+
         uint16_t duty;
+        // The phase of the pwm signal, expressed as a number betweeen 0 and
+        // the maximum value representable by the pwm timer resolution.
+        int phase;
 };
 
 class Motion{
 protected:
     static inline uint16_t RIGHT_MOTOR_DUTY = DEFAULT_BASE_VALUE;
     static inline uint16_t LEFT_MOTOR_DUTY = DEFAULT_BASE_VALUE;
+    static inline int LEFT_MOTOR_PHASE = 0;
+    static inline int RIGHT_MOTOR_PHASE = PHASE_180_DEG;
     static const int MOTOR_RIGHT_PIN = 11; 
     static const int MOTOR_LEFT_PIN = 12;
     static void moveTask(void * args);
@@ -75,8 +81,8 @@ protected:
 
 public:
     //Instances of the motors, so they can also be used from outside to set values for the motors directly.
-    static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT);
-    static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT);
+    static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT,LEFT_MOTOR_PHASE);
+    static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT,RIGHT_MOTOR_PHASE);
     
     //MotionDetection instance, for motion Correction and user (access with dezibot.motion.detection)
     static inline MotionDetection detection;
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 0c26a58..bc084ad 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -1,10 +1,11 @@
 #include "Motion.h"
 
-Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
+Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase){
     this->pin = pin;
     this->channel = channel;
     this->timer = timer;
     this->duty = 0;
+    this->phase = phase;
 };
 
 void Motor::begin(void){
@@ -16,7 +17,7 @@ void Motor::begin(void){
         .intr_type      = LEDC_INTR_DISABLE,
         .timer_sel      = this->timer,
         .duty           = 0, // Set duty to 0%
-        .hpoint         = 0
+        .hpoint         = this->phase
     };
     ledc_channel_config(&channelConfig);
     Serial.println("Motor begin done");

From 51a3d9e8f6a5cc7c15db1a2fa9b54a7685b04178 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 12 Dec 2024 17:09:13 +0100
Subject: [PATCH 04/59] Power management progress

---
 .../advanced/Power_Measurements/WLAN/WLAN.ino | 10 +-
 src/Dezibot.cpp                               | 18 ++--
 src/Dezibot.h                                 |  2 +
 src/motion/Motion.h                           |  3 +-
 src/motion/Motor.cpp                          |  9 +-
 src/power/Consumptions.h                      | 34 +++++++
 src/power/Power.cpp                           | 96 +++++++++++++++++--
 src/power/Power.h                             | 35 ++++++-
 8 files changed, 184 insertions(+), 23 deletions(-)
 create mode 100644 src/power/Consumptions.h

diff --git a/example/advanced/Power_Measurements/WLAN/WLAN.ino b/example/advanced/Power_Measurements/WLAN/WLAN.ino
index ece11b9..26b10a4 100644
--- a/example/advanced/Power_Measurements/WLAN/WLAN.ino
+++ b/example/advanced/Power_Measurements/WLAN/WLAN.ino
@@ -3,12 +3,14 @@
 
 Dezibot dezibot = Dezibot();
 
+const uint16_t cycleTime = 5e3;  //5000 ms = 5 s
+
 void setup() {
+  #ifdef DEBUG
   Serial.begin(112500);
   while (!Serial) {
     ; /* Wait for USB-CDC Serial init to complete. */
   }
-  #ifdef DEBUG
   dezibot.display.begin();
   dezibot.display.println("Debug enabled.");
   Serial.println("Debug enabled.");
@@ -31,6 +33,10 @@ void handle_receive(String &message) {
 
 void loop() {
     /* Continuously send to consume power on TX */
-    dezibot.communication.sendMessage("Power Test Message");
+    for(int i=0; i<cycleTime; i++) {
+      dezibot.communication.sendMessage("Power Test Message");
+      delay(1);
+    }
+    delay(cycleTime);
 
 }
\ No newline at end of file
diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp
index 5add5aa..7008351 100644
--- a/src/Dezibot.cpp
+++ b/src/Dezibot.cpp
@@ -5,18 +5,22 @@
 #include "Dezibot.h"
 #include <Wire.h>
 
+Dezibot::Dezibot() : multiColorLight() {};
 
-Dezibot::Dezibot():multiColorLight(){};
-
-void Dezibot::begin(void) {
-    Wire.begin(SDA_PIN,SCL_PIN);
+void Dezibot::begin(void)
+{
+    Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
-    lightDetection.begin();    
+    lightDetection.begin();
     motion.begin();
     lightDetection.begin();
     colorDetection.begin();
     multiColorLight.begin();
     display.begin();
+    Power.begin();
+    this->power = Power::getPowerManager();
+    if (!this->power.tryAccquirePowerAllowance(CONSUMPTION_ESP_BASE);)
+    {
+        throw "Could not allocate power for the base consumption of the ESP32";
+    }
 };
-
-
diff --git a/src/Dezibot.h b/src/Dezibot.h
index 8dcacf5..4d4274c 100644
--- a/src/Dezibot.h
+++ b/src/Dezibot.h
@@ -19,6 +19,7 @@
 #include "infraredLight/InfraredLight.h"
 #include "communication/Communication.h"
 #include "display/Display.h"
+#include "power/Power.h"
 
 
 class Dezibot {
@@ -33,6 +34,7 @@ public:
     InfraredLight infraredLight;
     Communication communication;
     Display display;
+    Power power;
     void begin(void);
 };
 
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index 2a7ed8a..69776d7 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -17,6 +17,7 @@
 #include <freertos/task.h>
 #include "driver/ledc.h"
 #include "motionDetection/MotionDetection.h"
+#include "power/Power.h"
 #define LEDC_MODE          LEDC_LOW_SPEED_MODE
 #define TIMER              LEDC_TIMER_2
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
@@ -52,7 +53,7 @@ class Motor{
         uint8_t pin;
         ledc_timer_t timer;
         ledc_channel_t channel;
-       
+        Power powerManager;
         uint16_t duty;
 };
 
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 0c26a58..da0b7c2 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -5,6 +5,7 @@ Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
     this->channel = channel;
     this->timer = timer;
     this->duty = 0;
+    this->powerManager = *Power::getPowerManager();
 };
 
 void Motor::begin(void){
@@ -23,7 +24,13 @@ void Motor::begin(void){
 };
 
 void Motor::setSpeed(uint16_t duty){
-    
+    if(duty>0) {
+        powerManager.waitForPowerAllowance(CONSUMPTION_MOTOR, portMAX_DELAY);
+        Serial.println("Motor got power");
+    } else {
+        powerManager.releasePower(CONSUMPTION_MOTOR);
+        Serial.println("Motor released power");
+    }
     int difference = duty-this->getSpeed();
     if (difference > 0){
         for(int i = 0;i<difference;i+=difference/20){
diff --git a/src/power/Consumptions.h b/src/power/Consumptions.h
new file mode 100644
index 0000000..0344110
--- /dev/null
+++ b/src/power/Consumptions.h
@@ -0,0 +1,34 @@
+/**
+ * @file Consumptions.h
+ * @author Phillip Kühne
+ * @brief 
+ * @version 0.1
+ * @date 2024-11-28
+ * 
+ * @copyright (c) 2024
+ * 
+ */
+
+#ifndef Consumptions_h
+#define Consumptions_h
+
+// The power budget afforded by the LIR2450 button cell, 700mW
+#define POWER_BUDGET 700 
+
+// The power consumptions of the different components are defined here.
+// These are "typical worst case" values for now.
+#define CONSUMPTION_ESP_BASE 96
+#define CONSUMPTION_ESP_LOAD 100
+#define CONSUMPTION_RGB_LED 56
+#define CONSUMPTION_IR_BOTTOM 327
+#define CONSUMPTION_IR_FRONT 492
+#define CONSUMPTION_OLED 92 // Assumes lots of lit pixels
+#define CONSUMPTION_MOTOR 323
+// These are placeholders for completeness for now
+#define CONSUMPTION_RGBW_SENSOR 1
+#define CONSUMPTION_IR_PT 1
+#define CONSUMPTION_UV_LED 200
+#define CONSUMPTION_IMU 1
+
+
+#endif //Consumptions_h
\ No newline at end of file
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index a22b6bf..dfd9bd7 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -4,21 +4,46 @@
  * @brief This component provides utilities for keeping track of power usage
  * consumption.
  * @version 0.1
- * @date 2024-11-23 
+ * @date 2024-11-23
  */
 
 #include "Power.h"
+static portMUX_TYPE mux;
 
 Power::Power()
 {
-    this->free_power_budget = this->total_power_budget;
+    this->freePowerBudget = this->totalPowerBudget;
+}
+
+void Power::begin()
+{
+    if (powerInstance == nullptr)
+    {
+        // Double check locking https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
+        taskENTER_CRITICAL(&mux);
+        if (powerInstance == nullptr)
+        {
+            powerInstance = new Power();
+        }
+        taskEXIT_CRITICAL(&mux);
+    }
+    else
+    {
+        // Power.begin() was called twice!
+        Serial.println("Power.begin() was called twice! No harm done, but this is a bug.");
+    }
+}
+
+Power *Power::getPowerManager()
+{
+    return powerInstance;
 }
 
 bool Power::tryAccquirePowerAllowance(uint16_t neededPower)
 {
-    if (this->free_power_budget >= neededPower)
+    if (this->freePowerBudget >= neededPower)
     {
-        this->free_power_budget -= neededPower;
+        this->freePowerBudget -= neededPower;
         return true;
     }
     else
@@ -29,12 +54,69 @@ bool Power::tryAccquirePowerAllowance(uint16_t neededPower)
 
 void Power::releasePower(uint16_t power)
 {
-    if (this->free_power_budget + power <= this->total_power_budget)
+    if (this->freePowerBudget + power <= this->totalPowerBudget)
     {
-        this->free_power_budget += power;
+        this->freePowerBudget += power;
     }
     else // TODO: Maybe we should actually throw an error here, since obviouslsy someone used us wrong.
     {
-        this->free_power_budget = this->total_power_budget;
+        this->freePowerBudget = this->totalPowerBudget;
+    }
+    // Check if there are tasks waiting for power
+    checkWaitingTasks();
+}
+
+bool Power::waitForPowerAllowance(uint16_t neededPower, TickType_t ticksToWait)
+{
+    if (tryAccquirePowerAllowance(neededPower))
+    {
+        return true;
+    }
+    else
+    {
+        // Suspend the task while waiting for power to be available
+        TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
+        TickType_t initialTickCount = xTaskGetTickCount();
+        waitingTasks.push(currentTask);
+        uint32_t notificationValue;
+        BaseType_t notificationStatus = xTaskNotifyWait(0, 0, &notificationValue, ticksToWait);
+        // Code below will be executed after the task is woken up
+        while (notificationStatus == pdPASS)
+        {
+            if (notificationValue == POWER_AVAILABLE)
+            {
+                // We were woken up because new power is available, check if it is enough
+                if (tryAccquirePowerAllowance(neededPower))
+                {
+                    return true;
+                }
+                else
+                {
+                    // Still not enough power available for us. Wait the remaining ticks.
+                    xTaskNotifyWait(0, 0, &notificationValue, ticksToWait - (xTaskGetTickCount() - initialTickCount));
+                }
+            }
+        }
+        if (notificationStatus == pdFALSE)
+        {
+            // We waited long enough...
+            return false;
+        }
+        else
+        {
+            // Should be impossible to reach
+            throw "Reached impossible state";
+        }
+    }
+}
+
+void Power::checkWaitingTasks(void)
+{
+    // Check if there are tasks waiting for power
+    if (!waitingTasks.empty())
+    {
+        TaskHandle_t task = waitingTasks.front();
+        waitingTasks.pop();
+        xTaskNotify(task, POWER_AVAILABLE, eSetValueWithOverwrite);
     }
 }
diff --git a/src/power/Power.h b/src/power/Power.h
index b4b64f9..5451a38 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -7,29 +7,54 @@
  * @date 2024-11-23 
  */
 
+#include <queue>
 #include <Arduino.h>
+#include "Consumptions.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
 
 #ifndef Power_h
 #define Power_h
 
-#define TOTAL_POWER_MILLIWATTS  700
+#define TOTAL_POWER_MILLIWATTS  POWER_BUDGET
+
+enum TaskResumptionReason {
+    POWER_AVAILABLE,
+    TIMEOUT
+};
 
 class Power {
     protected:
-    static const uint16_t total_power_budget = TOTAL_POWER_MILLIWATTS;
-    uint16_t free_power_budget;
+    static const uint16_t totalPowerBudget = TOTAL_POWER_MILLIWATTS;
+    uint16_t freePowerBudget;
+    std::queue<TaskHandle_t> waitingTasks;
+    void checkWaitingTasks(void);
+    bool takePowerIfAvailable(uint16_t neededPower);
+    static Power* powerInstance;
+    Power();
 
     public:
-    Power();
+    static void begin();
+    /// @brief Initialize the singleton instance of the power manager
+    /// @return reference to the power manager
+    static Power *getPowerManager();
     uint16_t getFreePowerBudget(void);
     /// @brief Request an allowance of a certain number of milliwatts from the power scheduler
     /// @param neededPower the amount of power we want to be accounted for (in mW)
     /// @return whether the power could be successfully allocated
     bool tryAccquirePowerAllowance(uint16_t neededPower);
     /// @brief "Return" a certain amount of power when it is no longer needed
-    /// @param power the amount of power to return (in mW)
+    /// @param neededPower the amount of power to return (in mW)
     /// @return whether the power
     void releasePower(uint16_t power);
+
+    /// @brief Wait for a certain amount of power to be available
+    /// @param neededPower the amount of power we want to be accounted for (in mW)
+    /// @param TicksToWait the amount of time to wait for the power to become available
+    /// @return whether the power could be successfully allocatedy
+    bool waitForPowerAllowance(uint16_t neededPower,TickType_t TicksToWait);
+    /// @brief Put the ESP32 into deep sleep mode, without a method to wake up again. Basically this is a shutdown.
+    void beginPermanentDeepSleep(void);
 };
 
 #endif //Power
\ No newline at end of file

From 2792aef45d4414393fe0a58bac6dbb79a0ee142f Mon Sep 17 00:00:00 2001
From: pkuehne <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 18 Dec 2024 11:58:54 +0000
Subject: [PATCH 05/59] Add remarks about IMU behaviour.

---
 src/motionDetection/MotionDetection.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 6386e2f..a06618e 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -21,9 +21,11 @@ void MotionDetection::begin(void){
     this->writeRegister(0x23,0x37);
     //Enable Gyro and Acceldata in FIFO
     this->initFIFO();
+    // TODO: Accelerometer Startup Time From sleep mode to valid data 10 
 };
 void MotionDetection::end(void){
     this->writeRegister(PWR_MGMT0,0x00);
+    // TODO: After powering the gyroscope off, a period of > 20ms should be allowed to elapse before it is powered back on.
 };
 IMUResult MotionDetection::getAcceleration(){
     IMUResult result;

From 915ad85526a55e9bcd3e317bb6744e05fed60291 Mon Sep 17 00:00:00 2001
From: pkuehne <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 18 Dec 2024 12:12:04 +0000
Subject: [PATCH 06/59] Remark about architecture.

---
 src/power/Power.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index dfd9bd7..0a47b16 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -12,6 +12,7 @@ static portMUX_TYPE mux;
 
 Power::Power()
 {
+    // TODO: Create wrappper around all this which handles single-instancing so we look like a normal arduino library from the outside.
     this->freePowerBudget = this->totalPowerBudget;
 }
 

From fbe205035e4075b8baead2c036799301bb0a87d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 22 Dec 2024 22:07:54 +0100
Subject: [PATCH 07/59] Added wrapper for PowerScheduler

---
 src/Dezibot.cpp              |   7 +--
 src/motion/Motion.h          |   2 +-
 src/motion/Motor.cpp         |   6 +-
 src/power/Power.cpp          | 113 +++++++----------------------------
 src/power/Power.h            |  38 +++++-------
 src/power/PowerScheduler.cpp | 109 +++++++++++++++++++++++++++++++++
 src/power/PowerScheduler.h   |  71 ++++++++++++++++++++++
 7 files changed, 219 insertions(+), 127 deletions(-)
 create mode 100644 src/power/PowerScheduler.cpp
 create mode 100644 src/power/PowerScheduler.h

diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp
index 7008351..6c9e9c4 100644
--- a/src/Dezibot.cpp
+++ b/src/Dezibot.cpp
@@ -9,6 +9,7 @@ Dezibot::Dezibot() : multiColorLight() {};
 
 void Dezibot::begin(void)
 {
+    Power.begin();
     Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
     lightDetection.begin();
@@ -17,10 +18,4 @@ void Dezibot::begin(void)
     colorDetection.begin();
     multiColorLight.begin();
     display.begin();
-    Power.begin();
-    this->power = Power::getPowerManager();
-    if (!this->power.tryAccquirePowerAllowance(CONSUMPTION_ESP_BASE);)
-    {
-        throw "Could not allocate power for the base consumption of the ESP32";
-    }
 };
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index 69776d7..daf70c0 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -53,7 +53,7 @@ class Motor{
         uint8_t pin;
         ledc_timer_t timer;
         ledc_channel_t channel;
-        Power powerManager;
+        Power* powerManager;
         uint16_t duty;
 };
 
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index da0b7c2..7c629b2 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -5,7 +5,7 @@ Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
     this->channel = channel;
     this->timer = timer;
     this->duty = 0;
-    this->powerManager = *Power::getPowerManager();
+    this->powerManager = 
 };
 
 void Motor::begin(void){
@@ -25,10 +25,10 @@ void Motor::begin(void){
 
 void Motor::setSpeed(uint16_t duty){
     if(duty>0) {
-        powerManager.waitForPowerAllowance(CONSUMPTION_MOTOR, portMAX_DELAY);
+        powerManager->waitForPowerAllowance(CONSUMPTION_MOTOR, portMAX_DELAY);
         Serial.println("Motor got power");
     } else {
-        powerManager.releasePower(CONSUMPTION_MOTOR);
+        powerManager->releasePower(CONSUMPTION_MOTOR);
         Serial.println("Motor released power");
     }
     int difference = duty-this->getSpeed();
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 0a47b16..29a44ff 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -10,114 +10,41 @@
 #include "Power.h"
 static portMUX_TYPE mux;
 
-Power::Power()
-{
-    // TODO: Create wrappper around all this which handles single-instancing so we look like a normal arduino library from the outside.
-    this->freePowerBudget = this->totalPowerBudget;
-}
-
 void Power::begin()
 {
-    if (powerInstance == nullptr)
+    // Check if another instance of us already initialized the power scheduler,
+    // if not, we will do it.
+    if (powerScheduler == nullptr)
     {
-        // Double check locking https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
-        taskENTER_CRITICAL(&mux);
-        if (powerInstance == nullptr)
+        powerScheduler = PowerScheduler::getPowerScheduler();
+        if (!(powerScheduler->tryAccquirePowerAllowance(CONSUMPTION_ESP_BASE)))
         {
-            powerInstance = new Power();
+            Serial.println("Alledgedly not enough power available to reserve the ESP32s base power consumption. Something is wrong.");
+            return;
         }
-        taskEXIT_CRITICAL(&mux);
-    }
-    else
-    {
-        // Power.begin() was called twice!
-        Serial.println("Power.begin() was called twice! No harm done, but this is a bug.");
     }
 }
 
-Power *Power::getPowerManager()
+uint16_t Power::getFreePowerBudget(void)
 {
-    return powerInstance;
+    return powerScheduler->getFreePowerBudget();
 }
 
 bool Power::tryAccquirePowerAllowance(uint16_t neededPower)
 {
-    if (this->freePowerBudget >= neededPower)
-    {
-        this->freePowerBudget -= neededPower;
-        return true;
-    }
-    else
-    {
-        return false;
-    }
+    return powerScheduler->tryAccquirePowerAllowance(neededPower);
+}
+
+bool Power::waitForPowerAllowance(uint16_t neededPower, TickType_t TicksToWait)
+{
+    return powerScheduler->waitForPowerAllowance(neededPower, TicksToWait);
+}
+
+void Power::beginPermanentDeepSleep(void)
+{
+    return powerScheduler->beginPermanentDeepSleep();
 }
 
 void Power::releasePower(uint16_t power)
 {
-    if (this->freePowerBudget + power <= this->totalPowerBudget)
-    {
-        this->freePowerBudget += power;
-    }
-    else // TODO: Maybe we should actually throw an error here, since obviouslsy someone used us wrong.
-    {
-        this->freePowerBudget = this->totalPowerBudget;
-    }
-    // Check if there are tasks waiting for power
-    checkWaitingTasks();
-}
-
-bool Power::waitForPowerAllowance(uint16_t neededPower, TickType_t ticksToWait)
-{
-    if (tryAccquirePowerAllowance(neededPower))
-    {
-        return true;
-    }
-    else
-    {
-        // Suspend the task while waiting for power to be available
-        TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
-        TickType_t initialTickCount = xTaskGetTickCount();
-        waitingTasks.push(currentTask);
-        uint32_t notificationValue;
-        BaseType_t notificationStatus = xTaskNotifyWait(0, 0, &notificationValue, ticksToWait);
-        // Code below will be executed after the task is woken up
-        while (notificationStatus == pdPASS)
-        {
-            if (notificationValue == POWER_AVAILABLE)
-            {
-                // We were woken up because new power is available, check if it is enough
-                if (tryAccquirePowerAllowance(neededPower))
-                {
-                    return true;
-                }
-                else
-                {
-                    // Still not enough power available for us. Wait the remaining ticks.
-                    xTaskNotifyWait(0, 0, &notificationValue, ticksToWait - (xTaskGetTickCount() - initialTickCount));
-                }
-            }
-        }
-        if (notificationStatus == pdFALSE)
-        {
-            // We waited long enough...
-            return false;
-        }
-        else
-        {
-            // Should be impossible to reach
-            throw "Reached impossible state";
-        }
-    }
-}
-
-void Power::checkWaitingTasks(void)
-{
-    // Check if there are tasks waiting for power
-    if (!waitingTasks.empty())
-    {
-        TaskHandle_t task = waitingTasks.front();
-        waitingTasks.pop();
-        xTaskNotify(task, POWER_AVAILABLE, eSetValueWithOverwrite);
-    }
 }
diff --git a/src/power/Power.h b/src/power/Power.h
index 5451a38..6e885c3 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -4,40 +4,27 @@
  * @brief This component provides utilities for keeping track of power usage
  * consumption.
  * @version 0.1
- * @date 2024-11-23 
+ * @date 2024-11-23
  */
 
-#include <queue>
-#include <Arduino.h>
-#include "Consumptions.h"
-#include "freertos/FreeRTOS.h"
-#include "freertos/task.h"
-
+#include "PowerScheduler.h"
 #ifndef Power_h
 #define Power_h
 
-#define TOTAL_POWER_MILLIWATTS  POWER_BUDGET
+#define TOTAL_POWER_MILLIWATTS POWER_BUDGET
 
-enum TaskResumptionReason {
+enum TaskResumptionReason
+{
     POWER_AVAILABLE,
     TIMEOUT
 };
 
-class Power {
-    protected:
-    static const uint16_t totalPowerBudget = TOTAL_POWER_MILLIWATTS;
-    uint16_t freePowerBudget;
-    std::queue<TaskHandle_t> waitingTasks;
-    void checkWaitingTasks(void);
-    bool takePowerIfAvailable(uint16_t neededPower);
-    static Power* powerInstance;
-    Power();
+class Power
+{
 
-    public:
+public:
     static void begin();
-    /// @brief Initialize the singleton instance of the power manager
-    /// @return reference to the power manager
-    static Power *getPowerManager();
+    Power();
     uint16_t getFreePowerBudget(void);
     /// @brief Request an allowance of a certain number of milliwatts from the power scheduler
     /// @param neededPower the amount of power we want to be accounted for (in mW)
@@ -52,9 +39,12 @@ class Power {
     /// @param neededPower the amount of power we want to be accounted for (in mW)
     /// @param TicksToWait the amount of time to wait for the power to become available
     /// @return whether the power could be successfully allocatedy
-    bool waitForPowerAllowance(uint16_t neededPower,TickType_t TicksToWait);
+    bool waitForPowerAllowance(uint16_t neededPower, TickType_t TicksToWait);
     /// @brief Put the ESP32 into deep sleep mode, without a method to wake up again. Basically this is a shutdown.
     void beginPermanentDeepSleep(void);
+
+protected:
+    static PowerScheduler *powerScheduler;
 };
 
-#endif //Power
\ No newline at end of file
+#endif // Power
\ No newline at end of file
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
new file mode 100644
index 0000000..b365bc4
--- /dev/null
+++ b/src/power/PowerScheduler.cpp
@@ -0,0 +1,109 @@
+/**
+ * @file PowerScheduler.cpp
+ * @author Phillip Kühne
+ * @brief The actual power scheduler class, which keeps track of the power budget and allocates power to different components.
+ * @version 0.1
+ * @date 2024-12-21
+ *
+ * @copyright (c) 2024
+ *
+ */
+
+#include "PowerScheduler.h"
+
+bool PowerScheduler::tryAccquirePowerAllowance(uint16_t neededPower)
+{
+    if (this->freePowerBudget >= neededPower)
+    {
+        this->freePowerBudget -= neededPower;
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void PowerScheduler::releasePower(uint16_t power)
+{
+    if (this->freePowerBudget + power <= this->totalPowerBudget)
+    {
+        this->freePowerBudget += power;
+    }
+    else // TODO: Maybe we should actually throw an error here, since obviouslsy someone used us wrong.
+    {
+        this->freePowerBudget = this->totalPowerBudget;
+    }
+    // Check if there are tasks waiting for power
+    checkWaitingTasks();
+}
+
+bool PowerScheduler::waitForPowerAllowance(uint16_t neededPower, TickType_t ticksToWait)
+{
+    if (tryAccquirePowerAllowance(neededPower))
+    {
+        return true;
+    }
+    else
+    {
+        // Suspend the task while waiting for power to be available
+        TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
+        TickType_t initialTickCount = xTaskGetTickCount();
+        waitingTasks.push(currentTask);
+        uint32_t notificationValue;
+        BaseType_t notificationStatus = xTaskNotifyWait(0, 0, &notificationValue, ticksToWait);
+        // Code below will be executed after the task is woken up
+        while (notificationStatus == pdPASS)
+        {
+            if (notificationValue == POWER_AVAILABLE)
+            {
+                // We were woken up because new power is available, check if it is enough
+                if (tryAccquirePowerAllowance(neededPower))
+                {
+                    return true;
+                }
+                else
+                {
+                    // Still not enough power available for us. Wait the remaining ticks.
+                    xTaskNotifyWait(0, 0, &notificationValue, ticksToWait - (xTaskGetTickCount() - initialTickCount));
+                }
+            }
+        }
+        if (notificationStatus == pdFALSE)
+        {
+            // We waited long enough...
+            return false;
+        }
+        else
+        {
+            // Should be impossible to reach
+            throw "Reached impossible state";
+        }
+    }
+}
+
+void PowerScheduler::checkWaitingTasks(void)
+{
+    // Check if there are tasks waiting for power
+    if (!waitingTasks.empty())
+    {
+        TaskHandle_t task = waitingTasks.front();
+        waitingTasks.pop();
+        xTaskNotify(task, POWER_AVAILABLE, eSetValueWithOverwrite);
+    }
+}
+
+PowerScheduler *PowerScheduler::getPowerScheduler()
+{
+    if (powerSchedulerInstance == nullptr)
+    {
+        // Double check locking https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
+        taskENTER_CRITICAL(&mux);
+        if (powerSchedulerInstance == nullptr)
+        {
+            powerSchedulerInstance = new PowerScheduler();
+        }
+        taskEXIT_CRITICAL(&mux);
+    }
+    return powerSchedulerInstance;
+}
\ No newline at end of file
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
new file mode 100644
index 0000000..236430e
--- /dev/null
+++ b/src/power/PowerScheduler.h
@@ -0,0 +1,71 @@
+/**
+ * @file PowerScheduler.hpp
+ * @author Phillip Kühne
+ * @brief The actual power scheduler class, which keeps track of the power budget and allocates power to different components.
+ * @version 0.1
+ * @date 2024-12-21
+ *
+ * @copyright (c) 2024
+ *
+ */
+
+#include <queue>
+#include <Arduino.h>
+#include "Consumptions.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+#ifndef PowerScheduler_h
+#define PowerScheduler_h
+
+#define TOTAL_POWER_MILLIWATTS POWER_BUDGET
+
+class PowerScheduler
+{
+private:
+    /* data */
+public:
+    PowerScheduler();
+    ~PowerScheduler();
+    /// @brief Initialize the singleton instance of the power manager
+    /// @return reference to the power manager
+    static PowerScheduler *getPowerScheduler();
+    uint16_t getFreePowerBudget(void);
+    /// @brief Request an allowance of a certain number of milliwatts from the power scheduler
+    /// @param neededPower the amount of power we want to be accounted for (in mW)
+    /// @return whether the power could be successfully allocated
+    bool tryAccquirePowerAllowance(uint16_t neededPower);
+    /// @brief "Return" a certain amount of power when it is no longer needed
+    /// @param neededPower the amount of power to return (in mW)
+    /// @return whether the power
+    void releasePower(uint16_t power);
+
+    /// @brief Wait for a certain amount of power to be available
+    /// @param neededPower the amount of power we want to be accounted for (in mW)
+    /// @param TicksToWait the amount of time to wait for the power to become available
+    /// @return whether the power could be successfully allocatedy
+    bool waitForPowerAllowance(uint16_t neededPower, TickType_t TicksToWait);
+    /// @brief Put the ESP32 into deep sleep mode, without a method to wake up again. Basically this is a shutdown.
+    void beginPermanentDeepSleep(void);
+
+protected:
+    static const uint16_t totalPowerBudget = TOTAL_POWER_MILLIWATTS;
+    uint16_t freePowerBudget;
+    std::queue<TaskHandle_t> waitingTasks;
+    void checkWaitingTasks(void);
+    bool takePowerIfAvailable(uint16_t neededPower);
+};
+
+PowerScheduler::PowerScheduler(/* args */)
+{
+    // TODO: Create wrappper around all this which handles single-instancing so we look like a normal arduino library from the outside.
+    this->freePowerBudget = TOTAL_POWER_MILLIWATTS;
+}
+
+PowerScheduler::~PowerScheduler()
+{
+}
+
+static PowerScheduler *powerSchedulerInstance;
+
+#endif // PowerScheduler_h
\ No newline at end of file

From 0fb888031e727f9940ddcb5b962736c11863918f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 10 Jan 2025 23:08:07 +0100
Subject: [PATCH 08/59] Add ignores for further dev-environment specific
 changes

---
 .gitignore | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/.gitignore b/.gitignore
index 0eb0b45..adefc18 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,3 +39,8 @@ docs/*
 .history/
 *.vsix
 *.workspace
+
+# Build artifacts and cache
+build/
+.cache/
+compile_commands.json
\ No newline at end of file

From e747b9d10b64074cb307272955b9ec3469406b50 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Tue, 11 Feb 2025 20:10:43 +0100
Subject: [PATCH 09/59] Add data gathered from measurement results

---
 src/power/Consumptions.h    |  34 -----------
 src/power/PowerParameters.h | 109 ++++++++++++++++++++++++++++++++++++
 2 files changed, 109 insertions(+), 34 deletions(-)
 delete mode 100644 src/power/Consumptions.h
 create mode 100644 src/power/PowerParameters.h

diff --git a/src/power/Consumptions.h b/src/power/Consumptions.h
deleted file mode 100644
index 0344110..0000000
--- a/src/power/Consumptions.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/**
- * @file Consumptions.h
- * @author Phillip Kühne
- * @brief 
- * @version 0.1
- * @date 2024-11-28
- * 
- * @copyright (c) 2024
- * 
- */
-
-#ifndef Consumptions_h
-#define Consumptions_h
-
-// The power budget afforded by the LIR2450 button cell, 700mW
-#define POWER_BUDGET 700 
-
-// The power consumptions of the different components are defined here.
-// These are "typical worst case" values for now.
-#define CONSUMPTION_ESP_BASE 96
-#define CONSUMPTION_ESP_LOAD 100
-#define CONSUMPTION_RGB_LED 56
-#define CONSUMPTION_IR_BOTTOM 327
-#define CONSUMPTION_IR_FRONT 492
-#define CONSUMPTION_OLED 92 // Assumes lots of lit pixels
-#define CONSUMPTION_MOTOR 323
-// These are placeholders for completeness for now
-#define CONSUMPTION_RGBW_SENSOR 1
-#define CONSUMPTION_IR_PT 1
-#define CONSUMPTION_UV_LED 200
-#define CONSUMPTION_IMU 1
-
-
-#endif //Consumptions_h
\ No newline at end of file
diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
new file mode 100644
index 0000000..a3f6066
--- /dev/null
+++ b/src/power/PowerParameters.h
@@ -0,0 +1,109 @@
+/**
+ * @file PowerParameters.h
+ * @author Phillip Kühne
+ * @brief
+ * @version 0.1
+ * @date 2024-11-28
+ *
+ * @copyright (c) 2024
+ *
+ */
+
+#ifndef PowerParameters_h
+#define PowerParameters_h
+
+namespace PowerParameters {
+
+  struct Battery {
+    // Datasheet values
+    static constexpr float CELL_CAPACITY_MAH = 120;
+    static constexpr float CELL_VOLTAGE_NOMINAL = 3.7;
+    struct DISCHARGE_CURVE {
+      static constexpr float REFERENCE_CURRENT_A = 0.05;
+      static constexpr int NUM_POINTS = 22;
+      static constexpr float VOLTAGES[NUM_POINTS] = {
+          4.15, 4.05,  3.97, 3.9,   3.84, 3.795, 3.765, 3.74, 3.72, 3.7,  3.69,
+          3.68, 3.675, 3.67, 3.665, 3.66, 3.64,  3.61,  3.55, 3.45, 3.25, 3.0};
+      static constexpr int CHARGE_STATES[NUM_POINTS] = {
+          100, 95, 90, 85, 80, 75, 70, 65, 60, 55, 50,
+          45,  40, 35, 30, 25, 20, 15, 10, 5,  0,  -5};
+    };
+
+    // Derived values
+    static constexpr float CELL_CHARGE_FULL_COLOUMB = CELL_CAPACITY_MAH * 3.6;
+    static constexpr float CELL_ENERGY_FULL_JOULES =
+        CELL_CAPACITY_MAH * CELL_VOLTAGE_NOMINAL * 3.6;
+    static constexpr float CELL_CURRENT_1C = CELL_CAPACITY_MAH;
+    static constexpr float CELL_CURRENT_2C = CELL_CAPACITY_MAH * 2;
+    struct BAT_ADC {
+      static constexpr float VOLTAGE_DIVIDER_R12 = 27e3;
+      static constexpr float VOLTAGE_DIVIDER_R13 = 10e3;
+      static constexpr float VOLTAGE_DIVIDER_FACTOR =
+          (VOLTAGE_DIVIDER_R12 + VOLTAGE_DIVIDER_R13) / VOLTAGE_DIVIDER_R13;
+    };
+  };
+
+  // Factors concerning Buck-Boost-Converter
+  static constexpr float BUCK_BOOST_EFFICIENCY = 0.9;
+
+  /*
+   * The current consumptions in milliamperes of the different components are
+   * defined here. These values are measured on 3,3 Volts, and need to be
+   * converted to currents actually occuring at the battery.
+   */
+  struct CurrentConsumptions {
+    static constexpr float CURRENT_ESP_BASE = 37.42;
+    static constexpr float CURRENT_ESP_LOAD = 88.43;
+    static constexpr float CURRENT_ESP_AVG =
+        (CURRENT_ESP_BASE + CURRENT_ESP_LOAD) / 2;
+    // RGB LED quiescent current
+    static constexpr float CURRENT_LED_RGB_BASE = 0.7;
+    // RGB LED per channel current during PWM on-time.
+    static constexpr float CURRENT_LED_RGB_CHAN_T_ON = 16;
+    static constexpr float CURRENT_SENSOR_RGBW = 0.2;
+    static constexpr float CURRENT_LED_IR_BOTTOM = 100;
+    static constexpr float CURRENT_LED_IR_FRONT = 180.7;
+    // Phototransistor current when active and illuminated.
+    static constexpr float CURRENT_PT = 0.33005;
+    // Average value, as internal behaviour can not non-expensively and
+    // accurately be observed from code.
+    static constexpr float CURRENT_DISPLAY = 9;
+    // Per motor current during PWM on-time.
+    static constexpr float CURRENT_MOTOR_T_ON = 130;
+    // Current of IMU when activated.
+    static constexpr float CURRENT_IMU = 0.55;
+    // LED Current. Placeholder.
+    static constexpr float CURRENT_UV_LED = 200;
+  };
+
+  struct PinConfig {
+    static constexpr int BAT_ADC = 10;
+    static constexpr int BAT_ADC_EN = 9;
+    static constexpr int VUSB_SENS = 38;
+    static constexpr int BAT_CHG_STAT = 39;
+  };
+
+  enum PowerConsumers {
+    ESP,
+    WIFI,
+    LED_RGB_TOP_LEFT,
+    LED_RGB_TOP_RIGHT,
+    LED_RGB_BOTTOM,
+    RGBW_SENSOR,
+    LED_IR_BOTTOM,
+    LED_IR_FRONT,
+    PT_IR_LEFT,
+    PT_IR_RIGHT,
+    PT_IR_FRONT,
+    PT_IR_BACK,
+    PT_DL_FRONT,
+    PT_DL_BOTTOM,
+    LED_UV,
+    DISPLAY,
+    MOTOR_LEFT,
+    MOTOR_RIGHT,
+    IMU
+  };
+};
+
+#endif // Consumptions_h
\ No newline at end of file

From 893234ed24ac35afe6b6d32cdf4c5e541b4dba6a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Tue, 11 Feb 2025 23:33:00 +0100
Subject: [PATCH 10/59] FIxes

---
 src/Dezibot.cpp      | 2 +-
 src/motion/Motor.cpp | 9 +--------
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp
index 6c9e9c4..2282813 100644
--- a/src/Dezibot.cpp
+++ b/src/Dezibot.cpp
@@ -9,7 +9,7 @@ Dezibot::Dezibot() : multiColorLight() {};
 
 void Dezibot::begin(void)
 {
-    Power.begin();
+    power.begin();
     Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
     lightDetection.begin();
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 7c629b2..d6c68e3 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -1,11 +1,11 @@
 #include "Motion.h"
+#include "power/Power.h"
 
 Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
     this->pin = pin;
     this->channel = channel;
     this->timer = timer;
     this->duty = 0;
-    this->powerManager = 
 };
 
 void Motor::begin(void){
@@ -24,13 +24,6 @@ void Motor::begin(void){
 };
 
 void Motor::setSpeed(uint16_t duty){
-    if(duty>0) {
-        powerManager->waitForPowerAllowance(CONSUMPTION_MOTOR, portMAX_DELAY);
-        Serial.println("Motor got power");
-    } else {
-        powerManager->releasePower(CONSUMPTION_MOTOR);
-        Serial.println("Motor released power");
-    }
     int difference = duty-this->getSpeed();
     if (difference > 0){
         for(int i = 0;i<difference;i+=difference/20){

From b44538b4735685d7ea2fa2132b451fe61db1d78e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Tue, 11 Feb 2025 23:33:19 +0100
Subject: [PATCH 11/59] Add Power Scheduler

---
 src/power/Power.cpp          | 117 ++++++++++++----
 src/power/Power.h            |  91 +++++++++----
 src/power/PowerParameters.h  |   2 +-
 src/power/PowerScheduler.cpp | 254 +++++++++++++++++++++++------------
 src/power/PowerScheduler.h   | 141 +++++++++++++------
 5 files changed, 425 insertions(+), 180 deletions(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 29a44ff..b67f9ff 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -10,41 +10,106 @@
 #include "Power.h"
 static portMUX_TYPE mux;
 
-void Power::begin()
-{
-    // Check if another instance of us already initialized the power scheduler,
-    // if not, we will do it.
-    if (powerScheduler == nullptr)
-    {
-        powerScheduler = PowerScheduler::getPowerScheduler();
-        if (!(powerScheduler->tryAccquirePowerAllowance(CONSUMPTION_ESP_BASE)))
-        {
-            Serial.println("Alledgedly not enough power available to reserve the ESP32s base power consumption. Something is wrong.");
-            return;
-        }
+void Power::begin() {
+  // Check if another instance of us already initialized the power scheduler,
+  // if not, we will do it.
+  if (powerScheduler == nullptr) {
+    powerScheduler = &PowerScheduler::getPowerScheduler();
+
+    if (!(powerScheduler->tryAccquireCurrentAllowance(
+            PowerParameters::PowerConsumers::ESP,
+            PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
+      Serial.println("Alledgedly not enough power available to reserve the "
+                     "ESP32s base power consumption. Something is wrong.");
+      return;
     }
+  }
 }
 
-uint16_t Power::getFreePowerBudget(void)
-{
-    return powerScheduler->getFreePowerBudget();
+uint16_t Power::getFreeCurrentBudget(void) {
+  return powerScheduler->getFreeCurrentBudget();
 }
 
-bool Power::tryAccquirePowerAllowance(uint16_t neededPower)
-{
-    return powerScheduler->tryAccquirePowerAllowance(neededPower);
+bool Power::tryAccquireCurrentAllowance(
+    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
+    uint16_t requestedDurationMs) {
+  return powerScheduler->tryAccquireCurrentAllowance(consumer, neededCurrent,
+                                                     requestedDurationMs);
 }
 
-bool Power::waitForPowerAllowance(uint16_t neededPower, TickType_t TicksToWait)
-{
-    return powerScheduler->waitForPowerAllowance(neededPower, TicksToWait);
+bool Power::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                                    uint16_t neededCurrent,
+                                    TickType_t TicksToWait,
+                                    uint16_t requestedDurationMs) {
+  return powerScheduler->waitForCurrentAllowance(
+      consumer, neededCurrent, TicksToWait, requestedDurationMs);
 }
 
-void Power::beginPermanentDeepSleep(void)
-{
-    return powerScheduler->beginPermanentDeepSleep();
+void Power::beginPermanentDeepSleep(void) {
+  return powerScheduler->beginPermanentDeepSleep();
 }
 
-void Power::releasePower(uint16_t power)
-{
+void Power::releaseCurrent(PowerParameters::PowerConsumers consumer) {
+  powerScheduler->releaseCurrent(consumer);
 }
+
+float Power::getBatteryVoltage() {
+  // Get the battery voltage from the ADC and convert it to a voltage
+  // using the voltage divider.
+  portENTER_CRITICAL(&mux);
+  // Enable voltage divider
+  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH);
+  // Returns value between 0 and 4095 mapping to between 0 and 3.3V
+  uint16_t batteryAdcValue =
+      analogRead(PowerParameters::PinConfig::BAT_ADC) *
+      (PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR);
+  // Disable voltage divider
+  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW);
+  portEXIT_CRITICAL(&mux);
+  // Convert ADC value to voltage
+  float batteryVoltage =
+      (batteryAdcValue * 3.3 / 4095) *
+      PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR;
+  return batteryVoltage;
+}
+
+int Power::getBatteryChargePercent() {
+  // Get the battery voltage and calculate the charge state based on the
+  // discharge curve.
+  float batteryVoltage = getBatteryVoltage();
+  float chargeState = 0;
+  // Clamp edge cases
+  if (batteryVoltage >=
+      PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0]) {
+    return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[0];
+  }
+  if (batteryVoltage <=
+      PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES
+          [PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1]) {
+    return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES
+        [PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1];
+  }
+  float p1_x, p1_y, p2_x, p2_y;
+  for (int i = 0; i < PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS;
+       i++) {
+    if (batteryVoltage >=
+        PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i]) {
+      p1_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i];
+      p1_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i];
+      p2_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i + 1];
+      p2_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i + 1];
+      chargeState =
+          ((p2_y - p1_y) / (p2_x - p1_x)) * (batteryVoltage - p1_x) + p1_y;
+      return chargeState;
+    }
+  }
+}
+
+PowerScheduler* Power::powerScheduler = nullptr;
+
+Power::Power() {
+  // Initialize the power scheduler
+  powerScheduler = &PowerScheduler::getPowerScheduler();
+  // Initialize the mutex
+  mux = portMUX_INITIALIZER_UNLOCKED;
+}
\ No newline at end of file
diff --git a/src/power/Power.h b/src/power/Power.h
index 6e885c3..a748965 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -13,38 +13,77 @@
 
 #define TOTAL_POWER_MILLIWATTS POWER_BUDGET
 
-enum TaskResumptionReason
-{
-    POWER_AVAILABLE,
-    TIMEOUT
-};
+enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
-class Power
-{
+class Power {
 
 public:
-    static void begin();
-    Power();
-    uint16_t getFreePowerBudget(void);
-    /// @brief Request an allowance of a certain number of milliwatts from the power scheduler
-    /// @param neededPower the amount of power we want to be accounted for (in mW)
-    /// @return whether the power could be successfully allocated
-    bool tryAccquirePowerAllowance(uint16_t neededPower);
-    /// @brief "Return" a certain amount of power when it is no longer needed
-    /// @param neededPower the amount of power to return (in mW)
-    /// @return whether the power
-    void releasePower(uint16_t power);
+  static void begin(void);
+  Power();
+  uint16_t getFreeCurrentBudget(void);
+  /// @brief Request an allowance of a certain number of milliwatts from the
+  /// power scheduler
+  /// @param neededPower the amount of power we want to be accounted for (in mW)
+  /// @return whether the power could be successfully allocated
+  static bool
+  tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                              uint16_t neededCurrent,
+                              uint16_t requestedDurationMs);
+  /// @brief "Return" the power consumed by an active consumer
+  /// @param neededPower the amount of power to return (in mW)
+  /// @return whether the power
+  static void releaseCurrent(PowerParameters::PowerConsumers consumer);
 
-    /// @brief Wait for a certain amount of power to be available
-    /// @param neededPower the amount of power we want to be accounted for (in mW)
-    /// @param TicksToWait the amount of time to wait for the power to become available
-    /// @return whether the power could be successfully allocatedy
-    bool waitForPowerAllowance(uint16_t neededPower, TickType_t TicksToWait);
-    /// @brief Put the ESP32 into deep sleep mode, without a method to wake up again. Basically this is a shutdown.
-    void beginPermanentDeepSleep(void);
+  /// @brief Wait for a certain amount of power to be available
+  /// @param neededPower the amount of power we want to be accounted for (in mW)
+  /// @param TicksToWait the amount of time to wait for the power to become
+  /// available
+  /// @return whether the power could be successfully allocatedy
+  static bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                                      uint16_t neededCurrent,
+                                      TickType_t TicksToWait,
+                                      uint16_t requestedDurationMs);
+  /// @brief Put the ESP32 into deep sleep mode, without a method to wake up
+  /// again. Basically this is a shutdown.
+  static void beginPermanentDeepSleep(void);
+
+  /// @brief Get battery voltage measurement.
+  /// @return Battery Terminal Voltage in Volts
+  static float getBatteryVoltage();
+
+  /// @brief Get estimated battery charge state as percentage
+  /// @return Battery charge state in percent
+  static int getBatteryChargePercent();
+
+  /// @brief Get estimated battery charge state as coulombs
+  /// @return Battery charge state in coulombs
+  static float getBatteryChargeCoulombs();
+
+  /// @brief get available current (after voltage conversion and efficiency
+  /// losses, referencing 1C discharge)
+  /// @return available current in milliamps
+  static float getAvailableCurrent();
 
 protected:
-    static PowerScheduler *powerScheduler;
+  /// @brief PowerScheduler instance to manage power consumption
+  static PowerScheduler *powerScheduler;
+
+  /// @brief update Power State
+  static void updatePowerState();
+
+  /*
+   * Power State
+   */
+
+  /// @brief remaining Charge in coulombs
+  static int coloumbsRemaining;
+
+  /// @brief remaining Charge in percent
+  static int percentRemaining;
+
+  friend class PowerScheduler;
 };
 
+extern Power power;
+
 #endif // Power
\ No newline at end of file
diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index a3f6066..98a7a68 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -99,7 +99,7 @@ namespace PowerParameters {
     PT_DL_FRONT,
     PT_DL_BOTTOM,
     LED_UV,
-    DISPLAY,
+    DISPLAY_OLED,
     MOTOR_LEFT,
     MOTOR_RIGHT,
     IMU
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index b365bc4..4bfec42 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -1,7 +1,8 @@
 /**
  * @file PowerScheduler.cpp
  * @author Phillip Kühne
- * @brief The actual power scheduler class, which keeps track of the power budget and allocates power to different components.
+ * @brief The actual power scheduler class, which keeps track of the power
+ * budget and allocates power to different components.
  * @version 0.1
  * @date 2024-12-21
  *
@@ -11,99 +12,186 @@
 
 #include "PowerScheduler.h"
 
-bool PowerScheduler::tryAccquirePowerAllowance(uint16_t neededPower)
-{
-    if (this->freePowerBudget >= neededPower)
-    {
-        this->freePowerBudget -= neededPower;
-        return true;
-    }
-    else
-    {
-        return false;
-    }
+bool PowerScheduler::tryAccquireCurrentAllowance(
+    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
+    uint16_t requestedDurationMs) {
+  portENTER_CRITICAL(&mux);
+  if (this->freeLimitCurrentBudget > 0 &&
+      this->freeMaximumCurrentBudget >= neededCurrent) {
+    this->currentAllowances.push_back(
+        {.consumer = consumer,
+         .maxSlackTimeMs = 0,
+         .requestedDurationMs = requestedDurationMs,
+         .taskHandle = xTaskGetCurrentTaskHandle(),
+         .neededCurrent = neededCurrent,
+         .requestedAt = xTaskGetTickCount(),
+         .granted = false});
+    this->recalculateCurrentBudgets();
+    portEXIT_CRITICAL(&mux);
+    return true;
+  } else {
+    return false;
+  }
 }
 
-void PowerScheduler::releasePower(uint16_t power)
-{
-    if (this->freePowerBudget + power <= this->totalPowerBudget)
-    {
-        this->freePowerBudget += power;
+void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) {
+  portENTER_CRITICAL(&mux);
+  for (auto it = currentAllowances.begin(); it != currentAllowances.end();
+       ++it) {
+    if (it->consumer == consumer && it->granted) {
+      currentAllowances.erase(it);
+      break;
     }
-    else // TODO: Maybe we should actually throw an error here, since obviouslsy someone used us wrong.
-    {
-        this->freePowerBudget = this->totalPowerBudget;
-    }
-    // Check if there are tasks waiting for power
-    checkWaitingTasks();
+  }
+  recalculateCurrentBudgets();
+  portEXIT_CRITICAL(&mux);
+  // Check if there are tasks waiting for power
+  checkWaitingTasks();
 }
 
-bool PowerScheduler::waitForPowerAllowance(uint16_t neededPower, TickType_t ticksToWait)
-{
-    if (tryAccquirePowerAllowance(neededPower))
-    {
-        return true;
+bool PowerScheduler::waitForCurrentAllowance(
+    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
+    uint16_t maxSlackTimeMs, uint16_t requestedDurationMs) {
+  if (tryAccquireCurrentAllowance(consumer, neededCurrent,
+                                  requestedDurationMs)) {
+    return true;
+  } else {
+
+    // Suspend the task while waiting for power to be available
+    TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
+    TickType_t initialTickCount = xTaskGetTickCount();
+    PowerScheduler::CurrentAllowance newAllowance = {
+        .consumer = consumer,
+        .maxSlackTimeMs = maxSlackTimeMs,
+        .requestedDurationMs = requestedDurationMs,
+        .taskHandle = currentTask,
+        .neededCurrent = neededCurrent,
+        .requestedAt = initialTickCount,
+        .granted = false};
+
+    this->currentAllowances.push_back(newAllowance);
+    uint32_t notificationValue;
+    BaseType_t notificationStatus = xTaskNotifyWait(
+        0, 0, &notificationValue, pdMS_TO_TICKS(maxSlackTimeMs));
+    // Code below will be executed after the task is woken up
+    while (notificationStatus == pdPASS) {
+      if (notificationValue ==
+          PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE) {
+        // We were woken up because new power is available, check if it is
+        // enough
+        if (this->freeLimitCurrentBudget > 0 &&
+            this->freeMaximumCurrentBudget >= neededCurrent) {
+          return true;
+        } else {
+          // Still not enough power available for us. Wait the remaining ticks.
+          xTaskNotifyWait(0, 0, &notificationValue,
+                          pdMS_TO_TICKS(maxSlackTimeMs) -
+                              (xTaskGetTickCount() - initialTickCount));
+        }
+      }
     }
-    else
-    {
-        // Suspend the task while waiting for power to be available
-        TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
-        TickType_t initialTickCount = xTaskGetTickCount();
-        waitingTasks.push(currentTask);
-        uint32_t notificationValue;
-        BaseType_t notificationStatus = xTaskNotifyWait(0, 0, &notificationValue, ticksToWait);
-        // Code below will be executed after the task is woken up
-        while (notificationStatus == pdPASS)
-        {
-            if (notificationValue == POWER_AVAILABLE)
-            {
-                // We were woken up because new power is available, check if it is enough
-                if (tryAccquirePowerAllowance(neededPower))
-                {
-                    return true;
-                }
-                else
-                {
-                    // Still not enough power available for us. Wait the remaining ticks.
-                    xTaskNotifyWait(0, 0, &notificationValue, ticksToWait - (xTaskGetTickCount() - initialTickCount));
-                }
-            }
-        }
-        if (notificationStatus == pdFALSE)
-        {
-            // We waited long enough...
-            return false;
-        }
-        else
-        {
-            // Should be impossible to reach
-            throw "Reached impossible state";
+    if (notificationStatus == pdFALSE) {
+      // We waited long enough...
+      // Remove the task from the list of waiting tasks
+      for (auto it = currentAllowances.begin(); it != currentAllowances.end();
+           ++it) {
+        if (it->consumer == consumer &&
+            it->requestedAt == initialTickCount) {
+          currentAllowances.erase(it);
+          break;
         }
+      }
+      return false;
+    } else {
+      // Should be impossible to reach
+      throw "Reached impossible state";
     }
+  }
 }
 
-void PowerScheduler::checkWaitingTasks(void)
-{
-    // Check if there are tasks waiting for power
-    if (!waitingTasks.empty())
-    {
-        TaskHandle_t task = waitingTasks.front();
-        waitingTasks.pop();
-        xTaskNotify(task, POWER_AVAILABLE, eSetValueWithOverwrite);
+void PowerScheduler::checkWaitingTasks(void) {
+  // If there are requested allowances, try to grant the one expiring next
+  if (this->currentAllowances.size() > 0) {
+
+    PowerScheduler::CurrentAllowance* nextAllowance =
+        getNextExpiringAllowance();
+    if (nextAllowance != nullptr) {
+      xTaskNotify(nextAllowance->taskHandle,
+                  PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE,
+                  eSetValueWithOverwrite);
     }
+  }
 }
 
-PowerScheduler *PowerScheduler::getPowerScheduler()
-{
-    if (powerSchedulerInstance == nullptr)
-    {
-        // Double check locking https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
-        taskENTER_CRITICAL(&mux);
-        if (powerSchedulerInstance == nullptr)
-        {
-            powerSchedulerInstance = new PowerScheduler();
-        }
-        taskEXIT_CRITICAL(&mux);
+void PowerScheduler::recalculateCurrentBudgets(void) {
+  // TODO: replace with actual current modeling
+  this->freeLimitCurrentBudget = this->limitCurrent;
+  this->freeMaximumCurrentBudget = this->maximumCurrent * 2;
+  for (auto &allowance : currentAllowances) {
+    if (allowance.granted) {
+      this->freeLimitCurrentBudget -= allowance.neededCurrent;
+      this->freeMaximumCurrentBudget -= allowance.neededCurrent;
     }
-    return powerSchedulerInstance;
-}
\ No newline at end of file
+  }
+}
+
+PowerScheduler::CurrentAllowance*
+PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
+  for (auto &allowance : currentAllowances) {
+    if (allowance.consumer == consumer) {
+      return &allowance;
+    }
+  }
+  return nullptr;
+}
+PowerScheduler::CurrentAllowance*
+PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
+  for (auto &allowance : currentAllowances) {
+    if (allowance.taskHandle == taskHandle) {
+      return &allowance;
+    }
+  }
+  return nullptr;
+}
+PowerScheduler::CurrentAllowance*
+PowerScheduler::getNextExpiringAllowance(void) {
+  TickType_t minTicks = UINT32_MAX;
+  CurrentAllowance *nextAllowance = nullptr;
+  for (auto &allowance : currentAllowances) {
+    if (!(allowance.granted)) {
+      TickType_t ticks =
+          allowance.requestedAt + pdMS_TO_TICKS(allowance.maxSlackTimeMs);
+      if (ticks < minTicks) {
+        minTicks = ticks;
+        nextAllowance = &allowance;
+      }
+    }
+  }
+  // Will be nullptr if no allowance was found
+  return nextAllowance;
+}
+
+PowerScheduler& PowerScheduler::getPowerScheduler(float i_limit_ma,
+                                                  float i_max_ma) {
+  if (powerSchedulerInstance == nullptr) {
+    // Double check locking
+    // https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
+    taskENTER_CRITICAL(&mux);
+    if (powerSchedulerInstance == nullptr) {
+      powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma);
+    }
+    taskEXIT_CRITICAL(&mux);
+  }
+  return *powerSchedulerInstance;
+}
+
+PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) {
+  this->limitCurrent = i_limit_ma;
+  this->maximumCurrent = i_max_ma;
+  this->currentAllowances = std::vector<CurrentAllowance>();
+  mux = portMUX_INITIALIZER_UNLOCKED;
+}
+
+portMUX_TYPE PowerScheduler::mux = portMUX_INITIALIZER_UNLOCKED;
+
+PowerScheduler::~PowerScheduler() {}
\ No newline at end of file
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index 236430e..406192a 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -1,7 +1,8 @@
 /**
  * @file PowerScheduler.hpp
  * @author Phillip Kühne
- * @brief The actual power scheduler class, which keeps track of the power budget and allocates power to different components.
+ * @brief The actual power scheduler class, which keeps track of the power
+ * budget and allocates power to different components.
  * @version 0.1
  * @date 2024-12-21
  *
@@ -9,62 +10,114 @@
  *
  */
 
-#include <queue>
-#include <Arduino.h>
-#include "Consumptions.h"
+#include "PowerParameters.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
+#include <Arduino.h>
+#include <vector>
 
 #ifndef PowerScheduler_h
 #define PowerScheduler_h
 
-#define TOTAL_POWER_MILLIWATTS POWER_BUDGET
-
-class PowerScheduler
-{
+class PowerScheduler {
 private:
-    /* data */
-public:
-    PowerScheduler();
-    ~PowerScheduler();
-    /// @brief Initialize the singleton instance of the power manager
-    /// @return reference to the power manager
-    static PowerScheduler *getPowerScheduler();
-    uint16_t getFreePowerBudget(void);
-    /// @brief Request an allowance of a certain number of milliwatts from the power scheduler
-    /// @param neededPower the amount of power we want to be accounted for (in mW)
-    /// @return whether the power could be successfully allocated
-    bool tryAccquirePowerAllowance(uint16_t neededPower);
-    /// @brief "Return" a certain amount of power when it is no longer needed
-    /// @param neededPower the amount of power to return (in mW)
-    /// @return whether the power
-    void releasePower(uint16_t power);
+  static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100;
+  PowerScheduler(float i_limit_ma, float i_max_ma);
+  
+  public:
+  ~PowerScheduler();
+  /// @brief Initialize the singleton instance of the power manager
+  /// @return reference to the power manager
+  static PowerScheduler& getPowerScheduler(float i_limit_ma=0, float i_max_ma=0);
+  /// @brief Get the current free current budget (to C1 discharge)
+  /// @return the amount of power that is currently available (in mA)
+  uint16_t getFreeCurrentBudget(void);
+  /// @brief Get the current hard maximum free current (to C2 discharge)
+  /// @return the maximum amount of power that can be allocated (in mA)
+  uint16_t getFreeHardMaxCurrent(void);
 
-    /// @brief Wait for a certain amount of power to be available
-    /// @param neededPower the amount of power we want to be accounted for (in mW)
-    /// @param TicksToWait the amount of time to wait for the power to become available
-    /// @return whether the power could be successfully allocatedy
-    bool waitForPowerAllowance(uint16_t neededPower, TickType_t TicksToWait);
-    /// @brief Put the ESP32 into deep sleep mode, without a method to wake up again. Basically this is a shutdown.
-    void beginPermanentDeepSleep(void);
+  /// @brief Request an allowance of a certain number of milliamperes from the
+  /// power scheduler without waiting for it (meaning it will not be scheduled
+  /// for future allocation). Only one can be active per consumer.
+  /// @param neededCurrent the amount of current we want to be accounted for (in
+  /// mA)
+  /// @return whether the current could be successfully allocated
+  bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                                   uint16_t neededcurrent,
+                                   uint16_t requestedDurationMs = 0);
+  /// @brief "Return" the current currently allocated to a consumer
+  /// @param consumer the active consumer to release the current for
+  void releaseCurrent(PowerParameters::PowerConsumers consumer);
+
+  /// @brief Wait for a certain amount of current to be available. This will
+  /// "reseve a spot in the queue". Only one can be active per consumer.
+  /// @param neededCurrent the amount of power we want to be accounted for (in
+  /// mW)
+  /// @param TicksToWait the amount of time to wait for the power to become
+  /// available
+  /// @return whether the power could be successfully allocatedy
+  bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                             uint16_t neededCurrent,
+                             uint16_t maxSlackTimeMs = DEFAULT_SLACK_TIME_MS,
+                             uint16_t requestedDurationMs = 0);
+  /// @brief Put the ESP32 into deep sleep mode, without a method to wake up
+  /// again. Basically this is a shutdown.
+  void beginPermanentDeepSleep(void);
+
+  //// @brief Get currently granted power
+  /// @return the amount of power that is currently allocated (in mA)
+  uint16_t getCurrentCurrent(void);
+
+  /// @brief Power consumer data structure
+  struct CurrentAllowance {
+    PowerParameters::PowerConsumers consumer;
+    uint16_t maxSlackTimeMs;
+    uint16_t requestedDurationMs;
+    TaskHandle_t taskHandle;
+    uint16_t neededCurrent;
+    TickType_t requestedAt;
+    TickType_t startedAt;
+    bool granted;
+  };
+
+  // @brief waiting task wakeup reasons
+  enum PowerWakeupReasons {
+    POWER_AVAILABLE = 1,
+    POWER_EXPIRED = 2,
+  };
 
 protected:
-    static const uint16_t totalPowerBudget = TOTAL_POWER_MILLIWATTS;
-    uint16_t freePowerBudget;
-    std::queue<TaskHandle_t> waitingTasks;
-    void checkWaitingTasks(void);
-    bool takePowerIfAvailable(uint16_t neededPower);
+  // Current above which there will be no new scheduling
+  uint16_t limitCurrent;
+  // Absolute maximum current that can be allocated
+  uint16_t maximumCurrent;
+
+  // Current budget that is currently available to limitCurrent
+  int16_t freeLimitCurrentBudget;
+  // Current budget that is currently available to maximumCurrent
+  int16_t freeMaximumCurrentBudget;
+
+  // @brief Responsible for selecting the next task to be granted power
+  void checkWaitingTasks(void);
+
+  // @brief Responsible for recalculating the current budgets
+  void recalculateCurrentBudgets(void);
+  // @brief Retrieve the current allowance for a given consumer
+  CurrentAllowance *
+  getCurrentAllowance(PowerParameters::PowerConsumers consumer);
+  // @brief Retrieve the current allowance for a given task
+  CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
+
+  // @brief Retrieve the allowance that will expire next
+  CurrentAllowance *getNextExpiringAllowance(void);
+
+  // @brief Mutex to protect the power scheduler from concurrent access
+  static portMUX_TYPE mux;
+
+  std::vector<PowerScheduler::CurrentAllowance> currentAllowances;
 };
 
-PowerScheduler::PowerScheduler(/* args */)
-{
-    // TODO: Create wrappper around all this which handles single-instancing so we look like a normal arduino library from the outside.
-    this->freePowerBudget = TOTAL_POWER_MILLIWATTS;
-}
 
-PowerScheduler::~PowerScheduler()
-{
-}
 
 static PowerScheduler *powerSchedulerInstance;
 

From c63935a41378b4affee5267ff5761ad03e1d2f77 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 16:09:40 +0100
Subject: [PATCH 12/59] Add power modeling and scheduling based on thesis

---
 src/power/Power.cpp          |  74 +++++++++++++++++++++++++-
 src/power/Power.h            |  24 ++++++---
 src/power/PowerParameters.h  |   3 ++
 src/power/PowerScheduler.cpp | 100 ++++++++++++++++++++++++++---------
 src/power/PowerScheduler.h   |  48 +++++++++--------
 5 files changed, 195 insertions(+), 54 deletions(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index b67f9ff..cac1676 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -74,6 +74,16 @@ float Power::getBatteryVoltage() {
 }
 
 int Power::getBatteryChargePercent() {
+  return percentRemaining;
+}
+
+float Power::getBatteryChargeCoulombs() {
+  return coloumbsRemaining;
+}
+
+
+
+int Power::getBatteryVoltageChargePercent() {
   // Get the battery voltage and calculate the charge state based on the
   // discharge curve.
   float batteryVoltage = getBatteryVoltage();
@@ -105,7 +115,69 @@ int Power::getBatteryChargePercent() {
   }
 }
 
-PowerScheduler* Power::powerScheduler = nullptr;
+void Power::updatePowerStateHandler() {
+  float currentCurrent = powerScheduler->getCurrentCurrent();
+  int referenceCurrentMa =
+      PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
+
+  // Calculate remaining battery charge in Coulombs based on current and time
+  float coloumbsConsumedSinceLastUpdate =
+      (currentCurrent / 1000) *
+      ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
+
+  // Update coloumbs remaining
+  coloumbsRemaining -= coloumbsConsumedSinceLastUpdate;
+
+  float chargeState;
+
+  // If current flow is close enough to reference, get battery charge state via
+  // voltage curve
+  if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
+      (currentCurrent < (referenceCurrentMa * 1.4))) {
+    // Get battery charge state from voltage curve
+    chargeState = getBatteryVoltageChargePercent();
+  } else {
+    // Calculate battery charge state from Charge consumption
+    float oldChargeState = lastSOC[latestSoCIndex];
+    float chargeState =
+        oldChargeState - ((coloumbsConsumedSinceLastUpdate /
+                           PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
+                          100);
+  }
+
+  addSoCSample(chargeState);
+
+  // Update percentage remaining based on charge state average
+  float sampleSum = 0;
+  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
+    sampleSum += lastSOC[i];
+  }
+  percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
+
+  // Update last update time
+  lastPowerStateUpdate = xTaskGetTickCount();
+
+  // Update the available current (changes based on battery state of charge)
+  powerScheduler->recalculateCurrentBudgets();
+
+  return;
+
+}
+
+float Power::getMax3V3Current() {
+  float u_bat = getBatteryVoltage();
+  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C;
+  float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
+  constexpr float u_3v3 = 3.3;
+  return (u_bat * i_bat * eta) / u_3v3;
+}
+
+void Power::addSoCSample(float soc) {
+  latestSoCIndex = (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
+  lastSOC[latestSoCIndex] = soc;
+}
+
+PowerScheduler *Power::powerScheduler = nullptr;
 
 Power::Power() {
   // Initialize the power scheduler
diff --git a/src/power/Power.h b/src/power/Power.h
index a748965..4a32b09 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -11,8 +11,6 @@
 #ifndef Power_h
 #define Power_h
 
-#define TOTAL_POWER_MILLIWATTS POWER_BUDGET
-
 enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {
@@ -55,6 +53,11 @@ public:
   /// @return Battery charge state in percent
   static int getBatteryChargePercent();
 
+  /// @brief Get estimated battery charge state as percentage based on
+  // voltage directly
+  /// @return Battery charge state in percent
+  static int getBatteryVoltageChargePercent();
+
   /// @brief Get estimated battery charge state as coulombs
   /// @return Battery charge state in coulombs
   static float getBatteryChargeCoulombs();
@@ -62,26 +65,35 @@ public:
   /// @brief get available current (after voltage conversion and efficiency
   /// losses, referencing 1C discharge)
   /// @return available current in milliamps
-  static float getAvailableCurrent();
+  static float getMax3V3Current();
 
 protected:
   /// @brief PowerScheduler instance to manage power consumption
   static PowerScheduler *powerScheduler;
 
   /// @brief update Power State
-  static void updatePowerState();
+  static void updatePowerStateHandler();
 
   /*
    * Power State
    */
 
+  /// @brief last time of power state update
+  static TickType_t lastPowerStateUpdate;
+
   /// @brief remaining Charge in coulombs
-  static int coloumbsRemaining;
+  static float coloumbsRemaining;
 
   /// @brief remaining Charge in percent
   static int percentRemaining;
 
-  friend class PowerScheduler;
+  /// @brief Circular array of last calculated values for current state of
+  /// charge
+  static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
+  static int latestSoCIndex;
+
+  /// @brief Add calculated value to circular array, pushing out oldest value
+  static void addSoCSample(float soc);
 };
 
 extern Power power;
diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index 98a7a68..fab44e7 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -41,6 +41,9 @@ namespace PowerParameters {
       static constexpr float VOLTAGE_DIVIDER_FACTOR =
           (VOLTAGE_DIVIDER_R12 + VOLTAGE_DIVIDER_R13) / VOLTAGE_DIVIDER_R13;
     };
+
+    // Configuration
+    static constexpr int AVERAGING_SAMPLES = 10;
   };
 
   // Factors concerning Buck-Boost-Converter
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 4bfec42..22a72da 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -11,21 +11,27 @@
  */
 
 #include "PowerScheduler.h"
+#include "Power.h"
 
 bool PowerScheduler::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
     uint16_t requestedDurationMs) {
   portENTER_CRITICAL(&mux);
-  if (this->freeLimitCurrentBudget > 0 &&
-      this->freeMaximumCurrentBudget >= neededCurrent) {
-    this->currentAllowances.push_back(
-        {.consumer = consumer,
-         .maxSlackTimeMs = 0,
-         .requestedDurationMs = requestedDurationMs,
-         .taskHandle = xTaskGetCurrentTaskHandle(),
-         .neededCurrent = neededCurrent,
-         .requestedAt = xTaskGetTickCount(),
-         .granted = false});
+  float existingConsumption = getConsumerCurrent(consumer);
+  if ((this->freeLimitCurrentBudget + existingConsumption) > 0 &&
+      (this->freeMaximumCurrentBudget + existingConsumption) >= neededCurrent) {
+    if (existingConsumption > 0) {
+      releaseCurrent(consumer);
+    }
+    this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{
+        .consumer = consumer,
+        .maxSlackTimeMs = 0,
+        .requestedDurationMs = requestedDurationMs,
+        .taskHandle = xTaskGetCurrentTaskHandle(),
+        .neededCurrent = neededCurrent,
+        .requestedAt = xTaskGetTickCount(),
+        .grantedAt = xTaskGetTickCount(),
+        .granted = true});
     this->recalculateCurrentBudgets();
     portEXIT_CRITICAL(&mux);
     return true;
@@ -79,8 +85,24 @@ bool PowerScheduler::waitForCurrentAllowance(
           PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE) {
         // We were woken up because new power is available, check if it is
         // enough
-        if (this->freeLimitCurrentBudget > 0 &&
-            this->freeMaximumCurrentBudget >= neededCurrent) {
+        float existingConsumption = getConsumerCurrent(consumer);
+        if ((this->freeLimitCurrentBudget + existingConsumption) > 0 &&
+            (this->freeMaximumCurrentBudget + existingConsumption) >=
+                neededCurrent) {
+          // TODO Check if there is a currently active allowance for this
+          // consumer and if so, replace it with the new one
+          if (existingConsumption > 0) {
+            releaseCurrent(consumer);
+          }
+          this->currentAllowances.push_back(
+              {.consumer = consumer,
+               .maxSlackTimeMs = 0,
+               .requestedDurationMs = requestedDurationMs,
+               .taskHandle = xTaskGetCurrentTaskHandle(),
+               .neededCurrent = neededCurrent,
+               .requestedAt = initialTickCount,
+               .grantedAt = xTaskGetTickCount(),
+               .granted = true});
           return true;
         } else {
           // Still not enough power available for us. Wait the remaining ticks.
@@ -95,8 +117,7 @@ bool PowerScheduler::waitForCurrentAllowance(
       // Remove the task from the list of waiting tasks
       for (auto it = currentAllowances.begin(); it != currentAllowances.end();
            ++it) {
-        if (it->consumer == consumer &&
-            it->requestedAt == initialTickCount) {
+        if (it->consumer == consumer && it->requestedAt == initialTickCount) {
           currentAllowances.erase(it);
           break;
         }
@@ -113,7 +134,7 @@ void PowerScheduler::checkWaitingTasks(void) {
   // If there are requested allowances, try to grant the one expiring next
   if (this->currentAllowances.size() > 0) {
 
-    PowerScheduler::CurrentAllowance* nextAllowance =
+    PowerScheduler::CurrentAllowance *nextAllowance =
         getNextExpiringAllowance();
     if (nextAllowance != nullptr) {
       xTaskNotify(nextAllowance->taskHandle,
@@ -124,18 +145,20 @@ void PowerScheduler::checkWaitingTasks(void) {
 }
 
 void PowerScheduler::recalculateCurrentBudgets(void) {
-  // TODO: replace with actual current modeling
-  this->freeLimitCurrentBudget = this->limitCurrent;
-  this->freeMaximumCurrentBudget = this->maximumCurrent * 2;
+  // Get the respective maximums and subtract currently flowing currents
+  float tempFreeLimitCurrentBudget = Power::getMax3V3Current();
+  float tempFreeMaximumCurrentBudget = Power::getMax3V3Current() * 2;
   for (auto &allowance : currentAllowances) {
     if (allowance.granted) {
-      this->freeLimitCurrentBudget -= allowance.neededCurrent;
-      this->freeMaximumCurrentBudget -= allowance.neededCurrent;
+      tempFreeLimitCurrentBudget -= allowance.neededCurrent;
+      tempFreeMaximumCurrentBudget -= allowance.neededCurrent;
     }
   }
+  this->freeLimitCurrentBudget = tempFreeLimitCurrentBudget;
+  this->freeMaximumCurrentBudget = tempFreeMaximumCurrentBudget;
 }
 
-PowerScheduler::CurrentAllowance*
+PowerScheduler::CurrentAllowance *
 PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
   for (auto &allowance : currentAllowances) {
     if (allowance.consumer == consumer) {
@@ -144,7 +167,7 @@ PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance*
+PowerScheduler::CurrentAllowance *
 PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   for (auto &allowance : currentAllowances) {
     if (allowance.taskHandle == taskHandle) {
@@ -153,7 +176,7 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance*
+PowerScheduler::CurrentAllowance *
 PowerScheduler::getNextExpiringAllowance(void) {
   TickType_t minTicks = UINT32_MAX;
   CurrentAllowance *nextAllowance = nullptr;
@@ -171,7 +194,7 @@ PowerScheduler::getNextExpiringAllowance(void) {
   return nextAllowance;
 }
 
-PowerScheduler& PowerScheduler::getPowerScheduler(float i_limit_ma,
+PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,
                                                   float i_max_ma) {
   if (powerSchedulerInstance == nullptr) {
     // Double check locking
@@ -185,6 +208,35 @@ PowerScheduler& PowerScheduler::getPowerScheduler(float i_limit_ma,
   return *powerSchedulerInstance;
 }
 
+float PowerScheduler::getCurrentCurrent(void) {
+  float currentSum = 0;
+  for (auto &allowance : currentAllowances) {
+    if (allowance.granted) {
+      currentSum += allowance.neededCurrent;
+    }
+  }
+  return currentSum;
+}
+
+float PowerScheduler::getFreeCurrentBudget(void) {
+  return this->freeLimitCurrentBudget;
+}
+
+float PowerScheduler::getFreeHardMaxCurrent(void) {
+  return this->freeMaximumCurrentBudget;
+}
+
+float PowerScheduler::getConsumerCurrent(
+    PowerParameters::PowerConsumers consumer) {
+  float currentSum = 0;
+  for (auto &allowance : currentAllowances) {
+    if (allowance.consumer == consumer && allowance.granted) {
+      currentSum += allowance.neededCurrent;
+    }
+  }
+  return currentSum;
+}
+
 PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) {
   this->limitCurrent = i_limit_ma;
   this->maximumCurrent = i_max_ma;
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index 406192a..08a6852 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -23,18 +23,19 @@ class PowerScheduler {
 private:
   static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100;
   PowerScheduler(float i_limit_ma, float i_max_ma);
-  
-  public:
+
+public:
   ~PowerScheduler();
   /// @brief Initialize the singleton instance of the power manager
   /// @return reference to the power manager
-  static PowerScheduler& getPowerScheduler(float i_limit_ma=0, float i_max_ma=0);
+  static PowerScheduler &getPowerScheduler(float i_limit_ma = 0,
+                                           float i_max_ma = 0);
   /// @brief Get the current free current budget (to C1 discharge)
   /// @return the amount of power that is currently available (in mA)
-  uint16_t getFreeCurrentBudget(void);
+  float getFreeCurrentBudget(void);
   /// @brief Get the current hard maximum free current (to C2 discharge)
   /// @return the maximum amount of power that can be allocated (in mA)
-  uint16_t getFreeHardMaxCurrent(void);
+  float getFreeHardMaxCurrent(void);
 
   /// @brief Request an allowance of a certain number of milliamperes from the
   /// power scheduler without waiting for it (meaning it will not be scheduled
@@ -57,16 +58,16 @@ private:
   /// available
   /// @return whether the power could be successfully allocatedy
   bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                             uint16_t neededCurrent,
-                             uint16_t maxSlackTimeMs = DEFAULT_SLACK_TIME_MS,
-                             uint16_t requestedDurationMs = 0);
+                               uint16_t neededCurrent,
+                               uint16_t maxSlackTimeMs = DEFAULT_SLACK_TIME_MS,
+                               uint16_t requestedDurationMs = 0);
   /// @brief Put the ESP32 into deep sleep mode, without a method to wake up
   /// again. Basically this is a shutdown.
   void beginPermanentDeepSleep(void);
 
   //// @brief Get currently granted power
   /// @return the amount of power that is currently allocated (in mA)
-  uint16_t getCurrentCurrent(void);
+  float getCurrentCurrent(void);
 
   /// @brief Power consumer data structure
   struct CurrentAllowance {
@@ -76,7 +77,7 @@ private:
     TaskHandle_t taskHandle;
     uint16_t neededCurrent;
     TickType_t requestedAt;
-    TickType_t startedAt;
+    TickType_t grantedAt;
     bool granted;
   };
 
@@ -85,6 +86,20 @@ private:
     POWER_AVAILABLE = 1,
     POWER_EXPIRED = 2,
   };
+  // @brief Responsible for recalculating the current budgets
+  void recalculateCurrentBudgets(void);
+
+  // @brief Get current consumption of a consumer
+  float getConsumerCurrent(PowerParameters::PowerConsumers consumer);
+
+  // @brief Retrieve the current allowance for a given consumer
+  CurrentAllowance *
+  getCurrentAllowance(PowerParameters::PowerConsumers consumer);
+  // @brief Retrieve the current allowance for a given task
+  CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
+
+  // @brief Retrieve the allowance that will expire next
+  CurrentAllowance *getNextExpiringAllowance(void);
 
 protected:
   // Current above which there will be no new scheduling
@@ -100,25 +115,12 @@ protected:
   // @brief Responsible for selecting the next task to be granted power
   void checkWaitingTasks(void);
 
-  // @brief Responsible for recalculating the current budgets
-  void recalculateCurrentBudgets(void);
-  // @brief Retrieve the current allowance for a given consumer
-  CurrentAllowance *
-  getCurrentAllowance(PowerParameters::PowerConsumers consumer);
-  // @brief Retrieve the current allowance for a given task
-  CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
-
-  // @brief Retrieve the allowance that will expire next
-  CurrentAllowance *getNextExpiringAllowance(void);
-
   // @brief Mutex to protect the power scheduler from concurrent access
   static portMUX_TYPE mux;
 
   std::vector<PowerScheduler::CurrentAllowance> currentAllowances;
 };
 
-
-
 static PowerScheduler *powerSchedulerInstance;
 
 #endif // PowerScheduler_h
\ No newline at end of file

From 8617f420a2c85ade57e96e5f09dceb25dd11fd80 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 16:27:54 +0100
Subject: [PATCH 13/59] Add special case for small power requests

---
 src/power/PowerParameters.h  |  7 +++++++
 src/power/PowerScheduler.cpp | 22 +++++++++++++++++-----
 2 files changed, 24 insertions(+), 5 deletions(-)

diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index fab44e7..6b21529 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -79,6 +79,13 @@ namespace PowerParameters {
     static constexpr float CURRENT_UV_LED = 200;
   };
 
+  /*
+   * Single consumer current limit up to which requests are granted no matter what.
+   * The idea is, that this will allow Sensors (with their miniscule power draw)
+   * to always be granted power, which should massively improve behaviour.
+   */
+  static constexpr float CURRENT_INSIGNIFICANT = 1;
+
   struct PinConfig {
     static constexpr int BAT_ADC = 10;
     static constexpr int BAT_ADC_EN = 9;
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 22a72da..8c1f434 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -18,8 +18,13 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
     uint16_t requestedDurationMs) {
   portENTER_CRITICAL(&mux);
   float existingConsumption = getConsumerCurrent(consumer);
-  if ((this->freeLimitCurrentBudget + existingConsumption) > 0 &&
-      (this->freeMaximumCurrentBudget + existingConsumption) >= neededCurrent) {
+  const bool currentAvailableBelowLimit =
+      this->freeLimitCurrentBudget + existingConsumption > 0;
+  const bool currentAvailableBelowMaximum =
+      this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
+  const bool currentIsInsignificant = neededCurrent < 0.1;
+  if (currentIsInsignificant ||
+      (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
     if (existingConsumption > 0) {
       releaseCurrent(consumer);
     }
@@ -85,10 +90,17 @@ bool PowerScheduler::waitForCurrentAllowance(
           PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE) {
         // We were woken up because new power is available, check if it is
         // enough
+        // Exclude existing consumption from the same consumer, as it will be
+        // gone if this is granted
         float existingConsumption = getConsumerCurrent(consumer);
-        if ((this->freeLimitCurrentBudget + existingConsumption) > 0 &&
-            (this->freeMaximumCurrentBudget + existingConsumption) >=
-                neededCurrent) {
+        const bool currentAvailableBelowLimit =
+            this->freeLimitCurrentBudget + existingConsumption > 0;
+        const bool currentAvailableBelowMaximum =
+            this->freeMaximumCurrentBudget + existingConsumption >=
+            neededCurrent;
+        const bool currentIsInsignificant = neededCurrent < 0.1;
+        if (currentIsInsignificant ||
+            (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
           // TODO Check if there is a currently active allowance for this
           // consumer and if so, replace it with the new one
           if (existingConsumption > 0) {

From d64579eca4b3cdcceaa21f03fac7ed00e1c636e3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 17:42:22 +0100
Subject: [PATCH 14/59] Complete proxy functions in Power.h and refactor
 (mainly reorder) for clarity.

---
 src/power/Power.cpp          | 46 +++++++++++++++++++------------
 src/power/Power.h            | 53 ++++++++++++++++++++++++------------
 src/power/PowerScheduler.cpp |  4 +--
 src/power/PowerScheduler.h   |  4 +--
 4 files changed, 68 insertions(+), 39 deletions(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index cac1676..6888b7d 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -26,8 +26,12 @@ void Power::begin() {
   }
 }
 
-uint16_t Power::getFreeCurrentBudget(void) {
-  return powerScheduler->getFreeCurrentBudget();
+float Power::getFreeLimitCurrentBudget() {
+  return powerScheduler->getFreeLimitCurrentBudget();
+}
+
+float Power::getFreeMaximumCurrentBudget() {
+  return powerScheduler->getFreeMaximumCurrentBudget();
 }
 
 bool Power::tryAccquireCurrentAllowance(
@@ -37,20 +41,32 @@ bool Power::tryAccquireCurrentAllowance(
                                                      requestedDurationMs);
 }
 
+void Power::releaseCurrent(PowerParameters::PowerConsumers consumer) {
+  powerScheduler->releaseCurrent(consumer);
+}
+
 bool Power::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                     uint16_t neededCurrent,
-                                    TickType_t TicksToWait,
+                                    uint16_t maxSlackTimeMs,
                                     uint16_t requestedDurationMs) {
   return powerScheduler->waitForCurrentAllowance(
-      consumer, neededCurrent, TicksToWait, requestedDurationMs);
+      consumer, neededCurrent, maxSlackTimeMs, requestedDurationMs);
 }
 
 void Power::beginPermanentDeepSleep(void) {
   return powerScheduler->beginPermanentDeepSleep();
 }
 
-void Power::releaseCurrent(PowerParameters::PowerConsumers consumer) {
-  powerScheduler->releaseCurrent(consumer);
+float Power::getCurrentCurrent(void) {
+  return powerScheduler->getCurrentCurrent();
+}
+
+void Power::recalculateCurrentBudgets(void) {
+  return powerScheduler->recalculateCurrentBudgets();
+}
+
+float Power::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
+  return powerScheduler->getConsumerCurrent(consumer);
 }
 
 float Power::getBatteryVoltage() {
@@ -73,19 +89,13 @@ float Power::getBatteryVoltage() {
   return batteryVoltage;
 }
 
-int Power::getBatteryChargePercent() {
-  return percentRemaining;
-}
-
-float Power::getBatteryChargeCoulombs() {
-  return coloumbsRemaining;
-}
-
+int Power::getBatteryChargePercent() { return percentRemaining; }
 
+float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }
 
 int Power::getBatteryVoltageChargePercent() {
-  // Get the battery voltage and calculate the charge state based on the
-  // discharge curve.
+  // Directly get the battery voltage and calculate the charge state based on
+  // the discharge curve.
   float batteryVoltage = getBatteryVoltage();
   float chargeState = 0;
   // Clamp edge cases
@@ -161,7 +171,6 @@ void Power::updatePowerStateHandler() {
   powerScheduler->recalculateCurrentBudgets();
 
   return;
-
 }
 
 float Power::getMax3V3Current() {
@@ -173,7 +182,8 @@ float Power::getMax3V3Current() {
 }
 
 void Power::addSoCSample(float soc) {
-  latestSoCIndex = (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
+  latestSoCIndex =
+      (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
   lastSOC[latestSoCIndex] = soc;
 }
 
diff --git a/src/power/Power.h b/src/power/Power.h
index 4a32b09..162fc1b 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -18,33 +18,52 @@ class Power {
 public:
   static void begin(void);
   Power();
-  uint16_t getFreeCurrentBudget(void);
-  /// @brief Request an allowance of a certain number of milliwatts from the
-  /// power scheduler
-  /// @param neededPower the amount of power we want to be accounted for (in mW)
-  /// @return whether the power could be successfully allocated
-  static bool
-  tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                              uint16_t neededCurrent,
-                              uint16_t requestedDurationMs);
-  /// @brief "Return" the power consumed by an active consumer
-  /// @param neededPower the amount of power to return (in mW)
-  /// @return whether the power
+   /// @brief Get the current free current budget (to C1 discharge)
+  /// @return the amount of power that is currently available (in mA)
+  static float getFreeLimitCurrentBudget(void);
+  /// @brief Get the current hard maximum free current (to C2 discharge)
+  /// @return the maximum amount of power that can be allocated (in mA)
+  static float getFreeMaximumCurrentBudget(void);
+
+  /// @brief Request an allowance of a certain number of milliamperes from the
+  /// power scheduler without waiting for it (meaning it will not be scheduled
+  /// for future allocation). Only one can be active per consumer.
+  /// @param neededCurrent the amount of current we want to be accounted for (in
+  /// mA)
+  /// @return whether the current could be successfully allocated
+  static bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                                   uint16_t neededcurrent,
+                                   uint16_t requestedDurationMs = 0);
+  /// @brief "Return" the current currently allocated to a consumer
+  /// @param consumer the active consumer to release the current for
   static void releaseCurrent(PowerParameters::PowerConsumers consumer);
 
-  /// @brief Wait for a certain amount of power to be available
-  /// @param neededPower the amount of power we want to be accounted for (in mW)
+  /// @brief Wait for a certain amount of current to be available. This will
+  /// "reseve a spot in the queue". Only one can be active per consumer.
+  /// @param neededCurrent the amount of power we want to be accounted for (in
+  /// mW)
   /// @param TicksToWait the amount of time to wait for the power to become
   /// available
   /// @return whether the power could be successfully allocatedy
   static bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                                      uint16_t neededCurrent,
-                                      TickType_t TicksToWait,
-                                      uint16_t requestedDurationMs);
+                               uint16_t neededCurrent,
+                               uint16_t maxSlackTimeMs,
+                               uint16_t requestedDurationMs);
   /// @brief Put the ESP32 into deep sleep mode, without a method to wake up
   /// again. Basically this is a shutdown.
   static void beginPermanentDeepSleep(void);
 
+  //// @brief Get currently granted power
+  /// @return the amount of power that is currently allocated (in mA)
+  static float getCurrentCurrent(void);
+
+  // @brief Responsible for recalculating the current budgets
+  static void recalculateCurrentBudgets(void);
+
+  // @brief Get current consumption of a consumer
+  static float getConsumerCurrent(PowerParameters::PowerConsumers consumer);
+
+
   /// @brief Get battery voltage measurement.
   /// @return Battery Terminal Voltage in Volts
   static float getBatteryVoltage();
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 8c1f434..5d2ab23 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -230,11 +230,11 @@ float PowerScheduler::getCurrentCurrent(void) {
   return currentSum;
 }
 
-float PowerScheduler::getFreeCurrentBudget(void) {
+float PowerScheduler::getFreeLimitCurrentBudget(void) {
   return this->freeLimitCurrentBudget;
 }
 
-float PowerScheduler::getFreeHardMaxCurrent(void) {
+float PowerScheduler::getFreeMaximumCurrentBudget(void) {
   return this->freeMaximumCurrentBudget;
 }
 
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index 08a6852..641f8e3 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -32,10 +32,10 @@ public:
                                            float i_max_ma = 0);
   /// @brief Get the current free current budget (to C1 discharge)
   /// @return the amount of power that is currently available (in mA)
-  float getFreeCurrentBudget(void);
+  float getFreeLimitCurrentBudget(void);
   /// @brief Get the current hard maximum free current (to C2 discharge)
   /// @return the maximum amount of power that can be allocated (in mA)
-  float getFreeHardMaxCurrent(void);
+  float getFreeMaximumCurrentBudget(void);
 
   /// @brief Request an allowance of a certain number of milliamperes from the
   /// power scheduler without waiting for it (meaning it will not be scheduled

From 4bfae98f6b203f7490536af62a2d2394d5804e7c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 17:58:32 +0100
Subject: [PATCH 15/59] Integrate Communication into Power Management

---
 src/communication/Communication.cpp | 116 +++++++++++++++-------------
 src/communication/Communication.h   |  20 +++--
 src/power/PowerParameters.h         |   3 +
 3 files changed, 76 insertions(+), 63 deletions(-)

diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index 6e443f9..d2ef5d8 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -1,4 +1,5 @@
 #include "Communication.h"
+#include "../power/Power.h"
 
 Scheduler userScheduler; // to control your personal task
 painlessMesh mesh;
@@ -7,81 +8,86 @@ uint32_t Communication::groupNumber = 0;
 // User-defined callback function pointer
 void (*Communication::userCallback)(String &msg) = nullptr;
 
-void Communication::sendMessage(String msg)
-{
-    String data = String(groupNumber) + "#" + msg;
-    mesh.sendBroadcast(data);
+void Communication::sendMessage(String msg) {
+  String data = String(groupNumber) + "#" + msg;
+  mesh.sendBroadcast(data);
 }
 
 // Needed for painless library
-void Communication::receivedCallback(uint32_t from, String &msg)
-{
-    int separatorIndex = msg.indexOf('#');
-    if (separatorIndex != -1) {
-        String groupNumberStr = msg.substring(0, separatorIndex);
-        uint32_t num = groupNumberStr.toInt();
-        String restOfMsg = msg.substring(separatorIndex + 1);
+void Communication::receivedCallback(uint32_t from, String &msg) {
+  int separatorIndex = msg.indexOf('#');
+  if (separatorIndex != -1) {
+    String groupNumberStr = msg.substring(0, separatorIndex);
+    uint32_t num = groupNumberStr.toInt();
+    String restOfMsg = msg.substring(separatorIndex + 1);
 
-        Serial.printf("startHere: Received from %u groupNumber=%u msg=%s\n", from, num, restOfMsg.c_str());
+    Serial.printf("startHere: Received from %u groupNumber=%u msg=%s\n", from,
+                  num, restOfMsg.c_str());
 
-        if (groupNumber != num) return;
+    if (groupNumber != num)
+      return;
 
-        // Execute user-defined callback if it is set
-        if (userCallback) {
-            userCallback(restOfMsg);
-        }
+    // Execute user-defined callback if it is set
+    if (userCallback) {
+      userCallback(restOfMsg);
     }
+  }
 }
 
-void newConnectionCallback(uint32_t nodeId)
-{
-    Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId);
+void newConnectionCallback(uint32_t nodeId) {
+  Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId);
 }
 
-void changedConnectionCallback()
-{
-    Serial.printf("Changed connections\n");
+void changedConnectionCallback() { Serial.printf("Changed connections\n"); }
+
+void nodeTimeAdjustedCallback(int32_t offset) {
+  Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(), offset);
 }
 
-void nodeTimeAdjustedCallback(int32_t offset)
-{
-    Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(), offset);
+void vTaskUpdate(void *pvParameters) {
+  for (;;) {
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::WIFI,
+        PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
+            PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
+        MESSH_MAX_EXECUTION_DELAY_MS, NULL);
+    mesh.update();
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::WIFI,
+        PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
+        MESSH_MAX_EXECUTION_DELAY_MS, NULL);
+  }
 }
 
-void vTaskUpdate(void *pvParameters)
-{
-    for (;;)
-    {
-        mesh.update();
-    }
-}
-
-void Communication::setGroupNumber(uint32_t number) {
-    groupNumber = number;
-}
+void Communication::setGroupNumber(uint32_t number) { groupNumber = number; }
 
 // Method to set the user-defined callback function
-void Communication::onReceive(void (*callbackFunc)(String &msg))
-{
-    userCallback = callbackFunc;
+void Communication::onReceive(void (*callbackFunc)(String &msg)) {
+  userCallback = callbackFunc;
 }
 
-void Communication::begin(void)
-{
-    Serial.begin(115200);
+void Communication::begin(void) {
+  Serial.begin(115200);
 
-    // mesh.setDebugMsgTypes( ERROR | MESH_STATUS | CONNECTION | SYNC | COMMUNICATION | GENERAL | MSG_TYPES | REMOTE ); // all types on
-    mesh.setDebugMsgTypes(ERROR | STARTUP); // set before init() so that you can see startup messages
+  // mesh.setDebugMsgTypes( ERROR | MESH_STATUS | CONNECTION | SYNC |
+  // COMMUNICATION | GENERAL | MSG_TYPES | REMOTE ); // all types on
+  mesh.setDebugMsgTypes(
+      ERROR |
+      STARTUP); // set before init() so that you can see startup messages
+  Power::waitForCurrentAllowance(
+      PowerParameters::PowerConsumers::WIFI,
+      PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
+      MESSH_MAX_EXECUTION_DELAY_MS, NULL);
+  mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
+  mesh.onReceive(&receivedCallback);
+  mesh.onNewConnection(&newConnectionCallback);
+  mesh.onChangedConnections(&changedConnectionCallback);
+  mesh.onNodeTimeAdjusted(&nodeTimeAdjustedCallback);
 
-    mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
-    mesh.onReceive(&receivedCallback);
-    mesh.onNewConnection(&newConnectionCallback);
-    mesh.onChangedConnections(&changedConnectionCallback);
-    mesh.onNodeTimeAdjusted(&nodeTimeAdjustedCallback);
+  static uint8_t ucParameterToPass;
+  TaskHandle_t xHandle = NULL;
 
-    static uint8_t ucParameterToPass;
-    TaskHandle_t xHandle = NULL;
-
-    xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass, tskIDLE_PRIORITY, &xHandle);
-    configASSERT(xHandle);
+  xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass,
+              tskIDLE_PRIORITY, &xHandle);
+  configASSERT(xHandle);
 };
\ No newline at end of file
diff --git a/src/communication/Communication.h b/src/communication/Communication.h
index e6644f7..215fc9f 100644
--- a/src/communication/Communication.h
+++ b/src/communication/Communication.h
@@ -8,22 +8,26 @@
 #define   MESH_PREFIX     "DEZIBOT_MESH"
 #define   MESH_PASSWORD   "somethingSneaky"
 #define   MESH_PORT       5555
+#define   MESSH_MAX_EXECUTION_DELAY_MS 10
+
+
 
 
 class Communication{
-public: 
+    public: 
     /**
-     * @brief initialize the Mesh Compnent, must be called before the other methods are used.  
-     * 
-     */
+    * @brief initialize the Mesh Compnent, must be called before the other methods are used.  
+    * 
+    */
     static void begin(void);
-
+    
     void setGroupNumber(uint32_t number);
-
+    
     void sendMessage(String msg);
-
+    
     void onReceive(void (*callbackFunc)(String &msg));
-private:
+
+    private:
     static void (*userCallback)(String &msg);
     static void receivedCallback(uint32_t from, String &msg);
     static uint32_t groupNumber;
diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index 6b21529..1487e1c 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -59,6 +59,9 @@ namespace PowerParameters {
     static constexpr float CURRENT_ESP_LOAD = 88.43;
     static constexpr float CURRENT_ESP_AVG =
         (CURRENT_ESP_BASE + CURRENT_ESP_LOAD) / 2;
+    // WiFi current consumptions
+    static constexpr float CURRENT_WIFI_BASE = 64.58;
+    static constexpr float CURRENT_WIFI_PEAK = 128;
     // RGB LED quiescent current
     static constexpr float CURRENT_LED_RGB_BASE = 0.7;
     // RGB LED per channel current during PWM on-time.

From 8a93e0ca93683ddd8850127a617697129d792e18 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 18:27:48 +0100
Subject: [PATCH 16/59] Integrate RGB LEDs into power management

---
 src/multiColorLight/MultiColorLight.cpp | 24 +++++++++++++++++++++++-
 src/multiColorLight/MultiColorLight.h   |  5 +++++
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index aa22a4a..c6936b1 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -5,6 +5,9 @@ MultiColorLight::MultiColorLight():rgbLeds(ledAmount,ledPin){
 };
 
 void MultiColorLight::begin(void){
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
     rgbLeds.begin();
     this->turnOffLed();
 };
@@ -13,7 +16,26 @@ void MultiColorLight::setLed(uint8_t index , uint32_t color){
     if (index > ledAmount-1){
         //TODO: logging
     }
-    rgbLeds.setPixelColor(index, normalizeColor(color));
+    uint32_t normalizedColor = normalizeColor(color);
+    uint16_t colorComponentRed = (normalizedColor & 0x00FF0000) >> 16;
+    uint16_t colorComponentGreen = (normalizedColor & 0x0000FF00) >> 8;
+    uint16_t colorComponentBlue = (normalizedColor & 0x000000FF);
+    float redChannelConsumption = (colorComponentRed/255.0) * PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
+    float greenChannelConsumption = (colorComponentGreen/255.0) * PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
+    float blueChannelConsumption = (colorComponentBlue/255.0) * PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
+    float totalConsumption = redChannelConsumption + greenChannelConsumption + blueChannelConsumption + PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE;
+    switch (index) {
+        case 0:
+            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            break;
+        case 1:
+            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            break;
+        case 2:
+            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            break;
+    }
+    rgbLeds.setPixelColor(index, normalizedColor);
     rgbLeds.show();
 };
 
diff --git a/src/multiColorLight/MultiColorLight.h b/src/multiColorLight/MultiColorLight.h
index 44498cf..0d5634e 100644
--- a/src/multiColorLight/MultiColorLight.h
+++ b/src/multiColorLight/MultiColorLight.h
@@ -12,6 +12,10 @@
 #define MultiColorLight_h
 #include <Adafruit_NeoPixel.h>
 #include "ColorConstants.h"
+#include "../power/Power.h"
+
+#define MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS 20
+
 /**
  * @brief Describes combinations of leds on the Dezibot. 
  * With the Robot in Front of you, when the robot drives away from you, the left LED is TOP_LEFT 
@@ -31,6 +35,7 @@ protected:
     static const int16_t ledPin = 48;
     static const uint8_t maxBrightness = 150;
     Adafruit_NeoPixel rgbLeds;
+    static constexpr int maximumExecutionDelayMs = 10;
 public:
     
     MultiColorLight();

From 6611fba2dc920e3c4611f320a6f27e3b5dedde3a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 20:39:59 +0100
Subject: [PATCH 17/59] Allow release of reserved current regardless of the
 reservation having been granted

---
 src/power/PowerScheduler.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 5d2ab23..96198fc 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -49,7 +49,7 @@ void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) {
   portENTER_CRITICAL(&mux);
   for (auto it = currentAllowances.begin(); it != currentAllowances.end();
        ++it) {
-    if (it->consumer == consumer && it->granted) {
+    if (it->consumer == consumer) {
       currentAllowances.erase(it);
       break;
     }

From e5ff1e761049c7b0e8e89063e6e12b9578e0d98e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:24:57 +0100
Subject: [PATCH 18/59] Group non-independent sensors

---
 src/power/PowerParameters.h | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index 1487e1c..0cd3eb8 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -105,12 +105,8 @@ namespace PowerParameters {
     RGBW_SENSOR,
     LED_IR_BOTTOM,
     LED_IR_FRONT,
-    PT_IR_LEFT,
-    PT_IR_RIGHT,
-    PT_IR_FRONT,
-    PT_IR_BACK,
-    PT_DL_FRONT,
-    PT_DL_BOTTOM,
+    PT_IR,
+    PT_DL,
     LED_UV,
     DISPLAY_OLED,
     MOTOR_LEFT,

From 54075436588c35d09b6c2fbfcf1070ed5a564748 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:26:29 +0100
Subject: [PATCH 19/59] Integrate color sensor, infrared LEDs and
 phototransistors into Power Management

---
 src/colorDetection/ColorDetection.cpp |  1 +
 src/colorDetection/ColorDetection.h   |  7 +++--
 src/infraredLight/InfraredLED.cpp     | 43 +++++++++++++++++++++++++--
 src/infraredLight/InfraredLight.h     |  7 +++--
 src/lightDetection/LightDetection.cpp | 10 +++++++
 src/lightDetection/LightDetection.h   |  5 ++--
 6 files changed, 65 insertions(+), 8 deletions(-)

diff --git a/src/colorDetection/ColorDetection.cpp b/src/colorDetection/ColorDetection.cpp
index b41bb16..a19ec0a 100644
--- a/src/colorDetection/ColorDetection.cpp
+++ b/src/colorDetection/ColorDetection.cpp
@@ -1,6 +1,7 @@
 #include  "ColorDetection.h"
 
 void ColorDetection::begin(void){
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::RGBW_SENSOR, PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW, COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
     ColorDetection::configure(VEML_CONFIG{.mode = AUTO,.enabled = true,.exposureTime=MS40});
 };
 void ColorDetection::configure(VEML_CONFIG config){
diff --git a/src/colorDetection/ColorDetection.h b/src/colorDetection/ColorDetection.h
index 6fd284b..d3dcc98 100644
--- a/src/colorDetection/ColorDetection.h
+++ b/src/colorDetection/ColorDetection.h
@@ -11,9 +11,10 @@
 
 #ifndef ColorDetection_h
 #define ColorDetection_h
-#include <stdint.h>
-#include <Wire.h>
+#include "../power/Power.h"
 #include <Arduino.h>
+#include <Wire.h>
+#include <stdint.h>
 //Definitions for I2c
 #define I2C_MASTER_SCL_IO           2      /*!< GPIO number used for I2C master clock */
 #define I2C_MASTER_SDA_IO           1      /*!< GPIO number used for I2C master data  */
@@ -28,6 +29,8 @@
 #define REG_BLUE                    0x0A
 #define REG_WHITE                   0x0B
 
+#define COLOR_DETECTION_MAX_EXECUTION_DELAY_MS 1
+
 
 enum duration{
     MS40,
diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp
index 7298603..e5e11d9 100644
--- a/src/infraredLight/InfraredLED.cpp
+++ b/src/infraredLight/InfraredLED.cpp
@@ -1,6 +1,9 @@
 #include "InfraredLight.h"
 
 #define pwmSpeedMode LEDC_LOW_SPEED_MODE
+#define IR_FRONT_PIN 14
+#define IR_BOTTOM_PIN 13
+#define DUTY_RESOLUTION LEDC_TIMER_10_BIT
 
 InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel){
     this->ledPin = pin;
@@ -12,7 +15,7 @@ void InfraredLED::begin(void){
    //we want to change frequency instead of 
     pwmTimer = ledc_timer_config_t{
         .speed_mode = pwmSpeedMode,
-        .duty_resolution = LEDC_TIMER_10_BIT,
+        .duty_resolution = DUTY_RESOLUTION,
         .timer_num = this->timer,
         .freq_hz = 800,
         .clk_cfg = LEDC_AUTO_CLK
@@ -42,8 +45,24 @@ void InfraredLED::turnOff(void){
 void InfraredLED::setState(bool state){
     ledc_set_freq(pwmSpeedMode,timer,1);
     if (state) {
+      if (this->ledPin == IR_BOTTOM_PIN) {
+        Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::LED_IR_BOTTOM,
+            PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM,
+            IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+      } else if (this->ledPin == IR_FRONT_PIN) {
+        Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::LED_IR_FRONT,
+            PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT,
+            IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+      }
         ledc_set_duty(pwmSpeedMode,channel,1023);
     } else {
+      if (this->ledPin == IR_BOTTOM_PIN) {
+        Power::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM);
+      } else {
+        Power::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT);
+      }
         ledc_set_duty(pwmSpeedMode,channel,0);
     }
     ledc_update_duty(pwmSpeedMode,channel);
@@ -51,7 +70,27 @@ void InfraredLED::setState(bool state){
 };
 
 void InfraredLED::sendFrequency(uint16_t frequency){
+  constexpr uint32_t duty = 512;
+  // Float to force float division without casting
+  constexpr float resolution = 1 << DUTY_RESOLUTION;
+  if (this->ledPin == IR_BOTTOM_PIN) {
+    float currentConsumption =
+        (duty / resolution) *
+        PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM;
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::LED_IR_BOTTOM,
+        currentConsumption,
+        IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+  } else if (this->ledPin == IR_FRONT_PIN) {
+    float currentConsumption =
+        (duty / resolution) *
+        PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT;
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::LED_IR_FRONT,
+        currentConsumption,
+        IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+  }
     ledc_set_freq(pwmSpeedMode,timer,frequency);
-    ledc_set_duty(pwmSpeedMode,channel,512);
+    ledc_set_duty(pwmSpeedMode,channel,duty);
     ledc_update_duty(pwmSpeedMode,channel);
 };
\ No newline at end of file
diff --git a/src/infraredLight/InfraredLight.h b/src/infraredLight/InfraredLight.h
index 024e10a..beaabf9 100644
--- a/src/infraredLight/InfraredLight.h
+++ b/src/infraredLight/InfraredLight.h
@@ -10,9 +10,12 @@
  */
 #ifndef InfraredLight_h
 #define InfraredLight_h
-#include <stdint.h>
-#include <Arduino.h>
+#include "../power/Power.h"
 #include "driver/ledc.h"
+#include <Arduino.h>
+#include <stdint.h>
+
+#define IR_LED_MAX_EXECUTION_DELAY_MS 1
 
 
 class InfraredLED{
diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index 8934941..622caa7 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -64,6 +64,10 @@ uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measu
 };
 
 void LightDetection::beginInfrared(void){
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::PT_IR,
+        PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
     digitalWrite(IR_PT_ENABLE,true);
     pinMode(IR_PT_ENABLE, OUTPUT);
     pinMode(IR_PT_FRONT_ADC, INPUT);
@@ -73,6 +77,10 @@ void LightDetection::beginInfrared(void){
 };
 
 void LightDetection::beginDaylight(void){
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::PT_DL,
+        PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
     digitalWrite(DL_PT_ENABLE,true);
     pinMode(DL_PT_ENABLE, OUTPUT);
     pinMode(DL_PT_BOTTOM_ADC, INPUT);
@@ -99,6 +107,7 @@ uint16_t LightDetection::readIRPT(photoTransistors sensor){
     default:
         break;
     }
+    //Power::releaseCurrent(PowerParameters::PowerConsumers::PT_IR);
     //digitalWrite(IR_PT_ENABLE,LOW);
     return result;
 };
@@ -117,6 +126,7 @@ uint16_t LightDetection::readDLPT(photoTransistors sensor){
     default:
         break;
     }
+    Power::releaseCurrent(PowerParameters::PowerConsumers::PT_DL);
     digitalWrite(DL_PT_ENABLE,LOW);
     return result;
 };
\ No newline at end of file
diff --git a/src/lightDetection/LightDetection.h b/src/lightDetection/LightDetection.h
index 32e25f3..2c6a0db 100644
--- a/src/lightDetection/LightDetection.h
+++ b/src/lightDetection/LightDetection.h
@@ -11,10 +11,11 @@
 
 #ifndef LightDetection_h
 #define LightDetection_h
-#include <stdint.h>
+#include "../power/Power.h"
 #include <Arduino.h>
+#include <stdint.h>
 
-
+#define LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS 1
 
 enum photoTransistors{
     IR_LEFT,

From dfa778024b2d1dd03692810ea76aa16d88bfde9c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:38:05 +0100
Subject: [PATCH 20/59] Integrate OLED display in Power management

---
 src/display/Display.cpp | 3 +++
 src/display/Display.h   | 8 ++++++--
 2 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/display/Display.cpp b/src/display/Display.cpp
index 9e687d5..1e67541 100644
--- a/src/display/Display.cpp
+++ b/src/display/Display.cpp
@@ -14,6 +14,9 @@
 
 
 void Display::begin(void){
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::DISPLAY_OLED, PowerParameters::CurrentConsumptions::CURRENT_DISPLAY
+        ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL);
     //set Mux Ratio
     sendDisplayCMD(muxRatio);
     sendDisplayCMD(0x3f);
diff --git a/src/display/Display.h b/src/display/Display.h
index 15774a4..c4bc1d7 100644
--- a/src/display/Display.h
+++ b/src/display/Display.h
@@ -11,9 +11,13 @@
 
 #ifndef Display_h
 #define Display_h
-#include <stdint.h>
-#include <Arduino.h>
+#include "../power/Power.h"
 #include "DisplayCMDs.h"
+#include <Arduino.h>
+#include <stdint.h>
+
+// This execution delay is basically only used for initial activation, so it can be set to a higher value
+#define DISPLAY_MAX_EXECUTION_DELAY_MS 100
 
 class Display{
     protected:

From a26acf4a927942c53215e2a83fbf5cba86295ac1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:50:29 +0100
Subject: [PATCH 21/59] Integrate Motors into Power Management

---
 src/motion/Motion.h  |  5 ++++-
 src/motion/Motor.cpp | 10 ++++++++++
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index daf70c0..bb26a72 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -17,7 +17,7 @@
 #include <freertos/task.h>
 #include "driver/ledc.h"
 #include "motionDetection/MotionDetection.h"
-#include "power/Power.h"
+#include "../power/Power.h"
 #define LEDC_MODE          LEDC_LOW_SPEED_MODE
 #define TIMER              LEDC_TIMER_2
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
@@ -25,6 +25,9 @@
 #define DUTY_RES           LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
 #define FREQUENCY          (5000) // Frequency in Hertz. Set frequency at 5 kHz
 #define DEFAULT_BASE_VALUE  3900
+
+#define MOTOR_MAX_EXECUTION_DELAY_MS 100
+
 class Motor{
     public:
         Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index d6c68e3..a5c4e2b 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -1,6 +1,9 @@
 #include "Motion.h"
 #include "power/Power.h"
 
+#define MOTOR_LEFT_PIN  12
+#define MOTOR_RIGHT_PIN 11
+
 Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
     this->pin = pin;
     this->channel = channel;
@@ -24,6 +27,13 @@ void Motor::begin(void){
 };
 
 void Motor::setSpeed(uint16_t duty){
+    const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
+    const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
+    if (this->pin == MOTOR_LEFT_PIN){
+        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_LEFT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+    } else {
+        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_RIGHT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+    }
     int difference = duty-this->getSpeed();
     if (difference > 0){
         for(int i = 0;i<difference;i+=difference/20){

From 7c349a32890df29336edfba3fef02045603ba054 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:56:40 +0100
Subject: [PATCH 22/59] Integrate IMU into Power Management

---
 src/motionDetection/MotionDetection.cpp | 4 ++++
 src/motionDetection/MotionDetection.h   | 8 ++++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index a06618e..ba9ce17 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -6,6 +6,10 @@ MotionDetection::MotionDetection(){
 };
 
 void MotionDetection::begin(void){
+    Power::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::IMU,
+        PowerParameters::CurrentConsumptions::CURRENT_IMU,
+        IMU_MAX_EXECUTION_DELAY_MS, NULL);
     pinMode(34,OUTPUT);
     digitalWrite(34,HIGH);
     handler->begin(36,37,35,34);
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 8fecbb6..4b5b3e6 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -10,9 +10,13 @@
  */
 #ifndef MotionDetection_h
 #define MotionDetection_h
-#include <SPI.h>
-#include <Arduino.h>
+#include "../power/Power.h"
 #include "IMU_CMDs.h"
+#include <Arduino.h>
+#include <SPI.h>
+
+#define IMU_MAX_EXECUTION_DELAY_MS 1
+
 struct IMUResult{
     int16_t x;
     int16_t y;

From 1730ea958c3b6905a99bd51260111d11714cb51c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:56:55 +0100
Subject: [PATCH 23/59] Fix Typo

---
 src/communication/Communication.cpp | 6 +++---
 src/communication/Communication.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index d2ef5d8..da2a005 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -50,12 +50,12 @@ void vTaskUpdate(void *pvParameters) {
         PowerParameters::PowerConsumers::WIFI,
         PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
             PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-        MESSH_MAX_EXECUTION_DELAY_MS, NULL);
+        MESH_MAX_EXECUTION_DELAY_MS, NULL);
     mesh.update();
     Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::WIFI,
         PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-        MESSH_MAX_EXECUTION_DELAY_MS, NULL);
+        MESH_MAX_EXECUTION_DELAY_MS, NULL);
   }
 }
 
@@ -77,7 +77,7 @@ void Communication::begin(void) {
   Power::waitForCurrentAllowance(
       PowerParameters::PowerConsumers::WIFI,
       PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-      MESSH_MAX_EXECUTION_DELAY_MS, NULL);
+      MESH_MAX_EXECUTION_DELAY_MS, NULL);
   mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
   mesh.onReceive(&receivedCallback);
   mesh.onNewConnection(&newConnectionCallback);
diff --git a/src/communication/Communication.h b/src/communication/Communication.h
index 215fc9f..e4c13df 100644
--- a/src/communication/Communication.h
+++ b/src/communication/Communication.h
@@ -8,7 +8,7 @@
 #define   MESH_PREFIX     "DEZIBOT_MESH"
 #define   MESH_PASSWORD   "somethingSneaky"
 #define   MESH_PORT       5555
-#define   MESSH_MAX_EXECUTION_DELAY_MS 10
+#define   MESH_MAX_EXECUTION_DELAY_MS 10
 
 
 

From 39daae8cc3b7e0a6dcfc38871fd770324f01a4c0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 22:02:21 +0100
Subject: [PATCH 24/59] Fix Include

---
 src/communication/Communication.cpp | 1 -
 src/communication/Communication.h   | 1 +
 2 files changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index da2a005..3347f06 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -1,5 +1,4 @@
 #include "Communication.h"
-#include "../power/Power.h"
 
 Scheduler userScheduler; // to control your personal task
 painlessMesh mesh;
diff --git a/src/communication/Communication.h b/src/communication/Communication.h
index e4c13df..522829b 100644
--- a/src/communication/Communication.h
+++ b/src/communication/Communication.h
@@ -4,6 +4,7 @@
 #include <stdint.h>
 #include <Arduino.h>
 #include <painlessMesh.h>
+#include "../power/Power.h"
 
 #define   MESH_PREFIX     "DEZIBOT_MESH"
 #define   MESH_PASSWORD   "somethingSneaky"

From 21f7d9ae8ae6f181b671b105f824a57730753c1a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 22:08:18 +0100
Subject: [PATCH 25/59] Behaviour clarification

---
 src/power/PowerScheduler.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index 641f8e3..eaf2946 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -48,6 +48,7 @@ public:
                                    uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
   /// @param consumer the active consumer to release the current for
+  /// @note This will delete all power requests for the given consumer
   void releaseCurrent(PowerParameters::PowerConsumers consumer);
 
   /// @brief Wait for a certain amount of current to be available. This will

From c130026f00d8dddb9287c1db20c5282015b0cbaf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 22:59:13 +0100
Subject: [PATCH 26/59] Handle denials of power appropriately

---
 src/colorDetection/ColorDetection.cpp   | 11 ++++++++--
 src/colorDetection/ColorDetection.h     |  1 +
 src/communication/Communication.cpp     | 27 ++++++++++++++++---------
 src/communication/Communication.h       |  7 +++----
 src/display/Display.cpp                 |  7 +++++--
 src/display/Display.h                   |  2 ++
 src/infraredLight/InfraredLED.cpp       | 24 ++++++++++++++--------
 src/infraredLight/InfraredLight.h       |  1 +
 src/lightDetection/LightDetection.cpp   | 14 +++++++++----
 src/lightDetection/LightDetection.h     |  2 ++
 src/motion/Motion.h                     |  2 ++
 src/motion/Motor.cpp                    | 20 +++++++++++++++---
 src/motionDetection/MotionDetection.cpp | 11 ++++++----
 src/multiColorLight/MultiColorLight.cpp | 25 +++++++++++++++++------
 src/multiColorLight/MultiColorLight.h   |  2 ++
 src/power/Power.cpp                     |  5 ++---
 src/power/Power.h                       |  2 ++
 src/power/PowerScheduler.cpp            |  5 ++++-
 src/power/PowerScheduler.h              |  5 +++++
 19 files changed, 126 insertions(+), 47 deletions(-)

diff --git a/src/colorDetection/ColorDetection.cpp b/src/colorDetection/ColorDetection.cpp
index a19ec0a..b350760 100644
--- a/src/colorDetection/ColorDetection.cpp
+++ b/src/colorDetection/ColorDetection.cpp
@@ -1,8 +1,15 @@
 #include  "ColorDetection.h"
 
 void ColorDetection::begin(void){
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::RGBW_SENSOR, PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW, COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
-    ColorDetection::configure(VEML_CONFIG{.mode = AUTO,.enabled = true,.exposureTime=MS40});
+  if(!Power::waitForCurrentAllowance(
+      PowerParameters::PowerConsumers::RGBW_SENSOR,
+      PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW,
+      COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)){
+        ESP_LOGE(TAG,"Could not get power for ColorDetection");
+        throw "Could not get power for ColorDetection";
+  }
+  ColorDetection::configure(
+      VEML_CONFIG{.mode = AUTO, .enabled = true, .exposureTime = MS40});
 };
 void ColorDetection::configure(VEML_CONFIG config){
      uint8_t configRegister = 0;
diff --git a/src/colorDetection/ColorDetection.h b/src/colorDetection/ColorDetection.h
index d3dcc98..4030432 100644
--- a/src/colorDetection/ColorDetection.h
+++ b/src/colorDetection/ColorDetection.h
@@ -31,6 +31,7 @@
 
 #define COLOR_DETECTION_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "ColorDetection"
 
 enum duration{
     MS40,
diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index 3347f06..0335347 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -45,12 +45,16 @@ void nodeTimeAdjustedCallback(int32_t offset) {
 
 void vTaskUpdate(void *pvParameters) {
   for (;;) {
-    Power::waitForCurrentAllowance(
-        PowerParameters::PowerConsumers::WIFI,
-        PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
-            PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-        MESH_MAX_EXECUTION_DELAY_MS, NULL);
-    mesh.update();
+    if (Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::WIFI,
+            PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
+                PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
+            MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
+
+      mesh.update();
+    } else {
+      ESP_LOGW(TAG, "Skipping mesh update after not being granted power");
+    }
     Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::WIFI,
         PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
@@ -73,10 +77,13 @@ void Communication::begin(void) {
   mesh.setDebugMsgTypes(
       ERROR |
       STARTUP); // set before init() so that you can see startup messages
-  Power::waitForCurrentAllowance(
-      PowerParameters::PowerConsumers::WIFI,
-      PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-      MESH_MAX_EXECUTION_DELAY_MS, NULL);
+  if (!Power::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::WIFI,
+          PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
+          MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Failed to get power for mesh initialization");
+    throw "Failed to get power for mesh initialization";
+  }
   mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
   mesh.onReceive(&receivedCallback);
   mesh.onNewConnection(&newConnectionCallback);
diff --git a/src/communication/Communication.h b/src/communication/Communication.h
index 522829b..6f75d5c 100644
--- a/src/communication/Communication.h
+++ b/src/communication/Communication.h
@@ -1,18 +1,17 @@
 
 #ifndef Communication_h
 #define Communication_h
-#include <stdint.h>
+#include "../power/Power.h"
 #include <Arduino.h>
 #include <painlessMesh.h>
-#include "../power/Power.h"
+#include <stdint.h>
 
 #define   MESH_PREFIX     "DEZIBOT_MESH"
 #define   MESH_PASSWORD   "somethingSneaky"
 #define   MESH_PORT       5555
 #define   MESH_MAX_EXECUTION_DELAY_MS 10
 
-
-
+#define TAG "Communication"
 
 class Communication{
     public: 
diff --git a/src/display/Display.cpp b/src/display/Display.cpp
index 1e67541..bae4b6d 100644
--- a/src/display/Display.cpp
+++ b/src/display/Display.cpp
@@ -14,9 +14,12 @@
 
 
 void Display::begin(void){
-    Power::waitForCurrentAllowance(
+    if(!Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::DISPLAY_OLED, PowerParameters::CurrentConsumptions::CURRENT_DISPLAY
-        ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL);
+        ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL)){
+            ESP_LOGE(TAG,"Could not get power for Display");
+            throw "Could not get power for Display";
+    }
     //set Mux Ratio
     sendDisplayCMD(muxRatio);
     sendDisplayCMD(0x3f);
diff --git a/src/display/Display.h b/src/display/Display.h
index c4bc1d7..f9b439b 100644
--- a/src/display/Display.h
+++ b/src/display/Display.h
@@ -19,6 +19,8 @@
 // This execution delay is basically only used for initial activation, so it can be set to a higher value
 #define DISPLAY_MAX_EXECUTION_DELAY_MS 100
 
+#define TAG "Display"
+
 class Display{
     protected:
         //how many chars are on current line
diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp
index e5e11d9..3819a35 100644
--- a/src/infraredLight/InfraredLED.cpp
+++ b/src/infraredLight/InfraredLED.cpp
@@ -46,15 +46,23 @@ void InfraredLED::setState(bool state){
     ledc_set_freq(pwmSpeedMode,timer,1);
     if (state) {
       if (this->ledPin == IR_BOTTOM_PIN) {
-        Power::waitForCurrentAllowance(
-            PowerParameters::PowerConsumers::LED_IR_BOTTOM,
-            PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM,
-            IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+        if (!Power::waitForCurrentAllowance(
+                PowerParameters::PowerConsumers::LED_IR_BOTTOM,
+                PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM,
+                IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
+          ESP_LOGE(TAG,
+                   "Could not get power for Bottom IR LED. Not turning on.");
+          return;
+        }
       } else if (this->ledPin == IR_FRONT_PIN) {
-        Power::waitForCurrentAllowance(
-            PowerParameters::PowerConsumers::LED_IR_FRONT,
-            PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT,
-            IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+        if (!Power::waitForCurrentAllowance(
+                PowerParameters::PowerConsumers::LED_IR_FRONT,
+                PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT,
+                IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
+          ESP_LOGE(TAG,
+                   "Could not get power for Front IR LED. Not turning on.");
+          return;
+        }
       }
         ledc_set_duty(pwmSpeedMode,channel,1023);
     } else {
diff --git a/src/infraredLight/InfraredLight.h b/src/infraredLight/InfraredLight.h
index beaabf9..56ac538 100644
--- a/src/infraredLight/InfraredLight.h
+++ b/src/infraredLight/InfraredLight.h
@@ -17,6 +17,7 @@
 
 #define IR_LED_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "InfraredLight"
 
 class InfraredLED{
     public: 
diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index 622caa7..b33588a 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -64,10 +64,13 @@ uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measu
 };
 
 void LightDetection::beginInfrared(void){
-    Power::waitForCurrentAllowance(
+    if(!Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::PT_IR,
         PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
-        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+            ESP_LOGE(TAG,"Could not get power for Infrared Phototransistors");
+            throw "Could not get power for Infrared Phototransistors";
+    }
     digitalWrite(IR_PT_ENABLE,true);
     pinMode(IR_PT_ENABLE, OUTPUT);
     pinMode(IR_PT_FRONT_ADC, INPUT);
@@ -77,10 +80,13 @@ void LightDetection::beginInfrared(void){
 };
 
 void LightDetection::beginDaylight(void){
-    Power::waitForCurrentAllowance(
+    if(!Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::PT_DL,
         PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
-        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+            ESP_LOGE(TAG,"Could not get power for Daylight Phototransistors");
+            throw "Could not get power for Daylight Phototransistors";
+    }
     digitalWrite(DL_PT_ENABLE,true);
     pinMode(DL_PT_ENABLE, OUTPUT);
     pinMode(DL_PT_BOTTOM_ADC, INPUT);
diff --git a/src/lightDetection/LightDetection.h b/src/lightDetection/LightDetection.h
index 2c6a0db..5c64027 100644
--- a/src/lightDetection/LightDetection.h
+++ b/src/lightDetection/LightDetection.h
@@ -17,6 +17,8 @@
 
 #define LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "LightDetection"
+
 enum photoTransistors{
     IR_LEFT,
     IR_RIGHT,
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index bb26a72..fcc57b4 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -28,6 +28,8 @@
 
 #define MOTOR_MAX_EXECUTION_DELAY_MS 100
 
+#define TAG "Motion"
+
 class Motor{
     public:
         Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index a5c4e2b..74275f7 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -29,10 +29,24 @@ void Motor::begin(void){
 void Motor::setSpeed(uint16_t duty){
     const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
     const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
-    if (this->pin == MOTOR_LEFT_PIN){
-        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_LEFT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+    if (this->pin == MOTOR_LEFT_PIN) {
+      if (!Power::waitForCurrentAllowance(
+              PowerParameters::PowerConsumers::MOTOR_LEFT, current,
+              MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
+        ESP_LOGW(TAG,
+                 "Power to set LEFT MOTOR to speed %d not granted in time. "
+                 "Skipping.",
+                 duty);
+      }
     } else {
-        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_RIGHT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+      if (!Power::waitForCurrentAllowance(
+              PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
+              MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
+        ESP_LOGW(TAG,
+                 "Power to set RIGHT MOTOR to speed %d not granted in time. "
+                 "Skipping.",
+                 duty);
+      }
     }
     int difference = duty-this->getSpeed();
     if (difference > 0){
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index ba9ce17..199d8fd 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -6,10 +6,13 @@ MotionDetection::MotionDetection(){
 };
 
 void MotionDetection::begin(void){
-    Power::waitForCurrentAllowance(
-        PowerParameters::PowerConsumers::IMU,
-        PowerParameters::CurrentConsumptions::CURRENT_IMU,
-        IMU_MAX_EXECUTION_DELAY_MS, NULL);
+  if (!Power::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::IMU,
+          PowerParameters::CurrentConsumptions::CURRENT_IMU,
+          IMU_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Could not get power for MotionDetection");
+    throw "Could not get power for MotionDetection";
+  }
     pinMode(34,OUTPUT);
     digitalWrite(34,HIGH);
     handler->begin(36,37,35,34);
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index c6936b1..a75c0a6 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -5,13 +5,17 @@ MultiColorLight::MultiColorLight():rgbLeds(ledAmount,ledPin){
 };
 
 void MultiColorLight::begin(void){
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+    if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) ){
+      ESP_LOGE(TAG, "Could not get power for MultiColorLight");
+      throw "Could not get power for MultiColorLight";
+    }
     rgbLeds.begin();
     this->turnOffLed();
 };
 
+
 void MultiColorLight::setLed(uint8_t index , uint32_t color){
     if (index > ledAmount-1){
         //TODO: logging
@@ -26,13 +30,22 @@ void MultiColorLight::setLed(uint8_t index , uint32_t color){
     float totalConsumption = redChannelConsumption + greenChannelConsumption + blueChannelConsumption + PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE;
     switch (index) {
         case 0:
-            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
+                ESP_LOGW(TAG,  "Power to set LED RGB TOP RIGHT to color %d not granted in time. Skipping.", color);
+                return;
+            }
             break;
         case 1:
-            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
+                ESP_LOGW(TAG,  "Power to set LED RGB TOP LEFT to color %d not granted in time. Skipping.", color);
+                return;
+            }
             break;
         case 2:
-            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
+                ESP_LOGW(TAG,  "Power to set LED RGB BOTTOM to color %d not granted in time. Skipping.", color);
+                return;
+            }
             break;
     }
     rgbLeds.setPixelColor(index, normalizedColor);
diff --git a/src/multiColorLight/MultiColorLight.h b/src/multiColorLight/MultiColorLight.h
index 0d5634e..562958a 100644
--- a/src/multiColorLight/MultiColorLight.h
+++ b/src/multiColorLight/MultiColorLight.h
@@ -29,6 +29,8 @@ enum leds{
     ALL
 };
 
+#define TAG "MultiColorLight"
+
 class MultiColorLight{
 protected:
     static const uint16_t ledAmount = 3;
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 6888b7d..53e7ddc 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -19,9 +19,8 @@ void Power::begin() {
     if (!(powerScheduler->tryAccquireCurrentAllowance(
             PowerParameters::PowerConsumers::ESP,
             PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
-      Serial.println("Alledgedly not enough power available to reserve the "
-                     "ESP32s base power consumption. Something is wrong.");
-      return;
+      ESP_LOGE(TAG, "Could not get power for ESP");
+      throw "Could not get power for ESP";
     }
   }
 }
diff --git a/src/power/Power.h b/src/power/Power.h
index 162fc1b..11685b7 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -11,6 +11,8 @@
 #ifndef Power_h
 #define Power_h
 
+#define TAG "Power"
+
 enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 96198fc..5d59e77 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -22,7 +22,7 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
       this->freeLimitCurrentBudget + existingConsumption > 0;
   const bool currentAvailableBelowMaximum =
       this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
-  const bool currentIsInsignificant = neededCurrent < 0.1;
+  const bool currentIsInsignificant = neededCurrent < 1;
   if (currentIsInsignificant ||
       (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
     if (existingConsumption > 0) {
@@ -115,6 +115,9 @@ bool PowerScheduler::waitForCurrentAllowance(
                .requestedAt = initialTickCount,
                .grantedAt = xTaskGetTickCount(),
                .granted = true});
+          ESP_LOGV(TAG, "%d mA granted to consumer %d after %d ms",
+                   neededCurrent, static_cast<int>(consumer),
+                   xTaskGetTickCount() - initialTickCount);
           return true;
         } else {
           // Still not enough power available for us. Wait the remaining ticks.
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index eaf2946..636fbb1 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -19,6 +19,8 @@
 #ifndef PowerScheduler_h
 #define PowerScheduler_h
 
+#define TAG "PowerScheduler"
+
 class PowerScheduler {
 private:
   static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100;
@@ -43,6 +45,9 @@ public:
   /// @param neededCurrent the amount of current we want to be accounted for (in
   /// mA)
   /// @return whether the current could be successfully allocated
+  /// @note This takes existing power consumption by the same consumer into account,
+  /// so requesting the same or less power will always succeed. Also, small amounts
+  /// of power (below 1 mA) will always be granted.
   bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                    uint16_t neededcurrent,
                                    uint16_t requestedDurationMs = 0);

From b0068333c827fe16118caf46fa5af414ae77b6ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 00:54:03 +0100
Subject: [PATCH 27/59] Fixes

---
 src/Dezibot.cpp              |  2 ++
 src/power/Power.cpp          | 61 ++++++++++++++++++++++++++----------
 src/power/Power.h            |  9 ++++--
 src/power/PowerParameters.h  |  4 +--
 src/power/PowerScheduler.cpp | 31 +++++++++++-------
 src/power/PowerScheduler.h   |  7 ++---
 6 files changed, 78 insertions(+), 36 deletions(-)

diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp
index 2282813..d03f3bc 100644
--- a/src/Dezibot.cpp
+++ b/src/Dezibot.cpp
@@ -9,7 +9,9 @@ Dezibot::Dezibot() : multiColorLight() {};
 
 void Dezibot::begin(void)
 {
+    ESP_LOGI("Dezibot", "Initializing Dezibot");
     power.begin();
+    delay(10);
     Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
     lightDetection.begin();
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 53e7ddc..638484c 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -8,13 +8,31 @@
  */
 
 #include "Power.h"
-static portMUX_TYPE mux;
+
+void vTaskUpdatePowerState(void *pvParameters) {
+  for (;;) {
+    ESP_LOGV(TAG, "Updating Power State...");
+    Power::updatePowerStateHandler();
+    vTaskDelay(pdMS_TO_TICKS(10));
+  }
+}
 
 void Power::begin() {
   // Check if another instance of us already initialized the power scheduler,
   // if not, we will do it.
+  ESP_LOGI(TAG, "Initializing Power Management");
+
   if (powerScheduler == nullptr) {
-    powerScheduler = &PowerScheduler::getPowerScheduler();
+    ESP_LOGI(TAG, "Creating Power Scheduler");
+    powerScheduler = &PowerScheduler::getPowerScheduler(
+        PowerParameters::Battery::CELL_CURRENT_1C_MA,
+        PowerParameters::Battery::CELL_CURRENT_2C_MA);
+    Power::recalculateCurrentBudgets();
+    Power::updatePowerStateHandler();
+    TaskHandle_t xHandle = NULL;
+    xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
+                tskIDLE_PRIORITY, &xHandle);
+    configASSERT(xHandle);
 
     if (!(powerScheduler->tryAccquireCurrentAllowance(
             PowerParameters::PowerConsumers::ESP,
@@ -71,20 +89,22 @@ float Power::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
 float Power::getBatteryVoltage() {
   // Get the battery voltage from the ADC and convert it to a voltage
   // using the voltage divider.
-  portENTER_CRITICAL(&mux);
+  pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT);
+  pinMode(PowerParameters::PinConfig::BAT_ADC, INPUT);
   // Enable voltage divider
   digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH);
+  // Allow voltage to stabilize
+  delayMicroseconds(10);
   // Returns value between 0 and 4095 mapping to between 0 and 3.3V
-  uint16_t batteryAdcValue =
-      analogRead(PowerParameters::PinConfig::BAT_ADC) *
-      (PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR);
+  uint16_t batteryAdcValue = analogRead(PowerParameters::PinConfig::BAT_ADC);
   // Disable voltage divider
   digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW);
-  portEXIT_CRITICAL(&mux);
   // Convert ADC value to voltage
   float batteryVoltage =
-      (batteryAdcValue * 3.3 / 4095) *
+      (batteryAdcValue / 4095.0 * 3.3) *
       PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR;
+  ESP_LOGD(TAG, "Battery ADC value: %d, Calculated voltage: %.2f",
+           batteryAdcValue, batteryVoltage);
   return batteryVoltage;
 }
 
@@ -168,13 +188,14 @@ void Power::updatePowerStateHandler() {
 
   // Update the available current (changes based on battery state of charge)
   powerScheduler->recalculateCurrentBudgets();
-
+  ESP_LOGV(TAG, "Current: %f mA, Charge: %f Coulombs, %d %%", currentCurrent,
+           coloumbsRemaining, percentRemaining);
   return;
 }
 
 float Power::getMax3V3Current() {
   float u_bat = getBatteryVoltage();
-  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C;
+  float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA;
   float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
   constexpr float u_3v3 = 3.3;
   return (u_bat * i_bat * eta) / u_3v3;
@@ -186,11 +207,19 @@ void Power::addSoCSample(float soc) {
   lastSOC[latestSoCIndex] = soc;
 }
 
+void Power::initPowerState(void) {
+  // Initialize the power state
+  lastPowerStateUpdate = xTaskGetTickCount();
+  // TODO: Get initial battery charge state based on voltage, set coloumbs based
+  // on that
+}
+
+int Power::latestSoCIndex = 0;
+float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0};
+TickType_t Power::lastPowerStateUpdate = 0;
+float Power::coloumbsRemaining =
+    PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
+int Power::percentRemaining = 100.0;
 PowerScheduler *Power::powerScheduler = nullptr;
 
-Power::Power() {
-  // Initialize the power scheduler
-  powerScheduler = &PowerScheduler::getPowerScheduler();
-  // Initialize the mutex
-  mux = portMUX_INITIALIZER_UNLOCKED;
-}
\ No newline at end of file
+Power::Power() {}
\ No newline at end of file
diff --git a/src/power/Power.h b/src/power/Power.h
index 11685b7..daebe8d 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -88,12 +88,14 @@ public:
   /// @return available current in milliamps
   static float getMax3V3Current();
 
+  /// @brief update Power State
+  /// @note needs to be public for task creation
+  static void updatePowerStateHandler();
+
 protected:
   /// @brief PowerScheduler instance to manage power consumption
   static PowerScheduler *powerScheduler;
 
-  /// @brief update Power State
-  static void updatePowerStateHandler();
 
   /*
    * Power State
@@ -115,6 +117,9 @@ protected:
 
   /// @brief Add calculated value to circular array, pushing out oldest value
   static void addSoCSample(float soc);
+
+  /// @brief initialize the power state
+  static void initPowerState(void);
 };
 
 extern Power power;
diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index 0cd3eb8..6da3bd9 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -33,8 +33,8 @@ namespace PowerParameters {
     static constexpr float CELL_CHARGE_FULL_COLOUMB = CELL_CAPACITY_MAH * 3.6;
     static constexpr float CELL_ENERGY_FULL_JOULES =
         CELL_CAPACITY_MAH * CELL_VOLTAGE_NOMINAL * 3.6;
-    static constexpr float CELL_CURRENT_1C = CELL_CAPACITY_MAH;
-    static constexpr float CELL_CURRENT_2C = CELL_CAPACITY_MAH * 2;
+    static constexpr float CELL_CURRENT_1C_MA = CELL_CAPACITY_MAH;
+    static constexpr float CELL_CURRENT_2C_MA = CELL_CAPACITY_MAH * 2;
     struct BAT_ADC {
       static constexpr float VOLTAGE_DIVIDER_R12 = 27e3;
       static constexpr float VOLTAGE_DIVIDER_R13 = 10e3;
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 5d59e77..e2acd1c 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -14,9 +14,8 @@
 #include "Power.h"
 
 bool PowerScheduler::tryAccquireCurrentAllowance(
-    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
+    PowerParameters::PowerConsumers consumer, float neededCurrent,
     uint16_t requestedDurationMs) {
-  portENTER_CRITICAL(&mux);
   float existingConsumption = getConsumerCurrent(consumer);
   const bool currentAvailableBelowLimit =
       this->freeLimitCurrentBudget + existingConsumption > 0;
@@ -38,15 +37,19 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
         .grantedAt = xTaskGetTickCount(),
         .granted = true});
     this->recalculateCurrentBudgets();
-    portEXIT_CRITICAL(&mux);
     return true;
   } else {
+    ESP_LOGI(TAG,
+             "Task %p denied %f mA of power;"
+             "currently allocated: %f mA;"
+             "total available (2C): %f mA",
+             xTaskGetCurrentTaskHandle(), neededCurrent, getCurrentCurrent(),
+             maximumCurrent);
     return false;
   }
 }
 
 void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) {
-  portENTER_CRITICAL(&mux);
   for (auto it = currentAllowances.begin(); it != currentAllowances.end();
        ++it) {
     if (it->consumer == consumer) {
@@ -55,13 +58,12 @@ void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) {
     }
   }
   recalculateCurrentBudgets();
-  portEXIT_CRITICAL(&mux);
   // Check if there are tasks waiting for power
   checkWaitingTasks();
 }
 
 bool PowerScheduler::waitForCurrentAllowance(
-    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
+    PowerParameters::PowerConsumers consumer, float neededCurrent,
     uint16_t maxSlackTimeMs, uint16_t requestedDurationMs) {
   if (tryAccquireCurrentAllowance(consumer, neededCurrent,
                                   requestedDurationMs)) {
@@ -98,7 +100,7 @@ bool PowerScheduler::waitForCurrentAllowance(
         const bool currentAvailableBelowMaximum =
             this->freeMaximumCurrentBudget + existingConsumption >=
             neededCurrent;
-        const bool currentIsInsignificant = neededCurrent < 0.1;
+        const bool currentIsInsignificant = neededCurrent < 1;
         if (currentIsInsignificant ||
             (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
           // TODO Check if there is a currently active allowance for this
@@ -137,6 +139,12 @@ bool PowerScheduler::waitForCurrentAllowance(
           break;
         }
       }
+      ESP_LOGI(TAG,
+               "Task %p timed out waiting for %f mA of power;"
+               "currently allocated: %f mA;"
+               "total available (2C): %f mA",
+               xTaskGetCurrentTaskHandle(), neededCurrent, getCurrentCurrent(),
+               maximumCurrent);
       return false;
     } else {
       // Should be impossible to reach
@@ -161,7 +169,9 @@ void PowerScheduler::checkWaitingTasks(void) {
 
 void PowerScheduler::recalculateCurrentBudgets(void) {
   // Get the respective maximums and subtract currently flowing currents
+  ESP_LOGI(TAG, "Recalculating current budgets...");
   float tempFreeLimitCurrentBudget = Power::getMax3V3Current();
+  ESP_LOGI(TAG, "Got max 3V3 current: %.2f", tempFreeLimitCurrentBudget);
   float tempFreeMaximumCurrentBudget = Power::getMax3V3Current() * 2;
   for (auto &allowance : currentAllowances) {
     if (allowance.granted) {
@@ -171,6 +181,8 @@ void PowerScheduler::recalculateCurrentBudgets(void) {
   }
   this->freeLimitCurrentBudget = tempFreeLimitCurrentBudget;
   this->freeMaximumCurrentBudget = tempFreeMaximumCurrentBudget;
+  ESP_LOGV(TAG, "Current budgets recalculated: %f mA, %f mA",
+           this->freeLimitCurrentBudget, this->freeMaximumCurrentBudget);
 }
 
 PowerScheduler::CurrentAllowance *
@@ -214,11 +226,9 @@ PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,
   if (powerSchedulerInstance == nullptr) {
     // Double check locking
     // https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
-    taskENTER_CRITICAL(&mux);
     if (powerSchedulerInstance == nullptr) {
       powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma);
     }
-    taskEXIT_CRITICAL(&mux);
   }
   return *powerSchedulerInstance;
 }
@@ -256,9 +266,6 @@ PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) {
   this->limitCurrent = i_limit_ma;
   this->maximumCurrent = i_max_ma;
   this->currentAllowances = std::vector<CurrentAllowance>();
-  mux = portMUX_INITIALIZER_UNLOCKED;
 }
 
-portMUX_TYPE PowerScheduler::mux = portMUX_INITIALIZER_UNLOCKED;
-
 PowerScheduler::~PowerScheduler() {}
\ No newline at end of file
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index 636fbb1..f3fad60 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -49,7 +49,7 @@ public:
   /// so requesting the same or less power will always succeed. Also, small amounts
   /// of power (below 1 mA) will always be granted.
   bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                                   uint16_t neededcurrent,
+                                   float neededcurrent,
                                    uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
   /// @param consumer the active consumer to release the current for
@@ -64,7 +64,7 @@ public:
   /// available
   /// @return whether the power could be successfully allocatedy
   bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                               uint16_t neededCurrent,
+                               float neededCurrent,
                                uint16_t maxSlackTimeMs = DEFAULT_SLACK_TIME_MS,
                                uint16_t requestedDurationMs = 0);
   /// @brief Put the ESP32 into deep sleep mode, without a method to wake up
@@ -81,7 +81,7 @@ public:
     uint16_t maxSlackTimeMs;
     uint16_t requestedDurationMs;
     TaskHandle_t taskHandle;
-    uint16_t neededCurrent;
+    float neededCurrent;
     TickType_t requestedAt;
     TickType_t grantedAt;
     bool granted;
@@ -122,7 +122,6 @@ protected:
   void checkWaitingTasks(void);
 
   // @brief Mutex to protect the power scheduler from concurrent access
-  static portMUX_TYPE mux;
 
   std::vector<PowerScheduler::CurrentAllowance> currentAllowances;
 };

From 115b0e06793a757f39ac0acfb563f81fcaa38f27 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 21:12:40 +0100
Subject: [PATCH 28/59] Add power state initialisation

---
 src/power/Power.cpp | 51 ++++++++++++++++++++++++++++----
 src/power/Power.h   | 71 ++++++++++++++++++++++++---------------------
 2 files changed, 83 insertions(+), 39 deletions(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 638484c..dcd7eab 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -27,8 +27,8 @@ void Power::begin() {
     powerScheduler = &PowerScheduler::getPowerScheduler(
         PowerParameters::Battery::CELL_CURRENT_1C_MA,
         PowerParameters::Battery::CELL_CURRENT_2C_MA);
+    Power::initPowerState();
     Power::recalculateCurrentBudgets();
-    Power::updatePowerStateHandler();
     TaskHandle_t xHandle = NULL;
     xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
                 tskIDLE_PRIORITY, &xHandle);
@@ -108,11 +108,11 @@ float Power::getBatteryVoltage() {
   return batteryVoltage;
 }
 
-int Power::getBatteryChargePercent() { return percentRemaining; }
+float Power::getBatteryChargePercent() { return percentRemaining; }
 
 float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }
 
-int Power::getBatteryVoltageChargePercent() {
+float Power::getBatteryVoltageChargePercent() {
   // Directly get the battery voltage and calculate the charge state based on
   // the discharge curve.
   float batteryVoltage = getBatteryVoltage();
@@ -166,9 +166,9 @@ void Power::updatePowerStateHandler() {
     // Get battery charge state from voltage curve
     chargeState = getBatteryVoltageChargePercent();
   } else {
-    // Calculate battery charge state from Charge consumption
+    // Estimate battery charge state from charge consumption
     float oldChargeState = lastSOC[latestSoCIndex];
-    float chargeState =
+    chargeState =
         oldChargeState - ((coloumbsConsumedSinceLastUpdate /
                            PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
                           100);
@@ -212,6 +212,45 @@ void Power::initPowerState(void) {
   lastPowerStateUpdate = xTaskGetTickCount();
   // TODO: Get initial battery charge state based on voltage, set coloumbs based
   // on that
+  const float initialChargePercentages = getBatteryVoltageChargePercent();
+  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
+    lastSOC[i] = initialChargePercentages;
+  }
+  coloumbsRemaining = initialChargePercentages / 100 *
+                      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
+  constexpr float fullColoumbs =
+      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
+  percentRemaining = initialChargePercentages;
+}
+
+void Power::dumpPowerStatistics() {
+  Serial.printf("======== Dezibot Power Statistics ========\r\n");
+  Serial.printf("Current: %f mA\r\n", Power::getCurrentCurrent());
+  Serial.printf("Battery Voltage: %f V\r\n", Power::getBatteryVoltage());
+  Serial.printf("Battery Charge: %f %%\r\n", Power::getBatteryChargePercent());
+  Serial.printf("Battery Charge: %f Coulombs\r\n", Power::getBatteryChargeCoulombs());
+  Serial.printf("Max 3.3V Current in this state: %f mA\r\n", Power::getMax3V3Current());
+  Serial.printf("=========================================\r\n");
+}
+
+void Power::dumpConsumerStatistics() {
+  Serial.printf("======== Dezibot Consumer Statistics ========\r\n");
+  Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::ESP));
+  Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::WIFI));
+  Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT));
+  Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT));
+  Serial.printf("LED_RGB_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_BOTTOM));
+  Serial.printf("RGBW_SENSOR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
+  Serial.printf("LED_IR_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM));
+  Serial.printf("LED_IR_FRONT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
+  Serial.printf("PT_IR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
+  Serial.printf("PT_DL: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
+  Serial.printf("LED_UV: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
+  Serial.printf("DISPLAY_OLED: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
+  Serial.printf("MOTOR_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
+  Serial.printf("MOTOR_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
+  Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::IMU));
+  Serial.printf("=============================================\r\n");
 }
 
 int Power::latestSoCIndex = 0;
@@ -219,7 +258,7 @@ float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0};
 TickType_t Power::lastPowerStateUpdate = 0;
 float Power::coloumbsRemaining =
     PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
-int Power::percentRemaining = 100.0;
+float Power::percentRemaining = 100.0;
 PowerScheduler *Power::powerScheduler = nullptr;
 
 Power::Power() {}
\ No newline at end of file
diff --git a/src/power/Power.h b/src/power/Power.h
index daebe8d..90b3e2e 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -17,6 +17,34 @@ enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {
 
+protected:
+  /// @brief PowerScheduler instance to manage power consumption
+  static PowerScheduler *powerScheduler;
+
+  /*
+   * Power State
+   */
+
+  /// @brief last time of power state update
+  static TickType_t lastPowerStateUpdate;
+
+  /// @brief remaining Charge in coulombs
+  static float coloumbsRemaining;
+
+  /// @brief remaining Charge in percent
+  static float percentRemaining;
+
+  /// @brief Circular array of last calculated values for current state of
+  /// charge
+  static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
+  static int latestSoCIndex;
+
+  /// @brief Add calculated value to circular array, pushing out oldest value
+  static void addSoCSample(float soc);
+
+  /// @brief initialize the power state
+  static void initPowerState(void);
+
 public:
   static void begin(void);
   Power();
@@ -30,10 +58,11 @@ public:
   /// @brief Request an allowance of a certain number of milliamperes from the
   /// power scheduler without waiting for it (meaning it will not be scheduled
   /// for future allocation). Only one can be active per consumer.
-  /// @param neededCurrent the amount of current we want to be accounted for (in
-  /// mA)
+  /// @param neededCurrent the amount of current we want to be accounted for
+  /// (in mA)
   /// @return whether the current could be successfully allocated
-  static bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
+  static bool
+  tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                    uint16_t neededcurrent,
                                    uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
@@ -65,19 +94,18 @@ public:
   // @brief Get current consumption of a consumer
   static float getConsumerCurrent(PowerParameters::PowerConsumers consumer);
 
-
   /// @brief Get battery voltage measurement.
   /// @return Battery Terminal Voltage in Volts
   static float getBatteryVoltage();
 
   /// @brief Get estimated battery charge state as percentage
   /// @return Battery charge state in percent
-  static int getBatteryChargePercent();
+  static float getBatteryChargePercent();
 
   /// @brief Get estimated battery charge state as percentage based on
   // voltage directly
   /// @return Battery charge state in percent
-  static int getBatteryVoltageChargePercent();
+  static float getBatteryVoltageChargePercent();
 
   /// @brief Get estimated battery charge state as coulombs
   /// @return Battery charge state in coulombs
@@ -92,34 +120,11 @@ public:
   /// @note needs to be public for task creation
   static void updatePowerStateHandler();
 
-protected:
-  /// @brief PowerScheduler instance to manage power consumption
-  static PowerScheduler *powerScheduler;
+  /// @brief dump power statistics to serial
+  static void dumpPowerStatistics();
 
-
-  /*
-   * Power State
-   */
-
-  /// @brief last time of power state update
-  static TickType_t lastPowerStateUpdate;
-
-  /// @brief remaining Charge in coulombs
-  static float coloumbsRemaining;
-
-  /// @brief remaining Charge in percent
-  static int percentRemaining;
-
-  /// @brief Circular array of last calculated values for current state of
-  /// charge
-  static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
-  static int latestSoCIndex;
-
-  /// @brief Add calculated value to circular array, pushing out oldest value
-  static void addSoCSample(float soc);
-
-  /// @brief initialize the power state
-  static void initPowerState(void);
+  /// @brief dump consumer statistics to serial
+  static void dumpConsumerStatistics();
 };
 
 extern Power power;

From 51380ac692b43708d05a1244a068f5beb302731f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 21:29:53 +0100
Subject: [PATCH 29/59] Update Power Management Test

---
 .../Power_Management_Test.ino                 | 55 +++++++++++++++++++
 1 file changed, 55 insertions(+)
 create mode 100644 example/advanced/Power_Management_Test/Power_Management_Test.ino

diff --git a/example/advanced/Power_Management_Test/Power_Management_Test.ino b/example/advanced/Power_Management_Test/Power_Management_Test.ino
new file mode 100644
index 0000000..16cd0a7
--- /dev/null
+++ b/example/advanced/Power_Management_Test/Power_Management_Test.ino
@@ -0,0 +1,55 @@
+#include "Dezibot.h"
+#define HEADER_SERIAL
+#define HEADER_SERIAL_RX_PIN 16
+#define HEADER_SERIAL_TX_PIN 17
+
+Dezibot dezibot;
+
+void setup() {
+  // Initialize all modules separately so header stays unused
+  dezibot.multiColorLight.begin();
+  dezibot.motion.begin();
+  dezibot.infraredLight.begin();
+
+  #ifdef HEADER_SERIAL
+  Serial1.begin(115200, SERIAL_8N1, HEADER_SERIAL_RX_PIN, HEADER_SERIAL_TX_PIN);
+  #else
+  Serial1.begin(115200);
+  #endif
+  // Wait for Serial to init
+  while (!Serial) {
+    ;
+    ;
+  }
+  Serial1.println("Starting Power Management Test");
+  Serial1.println("Status on end of setup:");
+  #ifdef Power_h
+  dezibot.power.dumpPowerStatistics();
+  dezibot.power.dumpConsumerStatistics();
+  #endif
+}
+
+void dumpInfoAndWait() {
+  delay(100);
+  dezibot.power.dumpPowerStatistics();
+  dezibot.power.dumpConsumerStatistics();
+  delay(900);
+}
+
+void loop() {
+  dezibot.multiColorLight.setLed(ALL, 0x00FFFFFF);
+  Serial1.println("Turned on all RGB LEDs");
+  #ifdef Power_h
+  dumpInfoAndWait();
+  #endif
+  dezibot.motion.move();
+  Serial1.println("Turned on all motors");
+  #ifdef Power_h
+  dumpInfoAndWait();
+  #endif
+  dezibot.infraredLight.bottom.turnOn();
+  Serial1.println("Turned on bottom IR LEDs");
+  #ifdef Power_h
+  dumpInfoAndWait();
+  #endif
+}
\ No newline at end of file

From 4c13eb593e6c2279bae5fbd92e193542b998340d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 21:30:20 +0100
Subject: [PATCH 30/59] Remove left over debug prints

---
 src/motionDetection/MotionDetection.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 199d8fd..6d278c8 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -133,8 +133,8 @@ Direction MotionDetection::getTiltDirection(uint tolerance){
         return Flipped;
     }
     Orientation Rot = this->getTilt();
-    Serial.println(Rot.xRotation);
-    Serial.println(Rot.xRotation == INT_MAX);
+    /*Serial.println(Rot.xRotation);
+    Serial.println(Rot.xRotation == INT_MAX);*/
     if ((Rot.xRotation == INT_MAX)){
         return Error;
     }

From 598ac75e32677214e5eb78a42a78925967886f33 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 21:34:27 +0100
Subject: [PATCH 31/59] Change approach of notifying of power problem to be
 more beginner friendly

---
 src/communication/Communication.cpp     | 2 +-
 src/display/Display.cpp                 | 2 +-
 src/lightDetection/LightDetection.cpp   | 4 ++--
 src/motionDetection/MotionDetection.cpp | 2 +-
 src/multiColorLight/MultiColorLight.cpp | 2 +-
 src/power/Power.cpp                     | 2 +-
 6 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index 0335347..864c223 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -82,7 +82,7 @@ void Communication::begin(void) {
           PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
           MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
     ESP_LOGE(TAG, "Failed to get power for mesh initialization");
-    throw "Failed to get power for mesh initialization";
+    Serial.println("Failed to get power for mesh initialization");
   }
   mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
   mesh.onReceive(&receivedCallback);
diff --git a/src/display/Display.cpp b/src/display/Display.cpp
index bae4b6d..9451758 100644
--- a/src/display/Display.cpp
+++ b/src/display/Display.cpp
@@ -18,7 +18,7 @@ void Display::begin(void){
         PowerParameters::PowerConsumers::DISPLAY_OLED, PowerParameters::CurrentConsumptions::CURRENT_DISPLAY
         ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL)){
             ESP_LOGE(TAG,"Could not get power for Display");
-            throw "Could not get power for Display";
+            Serial.println("Could not get power for Display");
     }
     //set Mux Ratio
     sendDisplayCMD(muxRatio);
diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index b33588a..2d07cff 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -69,7 +69,7 @@ void LightDetection::beginInfrared(void){
         PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
         LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
             ESP_LOGE(TAG,"Could not get power for Infrared Phototransistors");
-            throw "Could not get power for Infrared Phototransistors";
+            Serial.println("Could not get power for Infrared Phototransistors");
     }
     digitalWrite(IR_PT_ENABLE,true);
     pinMode(IR_PT_ENABLE, OUTPUT);
@@ -85,7 +85,7 @@ void LightDetection::beginDaylight(void){
         PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
         LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
             ESP_LOGE(TAG,"Could not get power for Daylight Phototransistors");
-            throw "Could not get power for Daylight Phototransistors";
+            Serial.println("Could not get power for Daylight Phototransistors");
     }
     digitalWrite(DL_PT_ENABLE,true);
     pinMode(DL_PT_ENABLE, OUTPUT);
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 6d278c8..9872059 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -11,7 +11,7 @@ void MotionDetection::begin(void){
           PowerParameters::CurrentConsumptions::CURRENT_IMU,
           IMU_MAX_EXECUTION_DELAY_MS, NULL)) {
     ESP_LOGE(TAG, "Could not get power for MotionDetection");
-    throw "Could not get power for MotionDetection";
+    Serial.println("Could not get power for MotionDetection");
   }
     pinMode(34,OUTPUT);
     digitalWrite(34,HIGH);
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index a75c0a6..03759be 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -9,7 +9,7 @@ void MultiColorLight::begin(void){
     Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
     Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) ){
       ESP_LOGE(TAG, "Could not get power for MultiColorLight");
-      throw "Could not get power for MultiColorLight";
+      Serial.println("Could not get power for MultiColorLight");
     }
     rgbLeds.begin();
     this->turnOffLed();
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index dcd7eab..3525453 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -38,7 +38,7 @@ void Power::begin() {
             PowerParameters::PowerConsumers::ESP,
             PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
       ESP_LOGE(TAG, "Could not get power for ESP");
-      throw "Could not get power for ESP";
+      Serial.println("Could not get power for ESP");
     }
   }
 }

From c7e1af334f8e6b73e9d7f8187c89095b53c359bc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 22:56:10 +0100
Subject: [PATCH 32/59] Update test case

---
 .../Power_Management_Test.ino                 | 44 +++++++------------
 1 file changed, 17 insertions(+), 27 deletions(-)

diff --git a/example/advanced/Power_Management_Test/Power_Management_Test.ino b/example/advanced/Power_Management_Test/Power_Management_Test.ino
index 16cd0a7..a891ca5 100644
--- a/example/advanced/Power_Management_Test/Power_Management_Test.ino
+++ b/example/advanced/Power_Management_Test/Power_Management_Test.ino
@@ -1,32 +1,20 @@
 #include "Dezibot.h"
-#define HEADER_SERIAL
-#define HEADER_SERIAL_RX_PIN 16
-#define HEADER_SERIAL_TX_PIN 17
 
-Dezibot dezibot;
+std::Dezibot dezibot;
 
 void setup() {
-  // Initialize all modules separately so header stays unused
-  dezibot.multiColorLight.begin();
-  dezibot.motion.begin();
-  dezibot.infraredLight.begin();
-
-  #ifdef HEADER_SERIAL
-  Serial1.begin(115200, SERIAL_8N1, HEADER_SERIAL_RX_PIN, HEADER_SERIAL_TX_PIN);
-  #else
-  Serial1.begin(115200);
-  #endif
+  dezibot.begin();
+  Serial.begin(115200);
   // Wait for Serial to init
   while (!Serial) {
     ;
     ;
   }
-  Serial1.println("Starting Power Management Test");
-  Serial1.println("Status on end of setup:");
-  #ifdef Power_h
+  Serial.println("Starting Power Management Test");
+  Serial.println("Status on end of setup:");
+  
   dezibot.power.dumpPowerStatistics();
   dezibot.power.dumpConsumerStatistics();
-  #endif
 }
 
 void dumpInfoAndWait() {
@@ -38,18 +26,20 @@ void dumpInfoAndWait() {
 
 void loop() {
   dezibot.multiColorLight.setLed(ALL, 0x00FFFFFF);
-  Serial1.println("Turned on all RGB LEDs");
-  #ifdef Power_h
+  Serial.println("Turned on all RGB LEDs");
   dumpInfoAndWait();
-  #endif
+  
   dezibot.motion.move();
-  Serial1.println("Turned on all motors");
-  #ifdef Power_h
+  Serial.println("Turned on all motors");
   dumpInfoAndWait();
-  #endif
+  
   dezibot.infraredLight.bottom.turnOn();
-  Serial1.println("Turned on bottom IR LEDs");
-  #ifdef Power_h
+  Serial.println("Turned on bottom IR LEDs");
+  dumpInfoAndWait();
+  
+  dezibot.multiColorLight.turnOffLed(ALL);
+  dezibot.motion.stop();
+  dezibot.infraredLight.bottom.turnOff();
+  Serial.println("Turned off all LEDs and motors");
   dumpInfoAndWait();
-  #endif
 }
\ No newline at end of file

From 35c11f42e2d7eab28381d37f147c68650ec5193c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 22:57:23 +0100
Subject: [PATCH 33/59] Minor fixes

---
 src/communication/Communication.cpp     | 1 -
 src/motionDetection/MotionDetection.cpp | 4 ++--
 src/power/PowerScheduler.cpp            | 4 ++--
 3 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index 864c223..352e265 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -50,7 +50,6 @@ void vTaskUpdate(void *pvParameters) {
             PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
                 PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
             MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
-
       mesh.update();
     } else {
       ESP_LOGW(TAG, "Skipping mesh update after not being granted power");
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 9872059..9d83420 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -258,9 +258,9 @@ uint MotionDetection::getDataFromFIFO(FIFO_Package* buffer){
     fifocount = (fifohigh<<8)|fifolow;
     //fifocount |= this->readRegister(FIFO_COUNTL);
     //fifocount = (this->readRegister(FIFO_COUNTH)<<8);
-    Serial.println(fifolow);
+    /*Serial.println(fifolow);
     Serial.println(fifohigh);
-    Serial.println(fifocount);
+    Serial.println(fifocount);*/
     handler->beginTransaction(SPISettings(frequency,SPI_MSBFIRST,SPI_MODE0));
     digitalWrite(34,LOW);
     handler->transfer(cmdRead(FIFO_DATA));
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index e2acd1c..d0fb26e 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -139,12 +139,12 @@ bool PowerScheduler::waitForCurrentAllowance(
           break;
         }
       }
-      ESP_LOGI(TAG,
+      ESP_LOGW(TAG,
                "Task %p timed out waiting for %f mA of power;"
                "currently allocated: %f mA;"
                "total available (2C): %f mA",
                xTaskGetCurrentTaskHandle(), neededCurrent, getCurrentCurrent(),
-               maximumCurrent);
+               this->maximumCurrent);
       return false;
     } else {
       // Should be impossible to reach

From 0d6977e1484c033ecef27ab365946e101a4187b4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Thu, 13 Feb 2025 22:57:55 +0100
Subject: [PATCH 34/59] Improve power denial handling

---
 src/motion/Motion.cpp |  26 +++++++---
 src/motion/Motion.h   |   4 +-
 src/motion/Motor.cpp  | 117 +++++++++++++++++++++---------------------
 3 files changed, 81 insertions(+), 66 deletions(-)

diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp
index 2448f04..0db820c 100644
--- a/src/motion/Motion.cpp
+++ b/src/motion/Motion.cpp
@@ -31,8 +31,11 @@ void Motion::begin(void) {
 void Motion::moveTask(void * args) {
     uint32_t runtime = (uint32_t)args;
 
-    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
-    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+    if (!Motion::left.setSpeed(LEFT_MOTOR_DUTY) || !Motion::right.setSpeed(RIGHT_MOTOR_DUTY)) {
+        Serial.println("ailed to start motors due to power constraints");
+        Motion::stop();  // Use to clean up
+        return;
+    }
     Motion::xLastWakeTime = xTaskGetTickCount();
     while(1){
         if(runtime>40||runtime==0){
@@ -72,8 +75,11 @@ void Motion::moveTask(void * args) {
                 RIGHT_MOTOR_DUTY+=changerate;
             } 
 
-            Motion::left.setSpeed(LEFT_MOTOR_DUTY);
-            Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+            bool leftSpeedchangeSuccess = Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+            bool rightSpeedchangeSuccess = Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+            if (!leftSpeedchangeSuccess || !rightSpeedchangeSuccess) {
+                ESP_LOGW(TAG, "Failed to change motor speed due to power constraints");
+            }
         } else {
             vTaskDelayUntil(&xLastWakeTime,runtime);
             Motion::left.setSpeed(0);
@@ -116,7 +122,11 @@ void Motion::leftMotorTask(void * args) {
         xAntiClockwiseTaskHandle = NULL;
     }
     Motion::right.setSpeed(0);
-    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+    if(!Motion::left.setSpeed(LEFT_MOTOR_DUTY)){
+        Serial.println("Can not rotate Clockwise due to power constraints");
+        Motion::stop();
+        return;
+    }
     while(1){
         if((runtime>40)||(runtime==0)){
             vTaskDelayUntil(&xLastWakeTime,40);
@@ -155,7 +165,11 @@ void Motion::rightMotorTask(void * args) {
         vTaskDelete(xClockwiseTaskHandle);
         xClockwiseTaskHandle = NULL;
     }
-    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+    if(!Motion::left.setSpeed(RIGHT_MOTOR_DUTY)){
+        Serial.println("Can not rotate Antilockwise due to power constraints");
+        Motion::stop();
+        return;
+    }
     Motion::left.setSpeed(0);
     while(1){
         if(runtime>40||runtime==0){
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index fcc57b4..2c7c0bd 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -45,8 +45,10 @@ class Motor{
          * @attention it is requried at any time to use that method to access the motors or methods of the motionclass to avoid such peaks.
          * 
          * @param duty the duty cyle that should be set, can be between 0-8192
+         *
+         * @return true if the speed was set, false if the power was not granted in time
          */
-        void setSpeed(uint16_t duty);
+        bool setSpeed(uint16_t duty);
         
         /**
          * @brief returns the currently activ speed
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 74275f7..54b5bf6 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -1,72 +1,71 @@
 #include "Motion.h"
 #include "power/Power.h"
 
-#define MOTOR_LEFT_PIN  12
+#define MOTOR_LEFT_PIN 12
 #define MOTOR_RIGHT_PIN 11
 
-Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
-    this->pin = pin;
-    this->channel = channel;
-    this->timer = timer;
-    this->duty = 0;
+Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel) {
+  this->pin = pin;
+  this->channel = channel;
+  this->timer = timer;
+  this->duty = 0;
 };
 
-void Motor::begin(void){
-    pinMode(this->pin,OUTPUT);
-    ledc_channel_config_t channelConfig = {
-        .gpio_num       = this->pin,
-        .speed_mode     = LEDC_MODE,
-        .channel        = this->channel,
-        .intr_type      = LEDC_INTR_DISABLE,
-        .timer_sel      = this->timer,
-        .duty           = 0, // Set duty to 0%
-        .hpoint         = 0
-    };
-    ledc_channel_config(&channelConfig);
-    Serial.println("Motor begin done");
+void Motor::begin(void) {
+  pinMode(this->pin, OUTPUT);
+  ledc_channel_config_t channelConfig = {.gpio_num = this->pin,
+                                         .speed_mode = LEDC_MODE,
+                                         .channel = this->channel,
+                                         .intr_type = LEDC_INTR_DISABLE,
+                                         .timer_sel = this->timer,
+                                         .duty = 0, // Set duty to 0%
+                                         .hpoint = 0};
+  ledc_channel_config(&channelConfig);
+  Serial.println("Motor begin done");
 };
 
-void Motor::setSpeed(uint16_t duty){
-    const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
-    const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
-    if (this->pin == MOTOR_LEFT_PIN) {
-      if (!Power::waitForCurrentAllowance(
-              PowerParameters::PowerConsumers::MOTOR_LEFT, current,
-              MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
-        ESP_LOGW(TAG,
-                 "Power to set LEFT MOTOR to speed %d not granted in time. "
-                 "Skipping.",
-                 duty);
-      }
-    } else {
-      if (!Power::waitForCurrentAllowance(
-              PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
-              MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
-        ESP_LOGW(TAG,
-                 "Power to set RIGHT MOTOR to speed %d not granted in time. "
-                 "Skipping.",
-                 duty);
-      }
+bool Motor::setSpeed(uint16_t duty) {
+  const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
+  const float current =
+      PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
+  if (this->pin == MOTOR_LEFT_PIN) {
+    if (!Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::MOTOR_LEFT, current,
+            MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
+      ESP_LOGW(TAG,
+               "Power to set LEFT MOTOR to speed %d not granted in time. "
+               "Skipping.",
+               duty);
+      return false;
     }
-    int difference = duty-this->getSpeed();
-    if (difference > 0){
-        for(int i = 0;i<difference;i+=difference/20){
-            this->duty += difference/20;
-            ledc_set_duty(LEDC_MODE,this->channel,duty);
-            ledc_update_duty(LEDC_MODE,this->channel);
-            delayMicroseconds(5);
-        }
-    } else {
-        for(int i = 0;i>difference;i-=abs(difference/20)){
-            this->duty -= abs(difference/20);
-            ledc_set_duty(LEDC_MODE,this->channel,duty);
-            ledc_update_duty(LEDC_MODE,this->channel);
-            delayMicroseconds(5);
-        }
+  } else {
+    if (!Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
+            MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
+      ESP_LOGW(TAG,
+               "Power to set RIGHT MOTOR to speed %d not granted in time. "
+               "Skipping.",
+               duty);
+      return false;
     }
-
+  }
+  int difference = duty - this->getSpeed();
+  if (difference > 0) {
+    for (int i = 0; i < difference; i += difference / 20) {
+      this->duty += difference / 20;
+      ledc_set_duty(LEDC_MODE, this->channel, duty);
+      ledc_update_duty(LEDC_MODE, this->channel);
+      delayMicroseconds(5);
+    }
+  } else {
+    for (int i = 0; i > difference; i -= abs(difference / 20)) {
+      this->duty -= abs(difference / 20);
+      ledc_set_duty(LEDC_MODE, this->channel, duty);
+      ledc_update_duty(LEDC_MODE, this->channel);
+      delayMicroseconds(5);
+    }
+  }
+  return true;
 };
 
-uint16_t Motor::getSpeed(void){
-    return this->duty;
-};
+uint16_t Motor::getSpeed(void) { return this->duty; };

From 264e37c983fe393afcc02631ad9b40078b171c9f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 12:00:31 +0100
Subject: [PATCH 35/59] Add RAII-wrapped Mutexes for access to state-tracking
 variables

---
 src/Dezibot.cpp              |   1 -
 src/power/Power.cpp          | 148 +++++++++++++++++++++++++++--------
 src/power/Power.h            |  74 ++++++++++++++++--
 src/power/PowerScheduler.cpp |  75 ++++++++++++++----
 src/power/PowerScheduler.h   |  48 +++++++++++-
 5 files changed, 287 insertions(+), 59 deletions(-)

diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp
index d03f3bc..2576f9f 100644
--- a/src/Dezibot.cpp
+++ b/src/Dezibot.cpp
@@ -11,7 +11,6 @@ void Dezibot::begin(void)
 {
     ESP_LOGI("Dezibot", "Initializing Dezibot");
     power.begin();
-    delay(10);
     Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
     lightDetection.begin();
diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 3525453..45bd6ed 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -9,6 +9,8 @@
 
 #include "Power.h"
 
+SemaphoreHandle_t Power::powerMutex = NULL;
+
 void vTaskUpdatePowerState(void *pvParameters) {
   for (;;) {
     ESP_LOGV(TAG, "Updating Power State...");
@@ -22,6 +24,15 @@ void Power::begin() {
   // if not, we will do it.
   ESP_LOGI(TAG, "Initializing Power Management");
 
+  // Create mutex if it doesn't exist
+  if (powerMutex == NULL) {
+    powerMutex = xSemaphoreCreateMutex();
+    if (powerMutex == NULL) {
+      ESP_LOGE(TAG, "Failed to create power mutex");
+      Serial.println("Failed to create power mutex");
+    }
+  }
+
   if (powerScheduler == nullptr) {
     ESP_LOGI(TAG, "Creating Power Scheduler");
     powerScheduler = &PowerScheduler::getPowerScheduler(
@@ -149,29 +160,38 @@ void Power::updatePowerStateHandler() {
   int referenceCurrentMa =
       PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
 
+  float coloumbsConsumedSinceLastUpdate;
+
   // Calculate remaining battery charge in Coulombs based on current and time
-  float coloumbsConsumedSinceLastUpdate =
-      (currentCurrent / 1000) *
-      ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
-
-  // Update coloumbs remaining
-  coloumbsRemaining -= coloumbsConsumedSinceLastUpdate;
+  if (!busPowered) {
+    float coloumbsConsumedSinceLastUpdate =
+        (currentCurrent / 1000) *
+        ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
 
+    // Update coloumbs remaining
+    coloumbsRemaining -= coloumbsConsumedSinceLastUpdate;
+  }
   float chargeState;
 
   // If current flow is close enough to reference, get battery charge state via
   // voltage curve
-  if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
-      (currentCurrent < (referenceCurrentMa * 1.4))) {
-    // Get battery charge state from voltage curve
-    chargeState = getBatteryVoltageChargePercent();
+  if (!busPowered) {
+    if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
+        (currentCurrent < (referenceCurrentMa * 1.4))) {
+      // Get battery charge state from voltage curve
+      chargeState = getBatteryVoltageChargePercent();
+    } else {
+      // Estimate battery charge state from charge consumption
+      float oldChargeState = lastSOC[latestSoCIndex];
+      chargeState = oldChargeState -
+                    ((coloumbsConsumedSinceLastUpdate /
+                      PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
+                     100);
+    }
   } else {
-    // Estimate battery charge state from charge consumption
-    float oldChargeState = lastSOC[latestSoCIndex];
-    chargeState =
-        oldChargeState - ((coloumbsConsumedSinceLastUpdate /
-                           PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
-                          100);
+    // If we are charging, we can't estimate the charge state based on current
+    // consumption
+    chargeState = getBatteryVoltageChargePercent();
   }
 
   addSoCSample(chargeState);
@@ -190,10 +210,16 @@ void Power::updatePowerStateHandler() {
   powerScheduler->recalculateCurrentBudgets();
   ESP_LOGV(TAG, "Current: %f mA, Charge: %f Coulombs, %d %%", currentCurrent,
            coloumbsRemaining, percentRemaining);
+
+  // Update supply and charge state flags
+  busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS);
+  chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
+
   return;
 }
 
 float Power::getMax3V3Current() {
+  // Conversion from Thesis
   float u_bat = getBatteryVoltage();
   float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA;
   float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
@@ -202,6 +228,11 @@ float Power::getMax3V3Current() {
 }
 
 void Power::addSoCSample(float soc) {
+  PowerMutex lock(powerMutex);
+  if (!lock.isLocked()) {
+    ESP_LOGE(TAG, "Could not take power to add SoC sample");
+    return;
+  }
   latestSoCIndex =
       (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES;
   lastSOC[latestSoCIndex] = soc;
@@ -221,6 +252,11 @@ void Power::initPowerState(void) {
   constexpr float fullColoumbs =
       PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
   percentRemaining = initialChargePercentages;
+  // Set up flags and pins for them
+  pinMode(PowerParameters::PinConfig::VUSB_SENS, INPUT);
+  pinMode(PowerParameters::PinConfig::BAT_CHG_STAT, INPUT_PULLUP);
+  busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS);
+  chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
 }
 
 void Power::dumpPowerStatistics() {
@@ -228,31 +264,69 @@ void Power::dumpPowerStatistics() {
   Serial.printf("Current: %f mA\r\n", Power::getCurrentCurrent());
   Serial.printf("Battery Voltage: %f V\r\n", Power::getBatteryVoltage());
   Serial.printf("Battery Charge: %f %%\r\n", Power::getBatteryChargePercent());
-  Serial.printf("Battery Charge: %f Coulombs\r\n", Power::getBatteryChargeCoulombs());
-  Serial.printf("Max 3.3V Current in this state: %f mA\r\n", Power::getMax3V3Current());
+  Serial.printf("Battery Charge: %f Coulombs\r\n",
+                Power::getBatteryChargeCoulombs());
+  Serial.printf("Max 3.3V Current in this state (1C, 2C): %f mA, %f mA \r\n",
+                Power::getMax3V3Current(), Power::getMax3V3Current() * 2);
   Serial.printf("=========================================\r\n");
 }
 
 void Power::dumpConsumerStatistics() {
   Serial.printf("======== Dezibot Consumer Statistics ========\r\n");
-  Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::ESP));
-  Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::WIFI));
-  Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT));
-  Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT));
-  Serial.printf("LED_RGB_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_RGB_BOTTOM));
-  Serial.printf("RGBW_SENSOR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
-  Serial.printf("LED_IR_BOTTOM: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM));
-  Serial.printf("LED_IR_FRONT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
-  Serial.printf("PT_IR: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
-  Serial.printf("PT_DL: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
-  Serial.printf("LED_UV: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
-  Serial.printf("DISPLAY_OLED: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
-  Serial.printf("MOTOR_LEFT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
-  Serial.printf("MOTOR_RIGHT: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
-  Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent(PowerParameters::PowerConsumers::IMU));
+  Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent(
+                                      PowerParameters::PowerConsumers::ESP));
+  Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent(
+                                       PowerParameters::PowerConsumers::WIFI));
+  Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n",
+                Power::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT));
+  Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n",
+                Power::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT));
+  Serial.printf("LED_RGB_BOTTOM: %f mA\r\n",
+                Power::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_RGB_BOTTOM));
+  Serial.printf(
+      "RGBW_SENSOR: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
+  Serial.printf("LED_IR_BOTTOM: %f mA\r\n",
+                Power::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_IR_BOTTOM));
+  Serial.printf(
+      "LED_IR_FRONT: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
+  Serial.printf(
+      "PT_IR: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
+  Serial.printf(
+      "PT_DL: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
+  Serial.printf(
+      "LED_UV: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
+  Serial.printf(
+      "DISPLAY_OLED: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
+  Serial.printf(
+      "MOTOR_LEFT: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
+  Serial.printf(
+      "MOTOR_RIGHT: %f mA\r\n",
+      Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
+  Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent(
+                                      PowerParameters::PowerConsumers::IMU));
   Serial.printf("=============================================\r\n");
 }
 
+bool Power::isUSBPowered() { return busPowered; }
+
+bool Power::isBatteryPowered() { return !busPowered; }
+
+bool Power::isBatteryCharging() { return chargingState && busPowered; }
+
+bool Power::isBatteryDischarging() { return !chargingState && !busPowered; }
+
+bool Power::isBatteryFullyCharged() { return !chargingState && busPowered; }
 int Power::latestSoCIndex = 0;
 float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0};
 TickType_t Power::lastPowerStateUpdate = 0;
@@ -261,4 +335,10 @@ float Power::coloumbsRemaining =
 float Power::percentRemaining = 100.0;
 PowerScheduler *Power::powerScheduler = nullptr;
 
-Power::Power() {}
\ No newline at end of file
+bool Power::busPowered = false;
+
+bool Power::chargingState = false;
+
+Power::Power() {}
+
+Power::~Power() {}
\ No newline at end of file
diff --git a/src/power/Power.h b/src/power/Power.h
index 90b3e2e..343d078 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -8,6 +8,12 @@
  */
 
 #include "PowerScheduler.h"
+#include "esp_adc/adc_cali.h"
+#include "esp_adc/adc_cali_scheme.h"
+#include "driver/gpio.h"
+#include "driver/adc.h"
+#include "esp_adc/adc_oneshot.h"
+#include "freertos/semphr.h"
 #ifndef Power_h
 #define Power_h
 
@@ -17,6 +23,33 @@ enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {
 
+private:
+  static SemaphoreHandle_t powerMutex;
+  static constexpr uint16_t MUTEX_TIMEOUT_MS = 1;
+
+  // RAII for mutex
+  class PowerMutex {
+  private:
+    SemaphoreHandle_t &mutex;
+    bool locked;
+
+  public:
+    PowerMutex(SemaphoreHandle_t &mutex) : mutex(mutex), locked(false) {
+      locked =
+          (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) == pdTRUE);
+      if (!locked) {
+        ESP_LOGW(TAG, "Could not take power mutex");
+      }
+    }
+
+    ~PowerMutex() {
+      if (locked) {
+        xSemaphoreGive(mutex);
+      }
+    }
+    bool isLocked() { return locked; }
+  };
+
 protected:
   /// @brief PowerScheduler instance to manage power consumption
   static PowerScheduler *powerScheduler;
@@ -34,6 +67,10 @@ protected:
   /// @brief remaining Charge in percent
   static float percentRemaining;
 
+  static bool busPowered;
+
+  static bool chargingState;
+
   /// @brief Circular array of last calculated values for current state of
   /// charge
   static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];
@@ -48,7 +85,8 @@ protected:
 public:
   static void begin(void);
   Power();
-   /// @brief Get the current free current budget (to C1 discharge)
+  ~Power();
+  /// @brief Get the current free current budget (to C1 discharge)
   /// @return the amount of power that is currently available (in mA)
   static float getFreeLimitCurrentBudget(void);
   /// @brief Get the current hard maximum free current (to C2 discharge)
@@ -63,8 +101,8 @@ public:
   /// @return whether the current could be successfully allocated
   static bool
   tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                                   uint16_t neededcurrent,
-                                   uint16_t requestedDurationMs = 0);
+                              uint16_t neededcurrent,
+                              uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
   /// @param consumer the active consumer to release the current for
   static void releaseCurrent(PowerParameters::PowerConsumers consumer);
@@ -77,9 +115,9 @@ public:
   /// available
   /// @return whether the power could be successfully allocatedy
   static bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                               uint16_t neededCurrent,
-                               uint16_t maxSlackTimeMs,
-                               uint16_t requestedDurationMs);
+                                      uint16_t neededCurrent,
+                                      uint16_t maxSlackTimeMs,
+                                      uint16_t requestedDurationMs);
   /// @brief Put the ESP32 into deep sleep mode, without a method to wake up
   /// again. Basically this is a shutdown.
   static void beginPermanentDeepSleep(void);
@@ -88,7 +126,8 @@ public:
   /// @return the amount of power that is currently allocated (in mA)
   static float getCurrentCurrent(void);
 
-  // @brief Responsible for recalculating the current budgets
+  /// @brief Responsible for recalculating the current budgets
+  /// @note these change based on the current state of charge
   static void recalculateCurrentBudgets(void);
 
   // @brief Get current consumption of a consumer
@@ -125,6 +164,27 @@ public:
 
   /// @brief dump consumer statistics to serial
   static void dumpConsumerStatistics();
+
+  /// @brief get wether power is supplied via USB
+  /// @return true if power is supplied via USB
+  static bool isUSBPowered();
+
+  /// @brief get wether power is supplied via battery
+  /// @return true if power is supplied via battery
+  static bool isBatteryPowered();
+
+  /// @brief get wether the battery is currently charging
+  /// @return true if the battery is charging
+  static bool isBatteryCharging();
+
+  /// @brief get wether the battery is currently discharging
+  /// @return true if the battery is discharging
+  static bool isBatteryDischarging();
+
+  /// @brief get wether the battery is currently fully charged
+  /// @return true if the battery is fully charged
+  static bool isBatteryFullyCharged();
+
 };
 
 extern Power power;
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index d0fb26e..c82bd24 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -16,17 +16,22 @@
 bool PowerScheduler::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, float neededCurrent,
     uint16_t requestedDurationMs) {
-  float existingConsumption = getConsumerCurrent(consumer);
-  const bool currentAvailableBelowLimit =
+      float existingConsumption = getConsumerCurrent(consumer);
+      const bool currentAvailableBelowLimit =
       this->freeLimitCurrentBudget + existingConsumption > 0;
-  const bool currentAvailableBelowMaximum =
+      const bool currentAvailableBelowMaximum =
       this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
-  const bool currentIsInsignificant = neededCurrent < 1;
-  if (currentIsInsignificant ||
-      (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
+      const bool currentIsInsignificant = neededCurrent < 1;
+      if (currentIsInsignificant ||
+        (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
     if (existingConsumption > 0) {
       releaseCurrent(consumer);
     }
+    PowerSchedulerMutex lock(powerSchedulerMutex);
+    if (lock.isLocked() == false) {
+      ESP_LOGE(TAG, "Failed to Acquire PowerScheduler Mutex during Current Allocation");
+      return false;
+    }
     this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{
         .consumer = consumer,
         .maxSlackTimeMs = 0,
@@ -37,6 +42,7 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
         .grantedAt = xTaskGetTickCount(),
         .granted = true});
     this->recalculateCurrentBudgets();
+    lock.unlock();
     return true;
   } else {
     ESP_LOGI(TAG,
@@ -50,6 +56,10 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
 }
 
 void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) {
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return;
+  }
   for (auto it = currentAllowances.begin(); it != currentAllowances.end();
        ++it) {
     if (it->consumer == consumer) {
@@ -57,6 +67,7 @@ void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) {
       break;
     }
   }
+  lock.unlock();
   recalculateCurrentBudgets();
   // Check if there are tasks waiting for power
   checkWaitingTasks();
@@ -81,8 +92,12 @@ bool PowerScheduler::waitForCurrentAllowance(
         .neededCurrent = neededCurrent,
         .requestedAt = initialTickCount,
         .granted = false};
-
+    PowerSchedulerMutex lock(powerSchedulerMutex);
+    if (lock.isLocked() == false) {
+      return false;
+    }
     this->currentAllowances.push_back(newAllowance);
+    lock.unlock();
     uint32_t notificationValue;
     BaseType_t notificationStatus = xTaskNotifyWait(
         0, 0, &notificationValue, pdMS_TO_TICKS(maxSlackTimeMs));
@@ -103,8 +118,10 @@ bool PowerScheduler::waitForCurrentAllowance(
         const bool currentIsInsignificant = neededCurrent < 1;
         if (currentIsInsignificant ||
             (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
-          // TODO Check if there is a currently active allowance for this
-          // consumer and if so, replace it with the new one
+          PowerSchedulerMutex lock(powerSchedulerMutex);
+          if (lock.isLocked() == false) {
+            return false;
+          }
           if (existingConsumption > 0) {
             releaseCurrent(consumer);
           }
@@ -132,6 +149,10 @@ bool PowerScheduler::waitForCurrentAllowance(
     if (notificationStatus == pdFALSE) {
       // We waited long enough...
       // Remove the task from the list of waiting tasks
+      PowerSchedulerMutex lock(powerSchedulerMutex);
+      if (lock.isLocked() == false) {
+        return false;
+      }
       for (auto it = currentAllowances.begin(); it != currentAllowances.end();
            ++it) {
         if (it->consumer == consumer && it->requestedAt == initialTickCount) {
@@ -168,11 +189,16 @@ void PowerScheduler::checkWaitingTasks(void) {
 }
 
 void PowerScheduler::recalculateCurrentBudgets(void) {
+
   // Get the respective maximums and subtract currently flowing currents
   ESP_LOGI(TAG, "Recalculating current budgets...");
   float tempFreeLimitCurrentBudget = Power::getMax3V3Current();
   ESP_LOGI(TAG, "Got max 3V3 current: %.2f", tempFreeLimitCurrentBudget);
   float tempFreeMaximumCurrentBudget = Power::getMax3V3Current() * 2;
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return;
+  }
   for (auto &allowance : currentAllowances) {
     if (allowance.granted) {
       tempFreeLimitCurrentBudget -= allowance.neededCurrent;
@@ -187,6 +213,10 @@ void PowerScheduler::recalculateCurrentBudgets(void) {
 
 PowerScheduler::CurrentAllowance *
 PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return nullptr;
+  }
   for (auto &allowance : currentAllowances) {
     if (allowance.consumer == consumer) {
       return &allowance;
@@ -196,6 +226,10 @@ PowerScheduler::getCurrentAllowance(PowerParameters::PowerConsumers consumer) {
 }
 PowerScheduler::CurrentAllowance *
 PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return nullptr;
+  }
   for (auto &allowance : currentAllowances) {
     if (allowance.taskHandle == taskHandle) {
       return &allowance;
@@ -205,8 +239,12 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
 }
 PowerScheduler::CurrentAllowance *
 PowerScheduler::getNextExpiringAllowance(void) {
+  PowerSchedulerMutex lock(powerSchedulerMutex);
   TickType_t minTicks = UINT32_MAX;
   CurrentAllowance *nextAllowance = nullptr;
+  if (lock.isLocked() == false) {
+    return nullptr;
+  }
   for (auto &allowance : currentAllowances) {
     if (!(allowance.granted)) {
       TickType_t ticks =
@@ -224,17 +262,17 @@ PowerScheduler::getNextExpiringAllowance(void) {
 PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,
                                                   float i_max_ma) {
   if (powerSchedulerInstance == nullptr) {
-    // Double check locking
-    // https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
-    if (powerSchedulerInstance == nullptr) {
       powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma);
     }
-  }
   return *powerSchedulerInstance;
 }
 
 float PowerScheduler::getCurrentCurrent(void) {
   float currentSum = 0;
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return false;
+  }
   for (auto &allowance : currentAllowances) {
     if (allowance.granted) {
       currentSum += allowance.neededCurrent;
@@ -253,6 +291,10 @@ float PowerScheduler::getFreeMaximumCurrentBudget(void) {
 
 float PowerScheduler::getConsumerCurrent(
     PowerParameters::PowerConsumers consumer) {
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return false;
+  }
   float currentSum = 0;
   for (auto &allowance : currentAllowances) {
     if (allowance.consumer == consumer && allowance.granted) {
@@ -266,6 +308,11 @@ PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) {
   this->limitCurrent = i_limit_ma;
   this->maximumCurrent = i_max_ma;
   this->currentAllowances = std::vector<CurrentAllowance>();
+  this->powerSchedulerMutex = xSemaphoreCreateMutex();
 }
 
-PowerScheduler::~PowerScheduler() {}
\ No newline at end of file
+PowerScheduler::~PowerScheduler() {
+  if (powerSchedulerMutex != NULL) {
+    vSemaphoreDelete(powerSchedulerMutex);
+  }
+}
\ No newline at end of file
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index f3fad60..c32e6a7 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -24,8 +24,50 @@
 class PowerScheduler {
 private:
   static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100;
+  static constexpr uint16_t MUTEX_TIMEOUT_MS = 10;
+  SemaphoreHandle_t powerSchedulerMutex;
   PowerScheduler(float i_limit_ma, float i_max_ma);
 
+  // RAII for mutex
+  class PowerSchedulerMutex {
+  private:
+    SemaphoreHandle_t &mutex;
+    bool locked;
+
+  public:
+    PowerSchedulerMutex(SemaphoreHandle_t &mutex) : mutex(mutex), locked(false) {
+      locked =
+          (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) == pdTRUE);
+      if (!locked) {
+        ESP_LOGW(TAG, "Could not take powerScheduler mutex");
+      }
+    }
+
+    void unlock() {
+      if (locked) {
+        xSemaphoreGive(mutex);
+        locked = false;
+      }
+    }
+
+    void lock() {
+      if (!locked) {
+        locked = (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) ==
+                  pdTRUE);
+        if (!locked) {
+          ESP_LOGW(TAG, "Could not take power mutex");
+        }
+      }
+    }
+
+    ~PowerSchedulerMutex() {
+      if (locked) {
+        xSemaphoreGive(mutex);
+      }
+    }
+    bool isLocked() { return locked; }
+  };
+
 public:
   ~PowerScheduler();
   /// @brief Initialize the singleton instance of the power manager
@@ -45,9 +87,9 @@ public:
   /// @param neededCurrent the amount of current we want to be accounted for (in
   /// mA)
   /// @return whether the current could be successfully allocated
-  /// @note This takes existing power consumption by the same consumer into account,
-  /// so requesting the same or less power will always succeed. Also, small amounts
-  /// of power (below 1 mA) will always be granted.
+  /// @note This takes existing power consumption by the same consumer into
+  /// account, so requesting the same or less power will always succeed. Also,
+  /// small amounts of power (below 1 mA) will always be granted.
   bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                    float neededcurrent,
                                    uint16_t requestedDurationMs = 0);

From f753fdcc9b30f0de2afd982ab0a95f29f61218d2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 13:46:50 +0100
Subject: [PATCH 36/59] Add user-facing battery current calculation and fix
 logging

---
 src/power/Power.cpp          | 39 +++++++++++++++++++++++++++---------
 src/power/Power.h            |  8 ++++++--
 src/power/PowerScheduler.cpp |  2 +-
 3 files changed, 36 insertions(+), 13 deletions(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 45bd6ed..9a0fb82 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -89,6 +89,14 @@ float Power::getCurrentCurrent(void) {
   return powerScheduler->getCurrentCurrent();
 }
 
+float Power::getBatteryCurrent(void) {
+  const float i_3v3 = getCurrentCurrent();
+  const float u_3v3 = 3.3;
+  const float u_bat = getBatteryVoltage();
+  const float eta = PowerParameters::BUCK_BOOST_EFFICIENCY;
+  return (u_3v3 * i_3v3) / (u_bat * eta);
+}
+
 void Power::recalculateCurrentBudgets(void) {
   return powerScheduler->recalculateCurrentBudgets();
 }
@@ -156,6 +164,14 @@ float Power::getBatteryVoltageChargePercent() {
 }
 
 void Power::updatePowerStateHandler() {
+
+  // Update supply and charge state flags
+  Power::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS);
+  Power::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
+
+  ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState);
+
+
   float currentCurrent = powerScheduler->getCurrentCurrent();
   int referenceCurrentMa =
       PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
@@ -163,7 +179,7 @@ void Power::updatePowerStateHandler() {
   float coloumbsConsumedSinceLastUpdate;
 
   // Calculate remaining battery charge in Coulombs based on current and time
-  if (!busPowered) {
+  if (!Power::busPowered) {
     float coloumbsConsumedSinceLastUpdate =
         (currentCurrent / 1000) *
         ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
@@ -175,11 +191,12 @@ void Power::updatePowerStateHandler() {
 
   // If current flow is close enough to reference, get battery charge state via
   // voltage curve
-  if (!busPowered) {
+  if (!Power::busPowered) {
     if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
         (currentCurrent < (referenceCurrentMa * 1.4))) {
       // Get battery charge state from voltage curve
       chargeState = getBatteryVoltageChargePercent();
+      ESP_LOGD(TAG, "Charge state from voltage: %f", chargeState);
     } else {
       // Estimate battery charge state from charge consumption
       float oldChargeState = lastSOC[latestSoCIndex];
@@ -187,34 +204,36 @@ void Power::updatePowerStateHandler() {
                     ((coloumbsConsumedSinceLastUpdate /
                       PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) *
                      100);
+      ESP_LOGD(TAG, "Charge state from current: %f", chargeState);
     }
   } else {
     // If we are charging, we can't estimate the charge state based on current
     // consumption
     chargeState = getBatteryVoltageChargePercent();
+    ESP_LOGD(TAG, "Charge state from voltage (USB powered): %f", chargeState);
   }
 
   addSoCSample(chargeState);
 
+  for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
+    ESP_LOGD(TAG, "SoC[%d]: %f", i, lastSOC[i]);
+  }
+
   // Update percentage remaining based on charge state average
   float sampleSum = 0;
   for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
     sampleSum += lastSOC[i];
   }
-  percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
+  Power::percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
 
   // Update last update time
-  lastPowerStateUpdate = xTaskGetTickCount();
+  Power::lastPowerStateUpdate = xTaskGetTickCount();
 
   // Update the available current (changes based on battery state of charge)
-  powerScheduler->recalculateCurrentBudgets();
-  ESP_LOGV(TAG, "Current: %f mA, Charge: %f Coulombs, %d %%", currentCurrent,
+  Power::powerScheduler->recalculateCurrentBudgets();
+  ESP_LOGD(TAG, "Current: %f mA, Charge: %f Coulombs, %f %%", currentCurrent,
            coloumbsRemaining, percentRemaining);
 
-  // Update supply and charge state flags
-  busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS);
-  chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
-
   return;
 }
 
diff --git a/src/power/Power.h b/src/power/Power.h
index 343d078..8c25fd2 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -122,10 +122,14 @@ public:
   /// again. Basically this is a shutdown.
   static void beginPermanentDeepSleep(void);
 
-  //// @brief Get currently granted power
-  /// @return the amount of power that is currently allocated (in mA)
+  /// @brief Get currently granted current
+  /// @return the amount of current that is currently allocated (in mA)
   static float getCurrentCurrent(void);
 
+  /// @brief get the current theoretically flowing at the battery
+  /// @return the amount of current that is currently flowing (in mA)
+  static float getBatteryCurrent(void);
+
   /// @brief Responsible for recalculating the current budgets
   /// @note these change based on the current state of charge
   static void recalculateCurrentBudgets(void);
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index c82bd24..2166073 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -207,7 +207,7 @@ void PowerScheduler::recalculateCurrentBudgets(void) {
   }
   this->freeLimitCurrentBudget = tempFreeLimitCurrentBudget;
   this->freeMaximumCurrentBudget = tempFreeMaximumCurrentBudget;
-  ESP_LOGV(TAG, "Current budgets recalculated: %f mA, %f mA",
+  ESP_LOGV(TAG, "Current budgets recalculated: %d mA, %d mA",
            this->freeLimitCurrentBudget, this->freeMaximumCurrentBudget);
 }
 

From 2b57f300f25c8cf70511cdc53c9d7df5e998923f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 21:07:05 +0100
Subject: [PATCH 37/59] Fix VUSB_SENS input mode.

---
 src/power/Power.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 9a0fb82..3abf170 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -272,7 +272,7 @@ void Power::initPowerState(void) {
       PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
   percentRemaining = initialChargePercentages;
   // Set up flags and pins for them
-  pinMode(PowerParameters::PinConfig::VUSB_SENS, INPUT);
+  pinMode(PowerParameters::PinConfig::VUSB_SENS, INPUT_PULLUP);
   pinMode(PowerParameters::PinConfig::BAT_CHG_STAT, INPUT_PULLUP);
   busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS);
   chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);

From ef8a757772adf853fcb7721a5a7327a19b9b2969 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 21:08:03 +0100
Subject: [PATCH 38/59] Add power test cases

---
 .../Battery_discharge_logging.ino             | 29 ++++++++++
 .../Calculated_voltage_test.ino               | 56 +++++++++++++++++++
 .../Power-regulated_ColorCycle.ino            | 44 +++++++++++++++
 .../Sudden_Consumption_Jumps.ino}             | 15 +++--
 4 files changed, 139 insertions(+), 5 deletions(-)
 create mode 100644 example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino
 create mode 100644 example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino
 create mode 100644 example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino
 rename example/advanced/{Power_Management_Test/Power_Management_Test.ino => Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino} (84%)

diff --git a/example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino b/example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino
new file mode 100644
index 0000000..42fbb08
--- /dev/null
+++ b/example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino
@@ -0,0 +1,29 @@
+#include "Dezibot.h"
+// Output CSV-Data about the power state on secondary UART mapped to bottom
+// header
+
+// Using alternate Serial pins to not be powered by the USB port
+#define RXD_HEADER 16
+#define TXD_HEADER 17
+
+Dezibot dezibot = Dezibot();
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.begin();
+  Serial1.begin(9600, SERIAL_8N1, RXD_HEADER, TXD_HEADER);
+  Serial1.printf("Timestamp (ms),Current (mA),charge (%%),charge (C),voltage (V),isUSBPowered,isBatteryPowered,isBatteryCharging,isBatteryDischarging,isBatteryFullyCharged\r\n");
+}
+
+void loop() {
+  Serial1.printf("%d,%f,%f,%f,%f,%d,%d,%d,%d,%d\r\n",
+                 millis(),
+                 dezibot.power.getCurrentCurrent(),
+                 dezibot.power.getBatteryChargePercent(),
+                 dezibot.power.getBatteryChargeCoulombs(),
+                 dezibot.power.getBatteryVoltage(),
+                 dezibot.power.isUSBPowered(), dezibot.power.isBatteryPowered(),
+                 dezibot.power.isBatteryCharging(),
+                 dezibot.power.isBatteryDischarging(),
+                 dezibot.power.isBatteryFullyCharged());
+  sleep(1);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino b/example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino
new file mode 100644
index 0000000..cc48749
--- /dev/null
+++ b/example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino
@@ -0,0 +1,56 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.begin();
+}
+
+float i_3v3;
+float i_bat;
+float bat_chargePercent;
+float bat_coloumbs;
+float bat_voltage;
+bool isUSBPowered;
+bool isCharging;
+bool isFullyCharged;
+
+char i3v3_line[26];
+char ibat_line[26];
+char bat_chargePercent_line[26];
+char bat_coloumbs_line[26];
+char bat_voltage_line[26];
+char isUSBPowered_line[26];
+char isCharging_line[26];
+char isFullyCharged_line[26];
+
+void loop() {
+  i_3v3 = dezibot.power.getCurrentCurrent();
+  i_bat = dezibot.power.getBatteryCurrent();
+  bat_chargePercent = dezibot.power.getBatteryChargePercent();
+  bat_coloumbs = dezibot.power.getBatteryChargeCoulombs();
+  bat_voltage = dezibot.power.getBatteryVoltage();
+  isUSBPowered = dezibot.power.isUSBPowered();
+  isCharging = dezibot.power.isBatteryCharging();
+  isFullyCharged = dezibot.power.isBatteryFullyCharged();
+
+  sprintf(i3v3_line, "i_3v3: %.1f mA", i_3v3);
+  sprintf(ibat_line, "i_bat: %.1f mA", i_bat);
+  sprintf(bat_chargePercent_line, "CHG: %.2f %%", bat_chargePercent);
+  sprintf(bat_coloumbs_line, "CHG: %.2f C", bat_coloumbs);
+  sprintf(bat_voltage_line, "U: %.2f3V", bat_voltage);
+  sprintf(isUSBPowered_line, "USBPower: %d", isUSBPowered);
+  sprintf(isCharging_line, "Charging: %d", isCharging);
+  sprintf(isFullyCharged_line, "Full: %d", isFullyCharged);
+
+  dezibot.display.clear();
+  dezibot.display.println(i3v3_line);
+  dezibot.display.println(ibat_line);
+  dezibot.display.println(bat_chargePercent_line);
+  dezibot.display.println(bat_coloumbs_line);
+  dezibot.display.println(bat_voltage_line);
+  dezibot.display.println(isUSBPowered_line);
+  dezibot.display.println(isCharging_line);
+  dezibot.display.println(isFullyCharged_line);
+  delay(1000);
+}
\ No newline at end of file
diff --git a/example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino b/example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino
new file mode 100644
index 0000000..3c18529
--- /dev/null
+++ b/example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino
@@ -0,0 +1,44 @@
+#include "Dezibot.h"
+
+/*
+ * This serves as an example of how to use the power management system deals with gradual changes.
+ * The ColorCycle example is a good one, because it contains gradual overstepping of the power budget,
+ * with the effect being easily visible.
+ */
+
+Dezibot dezibot = Dezibot();
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.begin();
+  Serial.begin(115200);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  for (int d = 0; d < 255; d++) {
+    dezibot.multiColorLight.setLed(
+        ALL, dezibot.multiColorLight.color(d, 0, 255 - d));
+    delay(2);
+    Serial.printf("current: %f, battery: %f\r\n",
+                  dezibot.power.getCurrentCurrent(),
+                  dezibot.power.getBatteryChargePercent());
+  }
+
+  for (int d = 0; d < 255; d++) {
+    dezibot.multiColorLight.setLed(
+        ALL, dezibot.multiColorLight.color(255 - d, d, 0));
+    delay(2);
+    Serial.printf("current: %f, battery: %f\r\n",
+                  dezibot.power.getCurrentCurrent(),
+                  dezibot.power.getBatteryChargePercent());
+  }
+
+  for (int d = 0; d < 255; d++) {
+    dezibot.multiColorLight.setLed(
+        ALL, dezibot.multiColorLight.color(0, 255 - d, d));
+    delay(2);
+    Serial.printf("current: %f, battery: %f\r\n",
+                  dezibot.power.getCurrentCurrent(),
+                  dezibot.power.getBatteryChargePercent());
+  }
+}
diff --git a/example/advanced/Power_Management_Test/Power_Management_Test.ino b/example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino
similarity index 84%
rename from example/advanced/Power_Management_Test/Power_Management_Test.ino
rename to example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino
index a891ca5..2bc2725 100644
--- a/example/advanced/Power_Management_Test/Power_Management_Test.ino
+++ b/example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino
@@ -1,6 +1,11 @@
 #include "Dezibot.h"
 
-std::Dezibot dezibot;
+/*
+ * This serves as an example of how to use the power management system deals with huge requests
+ * for power on an already heavily loaded system.
+ */
+
+Dezibot dezibot;
 
 void setup() {
   dezibot.begin();
@@ -12,7 +17,7 @@ void setup() {
   }
   Serial.println("Starting Power Management Test");
   Serial.println("Status on end of setup:");
-  
+
   dezibot.power.dumpPowerStatistics();
   dezibot.power.dumpConsumerStatistics();
 }
@@ -28,15 +33,15 @@ void loop() {
   dezibot.multiColorLight.setLed(ALL, 0x00FFFFFF);
   Serial.println("Turned on all RGB LEDs");
   dumpInfoAndWait();
-  
+
   dezibot.motion.move();
   Serial.println("Turned on all motors");
   dumpInfoAndWait();
-  
+
   dezibot.infraredLight.bottom.turnOn();
   Serial.println("Turned on bottom IR LEDs");
   dumpInfoAndWait();
-  
+
   dezibot.multiColorLight.turnOffLed(ALL);
   dezibot.motion.stop();
   dezibot.infraredLight.bottom.turnOff();

From e6c7454e5b41d37e54d0c29d101b2f290bb758a1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 21:08:37 +0100
Subject: [PATCH 39/59] Replace discharge curve with measured one.

---
 src/power/PowerParameters.h | 191 ++++++++++++++++++------------------
 1 file changed, 96 insertions(+), 95 deletions(-)

diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index 6da3bd9..c895b65 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -14,105 +14,106 @@
 
 namespace PowerParameters {
 
-  struct Battery {
-    // Datasheet values
-    static constexpr float CELL_CAPACITY_MAH = 120;
-    static constexpr float CELL_VOLTAGE_NOMINAL = 3.7;
-    struct DISCHARGE_CURVE {
-      static constexpr float REFERENCE_CURRENT_A = 0.05;
-      static constexpr int NUM_POINTS = 22;
-      static constexpr float VOLTAGES[NUM_POINTS] = {
-          4.15, 4.05,  3.97, 3.9,   3.84, 3.795, 3.765, 3.74, 3.72, 3.7,  3.69,
-          3.68, 3.675, 3.67, 3.665, 3.66, 3.64,  3.61,  3.55, 3.45, 3.25, 3.0};
-      static constexpr int CHARGE_STATES[NUM_POINTS] = {
-          100, 95, 90, 85, 80, 75, 70, 65, 60, 55, 50,
-          45,  40, 35, 30, 25, 20, 15, 10, 5,  0,  -5};
-    };
-
-    // Derived values
-    static constexpr float CELL_CHARGE_FULL_COLOUMB = CELL_CAPACITY_MAH * 3.6;
-    static constexpr float CELL_ENERGY_FULL_JOULES =
-        CELL_CAPACITY_MAH * CELL_VOLTAGE_NOMINAL * 3.6;
-    static constexpr float CELL_CURRENT_1C_MA = CELL_CAPACITY_MAH;
-    static constexpr float CELL_CURRENT_2C_MA = CELL_CAPACITY_MAH * 2;
-    struct BAT_ADC {
-      static constexpr float VOLTAGE_DIVIDER_R12 = 27e3;
-      static constexpr float VOLTAGE_DIVIDER_R13 = 10e3;
-      static constexpr float VOLTAGE_DIVIDER_FACTOR =
-          (VOLTAGE_DIVIDER_R12 + VOLTAGE_DIVIDER_R13) / VOLTAGE_DIVIDER_R13;
-    };
-
-    // Configuration
-    static constexpr int AVERAGING_SAMPLES = 10;
+struct Battery {
+  // Datasheet values
+  static constexpr float CELL_CAPACITY_MAH = 120;
+  static constexpr float CELL_VOLTAGE_NOMINAL = 3.7;
+  struct DISCHARGE_CURVE {
+    static constexpr float REFERENCE_CURRENT_A = 0.063925;
+    static constexpr int NUM_POINTS = 22;
+    static constexpr float VOLTAGES[NUM_POINTS] = {
+        3.7426, 3.6110, 3.5621, 3.5027, 3.4826, 3.4391, 3.4005,
+        3.3674, 3.3387, 3.3137, 3.2846, 3.2400, 3.2212, 3.1949,
+        3.1749, 3.1575, 3.1148, 3.0967, 3.0234, 2.9689, 2.8903};
+    static constexpr int CHARGE_STATES[NUM_POINTS] = {
+        100, 95, 90, 85, 80, 75, 70, 65, 60, 55, 50,
+        45,  40, 35, 30, 25, 20, 15, 10, 5,  0};
   };
 
-  // Factors concerning Buck-Boost-Converter
-  static constexpr float BUCK_BOOST_EFFICIENCY = 0.9;
-
-  /*
-   * The current consumptions in milliamperes of the different components are
-   * defined here. These values are measured on 3,3 Volts, and need to be
-   * converted to currents actually occuring at the battery.
-   */
-  struct CurrentConsumptions {
-    static constexpr float CURRENT_ESP_BASE = 37.42;
-    static constexpr float CURRENT_ESP_LOAD = 88.43;
-    static constexpr float CURRENT_ESP_AVG =
-        (CURRENT_ESP_BASE + CURRENT_ESP_LOAD) / 2;
-    // WiFi current consumptions
-    static constexpr float CURRENT_WIFI_BASE = 64.58;
-    static constexpr float CURRENT_WIFI_PEAK = 128;
-    // RGB LED quiescent current
-    static constexpr float CURRENT_LED_RGB_BASE = 0.7;
-    // RGB LED per channel current during PWM on-time.
-    static constexpr float CURRENT_LED_RGB_CHAN_T_ON = 16;
-    static constexpr float CURRENT_SENSOR_RGBW = 0.2;
-    static constexpr float CURRENT_LED_IR_BOTTOM = 100;
-    static constexpr float CURRENT_LED_IR_FRONT = 180.7;
-    // Phototransistor current when active and illuminated.
-    static constexpr float CURRENT_PT = 0.33005;
-    // Average value, as internal behaviour can not non-expensively and
-    // accurately be observed from code.
-    static constexpr float CURRENT_DISPLAY = 9;
-    // Per motor current during PWM on-time.
-    static constexpr float CURRENT_MOTOR_T_ON = 130;
-    // Current of IMU when activated.
-    static constexpr float CURRENT_IMU = 0.55;
-    // LED Current. Placeholder.
-    static constexpr float CURRENT_UV_LED = 200;
+  // Derived values
+  static constexpr float CELL_CHARGE_FULL_COLOUMB = CELL_CAPACITY_MAH * 3.6;
+  static constexpr float CELL_ENERGY_FULL_JOULES =
+      CELL_CAPACITY_MAH * CELL_VOLTAGE_NOMINAL * 3.6;
+  static constexpr float CELL_CURRENT_1C_MA = CELL_CAPACITY_MAH;
+  static constexpr float CELL_CURRENT_2C_MA = CELL_CAPACITY_MAH * 2;
+  struct BAT_ADC {
+    static constexpr float VOLTAGE_DIVIDER_R12 = 27e3;
+    static constexpr float VOLTAGE_DIVIDER_R13 = 10e3;
+    static constexpr float VOLTAGE_DIVIDER_FACTOR =
+        (VOLTAGE_DIVIDER_R12 + VOLTAGE_DIVIDER_R13) / VOLTAGE_DIVIDER_R13;
   };
 
-  /*
-   * Single consumer current limit up to which requests are granted no matter what.
-   * The idea is, that this will allow Sensors (with their miniscule power draw)
-   * to always be granted power, which should massively improve behaviour.
-   */
-  static constexpr float CURRENT_INSIGNIFICANT = 1;
-
-  struct PinConfig {
-    static constexpr int BAT_ADC = 10;
-    static constexpr int BAT_ADC_EN = 9;
-    static constexpr int VUSB_SENS = 38;
-    static constexpr int BAT_CHG_STAT = 39;
-  };
-
-  enum PowerConsumers {
-    ESP,
-    WIFI,
-    LED_RGB_TOP_LEFT,
-    LED_RGB_TOP_RIGHT,
-    LED_RGB_BOTTOM,
-    RGBW_SENSOR,
-    LED_IR_BOTTOM,
-    LED_IR_FRONT,
-    PT_IR,
-    PT_DL,
-    LED_UV,
-    DISPLAY_OLED,
-    MOTOR_LEFT,
-    MOTOR_RIGHT,
-    IMU
-  };
+  // Configuration
+  static constexpr int AVERAGING_SAMPLES = 10;
 };
 
+// Factors concerning Buck-Boost-Converter
+static constexpr float BUCK_BOOST_EFFICIENCY = 0.9;
+
+/*
+ * The current consumptions in milliamperes of the different components are
+ * defined here. These values are measured on 3,3 Volts, and need to be
+ * converted to currents actually occuring at the battery.
+ */
+struct CurrentConsumptions {
+  static constexpr float CURRENT_ESP_BASE = 37.42;
+  static constexpr float CURRENT_ESP_LOAD = 88.43;
+  static constexpr float CURRENT_ESP_AVG =
+      (CURRENT_ESP_BASE + CURRENT_ESP_LOAD) / 2;
+  // WiFi current consumptions
+  static constexpr float CURRENT_WIFI_BASE = 64.58;
+  static constexpr float CURRENT_WIFI_PEAK = 128;
+  // RGB LED quiescent current
+  static constexpr float CURRENT_LED_RGB_BASE = 0.7;
+  // RGB LED per channel current during PWM on-time.
+  static constexpr float CURRENT_LED_RGB_CHAN_T_ON = 16;
+  static constexpr float CURRENT_SENSOR_RGBW = 0.2;
+  static constexpr float CURRENT_LED_IR_BOTTOM = 100;
+  static constexpr float CURRENT_LED_IR_FRONT = 180.7;
+  // Phototransistor current when active and illuminated.
+  static constexpr float CURRENT_PT = 0.33005;
+  // Average value, as internal behaviour can not non-expensively and
+  // accurately be observed from code.
+  static constexpr float CURRENT_DISPLAY = 9;
+  // Per motor current during PWM on-time.
+  static constexpr float CURRENT_MOTOR_T_ON = 130;
+  // Current of IMU when activated.
+  static constexpr float CURRENT_IMU = 0.55;
+  // LED Current. Placeholder.
+  static constexpr float CURRENT_UV_LED = 200;
+};
+
+/*
+ * Single consumer current limit up to which requests are granted no matter
+ * what. The idea is, that this will allow Sensors (with their miniscule power
+ * draw) to always be granted power, which should massively improve behaviour.
+ */
+static constexpr float CURRENT_INSIGNIFICANT = 1;
+
+struct PinConfig {
+  static constexpr int BAT_ADC = 10;
+  static constexpr int BAT_ADC_EN = 9;
+  static constexpr int VUSB_SENS = 38;
+  static constexpr int BAT_CHG_STAT = 39;
+};
+
+enum PowerConsumers {
+  ESP,
+  WIFI,
+  LED_RGB_TOP_LEFT,
+  LED_RGB_TOP_RIGHT,
+  LED_RGB_BOTTOM,
+  RGBW_SENSOR,
+  LED_IR_BOTTOM,
+  LED_IR_FRONT,
+  PT_IR,
+  PT_DL,
+  LED_UV,
+  DISPLAY_OLED,
+  MOTOR_LEFT,
+  MOTOR_RIGHT,
+  IMU
+};
+}; // namespace PowerParameters
+
 #endif // Consumptions_h
\ No newline at end of file

From 83674657b87c6cf229635a5685a673ee4f3308d1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 21:09:04 +0100
Subject: [PATCH 40/59] More processing of charge state

---
 src/power/Power.cpp | 24 ++++++++++++++++++------
 src/power/Power.h   |  2 ++
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/src/power/Power.cpp b/src/power/Power.cpp
index 3abf170..09ce96c 100644
--- a/src/power/Power.cpp
+++ b/src/power/Power.cpp
@@ -132,9 +132,9 @@ float Power::getBatteryChargePercent() { return percentRemaining; }
 float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }
 
 float Power::getBatteryVoltageChargePercent() {
-  // Directly get the battery voltage and calculate the charge state based on
-  // the discharge curve.
-  float batteryVoltage = getBatteryVoltage();
+  // Directly get the battery voltage, correct the curve with an offset and
+  // calculate the charge state based on the discharge curve.
+  float batteryVoltage = getBatteryVoltage() + Power::fullVoltageOffset;
   float chargeState = 0;
   // Clamp edge cases
   if (batteryVoltage >=
@@ -169,8 +169,13 @@ void Power::updatePowerStateHandler() {
   Power::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS);
   Power::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
 
-  ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState);
+  // If the battery is charging and fully charged
+  if (Power::busPowered && !Power::chargingState) {
+    // Calibrate voltage offset on full Battery
+    Power::fullVoltageOffset = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0] - getBatteryVoltage();
+  }
 
+  ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState);
 
   float currentCurrent = powerScheduler->getCurrentCurrent();
   int referenceCurrentMa =
@@ -180,7 +185,7 @@ void Power::updatePowerStateHandler() {
 
   // Calculate remaining battery charge in Coulombs based on current and time
   if (!Power::busPowered) {
-    float coloumbsConsumedSinceLastUpdate =
+    coloumbsConsumedSinceLastUpdate =
         (currentCurrent / 1000) *
         ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
 
@@ -213,6 +218,10 @@ void Power::updatePowerStateHandler() {
     ESP_LOGD(TAG, "Charge state from voltage (USB powered): %f", chargeState);
   }
 
+  // Bound value between 0 and 100
+  chargeState = chargeState > 100 ? 100 : chargeState;
+  chargeState = chargeState < 0 ? 0 : chargeState;
+
   addSoCSample(chargeState);
 
   for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
@@ -224,7 +233,8 @@ void Power::updatePowerStateHandler() {
   for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
     sampleSum += lastSOC[i];
   }
-  Power::percentRemaining = sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
+  Power::percentRemaining =
+      sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
 
   // Update last update time
   Power::lastPowerStateUpdate = xTaskGetTickCount();
@@ -358,6 +368,8 @@ bool Power::busPowered = false;
 
 bool Power::chargingState = false;
 
+float Power::fullVoltageOffset = 0;
+
 Power::Power() {}
 
 Power::~Power() {}
\ No newline at end of file
diff --git a/src/power/Power.h b/src/power/Power.h
index 8c25fd2..34be7cd 100644
--- a/src/power/Power.h
+++ b/src/power/Power.h
@@ -71,6 +71,8 @@ protected:
 
   static bool chargingState;
 
+  static float fullVoltageOffset;
+
   /// @brief Circular array of last calculated values for current state of
   /// charge
   static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES];

From 5cb25a412aa86bed017e013770290057e11bf72b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Fri, 14 Feb 2025 21:19:36 +0100
Subject: [PATCH 41/59] Increase voltage moving average filter window from 10
 values to 20 values

---
 src/power/PowerParameters.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index c895b65..bf16281 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -44,7 +44,7 @@ struct Battery {
   };
 
   // Configuration
-  static constexpr int AVERAGING_SAMPLES = 10;
+  static constexpr int AVERAGING_SAMPLES = 20;
 };
 
 // Factors concerning Buck-Boost-Converter

From ff80ebe4db1dd4ae053a79efde8424e8612e88c9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sat, 15 Feb 2025 21:42:41 +0100
Subject: [PATCH 42/59] Add power Modeling functions to all components

---
 src/colorDetection/ColorDetection.cpp   | 172 ++++++------
 src/colorDetection/ColorDetection.h     |  23 +-
 src/display/Display.cpp                 |   8 +
 src/display/Display.h                   |  13 +
 src/infraredLight/InfraredLED.cpp       |  51 +++-
 src/infraredLight/InfraredLight.h       |  27 +-
 src/lightDetection/LightDetection.h     |  18 +-
 src/motion/Motion.h                     |  18 ++
 src/motion/Motor.cpp                    |  13 +-
 src/motionDetection/MotionDetection.cpp |  11 +-
 src/motionDetection/MotionDetection.h   |  16 ++
 src/multiColorLight/MultiColorLight.cpp | 339 +++++++++++++++---------
 src/multiColorLight/MultiColorLight.h   | 297 +++++++++++++--------
 13 files changed, 672 insertions(+), 334 deletions(-)

diff --git a/src/colorDetection/ColorDetection.cpp b/src/colorDetection/ColorDetection.cpp
index b350760..eddbaf2 100644
--- a/src/colorDetection/ColorDetection.cpp
+++ b/src/colorDetection/ColorDetection.cpp
@@ -1,91 +1,103 @@
-#include  "ColorDetection.h"
+#include "ColorDetection.h"
 
-void ColorDetection::begin(void){
-  if(!Power::waitForCurrentAllowance(
-      PowerParameters::PowerConsumers::RGBW_SENSOR,
-      PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW,
-      COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)){
-        ESP_LOGE(TAG,"Could not get power for ColorDetection");
-        throw "Could not get power for ColorDetection";
+void ColorDetection::begin(void) {
+  if (!Power::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::RGBW_SENSOR,
+          PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW,
+          COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Could not get power for ColorDetection");
+    throw "Could not get power for ColorDetection";
   }
   ColorDetection::configure(
       VEML_CONFIG{.mode = AUTO, .enabled = true, .exposureTime = MS40});
 };
-void ColorDetection::configure(VEML_CONFIG config){
-     uint8_t configRegister = 0;
-    switch(config.exposureTime)
-    {
-        case MS40:
-            configRegister = 0x00;break;
-        case MS80:
-            configRegister = 0x01;break;
-        case MS160:
-            configRegister = 0x02;break;
-        case MS320:
-            configRegister = 0x03;break;
-        case MS640:
-            configRegister = 0x04;break;
-        case MS1280:
-            configRegister = 0x05;break;
-    }
-    configRegister = configRegister << 4;
-    if(config.mode == MANUAL)
-    {
-        configRegister = configRegister | (0x01<<1);
-    }
-    if(!config.enabled)
-    {
-        configRegister = configRegister | 1;
-    }
-    ColorDetection::writeDoubleRegister(CMD_CONFIG,(uint16_t)configRegister);
+void ColorDetection::configure(VEML_CONFIG config) {
+  uint8_t configRegister = 0;
+  switch (config.exposureTime) {
+  case MS40:
+    configRegister = 0x00;
+    break;
+  case MS80:
+    configRegister = 0x01;
+    break;
+  case MS160:
+    configRegister = 0x02;
+    break;
+  case MS320:
+    configRegister = 0x03;
+    break;
+  case MS640:
+    configRegister = 0x04;
+    break;
+  case MS1280:
+    configRegister = 0x05;
+    break;
+  }
+  configRegister = configRegister << 4;
+  if (config.mode == MANUAL) {
+    configRegister = configRegister | (0x01 << 1);
+  }
+  if (!config.enabled) {
+    configRegister = configRegister | 1;
+  }
+  ColorDetection::writeDoubleRegister(CMD_CONFIG, (uint16_t)configRegister);
 };
 
-uint16_t ColorDetection::getColorValue(color color){
-   
-    switch(color)
-    {
-        case VEML_RED:
-            return readDoubleRegister(REG_RED); 
-            break;
-        case VEML_GREEN:
-            return readDoubleRegister(REG_GREEN);
-            break;
-        case VEML_BLUE: 
-            return readDoubleRegister(REG_BLUE);
-            break;
-        case VEML_WHITE:
-            return readDoubleRegister(REG_WHITE);
-            break;
-        default:
-            Serial.println("Color is not supported by the sensor");
-            return 0;
-    } 
+uint16_t ColorDetection::getColorValue(color color) {
+
+  switch (color) {
+  case VEML_RED:
+    return readDoubleRegister(REG_RED);
+    break;
+  case VEML_GREEN:
+    return readDoubleRegister(REG_GREEN);
+    break;
+  case VEML_BLUE:
+    return readDoubleRegister(REG_BLUE);
+    break;
+  case VEML_WHITE:
+    return readDoubleRegister(REG_WHITE);
+    break;
+  default:
+    Serial.println("Color is not supported by the sensor");
+    return 0;
+  }
 };
 
-uint16_t ColorDetection::readDoubleRegister(uint8_t regAddr){
-    uint16_t result = 0;
-    Wire.beginTransmission(VEML_ADDR);
-    Wire.write(regAddr);
-    if(Wire.endTransmission() != 0){
-        Serial.printf("Reading Register %d failed",regAddr);
-    }
-    Wire.requestFrom(VEML_ADDR,2);
-    uint8_t offset = 0;
-    while(Wire.available()){
-        result = result << 8;
-        result = result | (Wire.read()<<offset);
-        offset = offset + 8;
-    }
-    return result;
+uint16_t ColorDetection::readDoubleRegister(uint8_t regAddr) {
+  uint16_t result = 0;
+  Wire.beginTransmission(VEML_ADDR);
+  Wire.write(regAddr);
+  if (Wire.endTransmission() != 0) {
+    Serial.printf("Reading Register %d failed", regAddr);
+  }
+  Wire.requestFrom(VEML_ADDR, 2);
+  uint8_t offset = 0;
+  while (Wire.available()) {
+    result = result << 8;
+    result = result | (Wire.read() << offset);
+    offset = offset + 8;
+  }
+  return result;
 };
 
-void ColorDetection::writeDoubleRegister(uint8_t regAddr, uint16_t data){
-    //erst low dann high
-    Wire.beginTransmission(VEML_ADDR);
-    Wire.write(regAddr);
-    Wire.write((uint8_t)(data&0x00FF));
-    Wire.write((uint8_t)((data>>8)&0x00FF));
-    if(Wire.endTransmission() != 0){
-        Serial.printf("Reading Register %d failed",regAddr);
-    }
-};
\ No newline at end of file
+void ColorDetection::writeDoubleRegister(uint8_t regAddr, uint16_t data) {
+  // erst low dann high
+  Wire.beginTransmission(VEML_ADDR);
+  Wire.write(regAddr);
+  Wire.write((uint8_t)(data & 0x00FF));
+  Wire.write((uint8_t)((data >> 8) & 0x00FF));
+  if (Wire.endTransmission() != 0) {
+    Serial.printf("Reading Register %d failed", regAddr);
+  }
+};
+
+
+float ColorDetection::modelCurrentConsumption() {
+  return PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW;
+};
+
+float ColorDetection::modelChargeConsumption(uint16_t durationMs) {
+  return PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW *
+         durationMs * 10e6;
+}
\ No newline at end of file
diff --git a/src/colorDetection/ColorDetection.h b/src/colorDetection/ColorDetection.h
index 4030432..4829e79 100644
--- a/src/colorDetection/ColorDetection.h
+++ b/src/colorDetection/ColorDetection.h
@@ -65,7 +65,28 @@ public:
     void begin(void);
     void configure(VEML_CONFIG config);
     uint16_t getColorValue(color color);
-protected:
+
+    /**
+     * @brief Current consumtion of the sensor
+     * @note May not be accurate, as it is not known if the consumption is
+     * constant, or only if sensor is active. Could not be measured.
+     * 
+     * @return 
+     */
+    float modelCurrentConsumption();
+
+
+    /**
+     * @brief Estimates charge consumption of the sensor for the given duration
+     * @note May not be accurate, as it is not known if the consumption is
+     * constant, or only if sensor is active. Could not be measured.
+     *
+     * @param durationMs
+     * @return float
+     */
+    float modelChargeConsumption(uint16_t durationMs);
+
+  protected:
     uint16_t readDoubleRegister(uint8_t regAddr);
     void writeDoubleRegister(uint8_t regAddr, uint16_t data);
 };
diff --git a/src/display/Display.cpp b/src/display/Display.cpp
index 9451758..ef906d9 100644
--- a/src/display/Display.cpp
+++ b/src/display/Display.cpp
@@ -179,3 +179,11 @@ void Display::invertColor(void){
     }
     this->colorInverted = !this->colorInverted;
 };
+
+float modelCurrentConsumption() {
+    return PowerParameters::CurrentConsumptions::CURRENT_DISPLAY;
+};
+
+float modelChargeConsumptionOn(uint16_t durationMs) {
+    return PowerParameters::CurrentConsumptions::CURRENT_DISPLAY * durationMs * 10e6;
+};
\ No newline at end of file
diff --git a/src/display/Display.h b/src/display/Display.h
index f9b439b..387cc35 100644
--- a/src/display/Display.h
+++ b/src/display/Display.h
@@ -127,6 +127,19 @@ class Display{
          * 
          */
         void invertColor(void);
+
+        /**
+         * @brief Estimate the current consumption of the display
+         * @return consumed current in milliamperes
+         */
+        float modelCurrentConsumption();
+
+        /**
+         * @brief Estimate the energy consumption of the display
+         * @param durationMs time the display will be on
+         * @return consumed energy in coloumbs
+         */
+        float modelChargeConsumptionOn(uint16_t durationMs);
 };
 
 
diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp
index 3819a35..53473a0 100644
--- a/src/infraredLight/InfraredLED.cpp
+++ b/src/infraredLight/InfraredLED.cpp
@@ -4,6 +4,7 @@
 #define IR_FRONT_PIN 14
 #define IR_BOTTOM_PIN 13
 #define DUTY_RESOLUTION LEDC_TIMER_10_BIT
+#define DUTY_CYCLE_FREQUENCY 512
 
 InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel){
     this->ledPin = pin;
@@ -78,27 +79,53 @@ void InfraredLED::setState(bool state){
 };
 
 void InfraredLED::sendFrequency(uint16_t frequency){
-  constexpr uint32_t duty = 512;
+  constexpr uint32_t duty = DUTY_CYCLE_FREQUENCY;
   // Float to force float division without casting
   constexpr float resolution = 1 << DUTY_RESOLUTION;
   if (this->ledPin == IR_BOTTOM_PIN) {
-    float currentConsumption =
-        (duty / resolution) *
-        PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM;
     Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::LED_IR_BOTTOM,
-        currentConsumption,
-        IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+        this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
+        NULL);
   } else if (this->ledPin == IR_FRONT_PIN) {
-    float currentConsumption =
-        (duty / resolution) *
-        PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT;
     Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::LED_IR_FRONT,
-        currentConsumption,
-        IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+        this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
+        NULL);
   }
     ledc_set_freq(pwmSpeedMode,timer,frequency);
     ledc_set_duty(pwmSpeedMode,channel,duty);
     ledc_update_duty(pwmSpeedMode,channel);
-};
\ No newline at end of file
+};
+
+float InfraredLED::modelCurrentConsumption(uint32_t duty){
+    // Float to force float division without casting
+    constexpr float max_value = 1 << DUTY_RESOLUTION;
+    const float duty_factor = duty / max_value;
+    if (this->ledPin == IR_BOTTOM_PIN) {
+        return duty_factor * PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM;
+    } else if (this->ledPin == IR_FRONT_PIN) {
+        return duty_factor * PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT;
+    }
+    return NAN;
+};
+
+float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) {
+  // Float to force float division without casting
+  constexpr float resolution = 1 << DUTY_RESOLUTION;
+  if (this->ledPin == IR_BOTTOM_PIN) {
+    return durationMs *
+           PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM * 10e6;
+  } else if (this->ledPin == IR_FRONT_PIN) {
+    return durationMs *
+           PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT *  10e6;
+  }
+  return NAN;
+}
+
+float InfraredLED::modelChargeConsumptionSendFrequency(uint16_t durationMs) {
+  // Float to force float division without casting
+
+  return durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY) *
+         10e6;
+}
diff --git a/src/infraredLight/InfraredLight.h b/src/infraredLight/InfraredLight.h
index 56ac538..fae6df3 100644
--- a/src/infraredLight/InfraredLight.h
+++ b/src/infraredLight/InfraredLight.h
@@ -48,7 +48,32 @@ class InfraredLED{
          * @param frequency 
          */
         void sendFrequency(uint16_t frequency);
-    protected:
+
+        /**
+         * @brief Estimate the current consumption of setting the specified led to the
+         * passed duty cycle
+         * @param duty the duty cycle of the led
+         * @return consumed current in milliamperes
+         */
+        float modelCurrentConsumption(uint32_t duty);
+
+        /**
+         * @brief Estimate the energy consumption of turning the infrared led on
+         * @param durationMs time the led will be on
+         * @return consumed energy in coloumbs
+         */
+        float modelChargeConsumptionOn(uint16_t durationMs);
+
+        /**
+         * @brief Estimate the energy consumption of sending a frequency on the
+         * infrared led
+         * @param durationMs time the led will be on
+         * @param frequency the frequency the led will be flashing
+         * @return consumed energy in coloumbs
+         */
+        float modelChargeConsumptionSendFrequency(uint16_t durationMs);
+
+      protected:
         uint8_t ledPin;
         ledc_timer_t timer;
         ledc_channel_t channel;
diff --git a/src/lightDetection/LightDetection.h b/src/lightDetection/LightDetection.h
index 5c64027..49f02bc 100644
--- a/src/lightDetection/LightDetection.h
+++ b/src/lightDetection/LightDetection.h
@@ -78,7 +78,23 @@ public:
      * @return the average of all taken meaurments
      */
     static uint32_t getAverageValue(photoTransistors sensor, uint32_t measurments, uint32_t timeBetween);
-protected: 
+
+    /**
+     * @brief Get current consumption of the selected PTs
+     *
+     * @return float the current consumption of the PTs
+     */
+    static float modelCurrentConsumption(void);
+
+    /**
+     * @brief Estimate the energy consumption of setting the specified led to
+     * the passed color
+     * @param durationMs time the led will be on
+     * @return consumed energy in coloumbs
+     */
+    float modelChargeConsumptionOn(uint16_t durationMs);
+
+  protected: 
     static const uint8_t IR_PT_FRONT_ADC = 3;
     static const uint8_t IR_PT_LEFT_ADC = 4;
     static const uint8_t IR_PT_RIGHT_ADC = 5;
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index 2c7c0bd..33650b5 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -56,6 +56,24 @@ class Motor{
          * @return current speedvalue of the motor
          */
         uint16_t getSpeed(void); 
+
+        /**
+         * @brief Get the current consumption of the motor at specified speed
+         * 
+         * @param duty the duty cyle that should be considered, can be between 0-8192
+         *
+         * @return current consumption in milliamperes
+         */
+        float modelCurrentConsumption(uint16_t duty);
+
+        /**
+         * @brief Estimate the energy consumption of the display
+         * @param durationMs time the display will be on
+         * @return consumed energy in coloumbs
+         */
+        float modelChargeConsumptionOn(uint16_t duty, uint16_t durationMs);
+
+
     protected:
         uint8_t pin;
         ledc_timer_t timer;
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 54b5bf6..e58ca95 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -25,9 +25,7 @@ void Motor::begin(void) {
 };
 
 bool Motor::setSpeed(uint16_t duty) {
-  const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
-  const float current =
-      PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
+  const float current = this->modelCurrentConsumption(duty);
   if (this->pin == MOTOR_LEFT_PIN) {
     if (!Power::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::MOTOR_LEFT, current,
@@ -69,3 +67,12 @@ bool Motor::setSpeed(uint16_t duty) {
 };
 
 uint16_t Motor::getSpeed(void) { return this->duty; };
+
+float modelCurrentConsumption(uint16_t duty) {
+  const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
+  return PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
+}
+
+float modelChargeConsumptionOn(uint16_t duty, uint16_t durationMs) {
+  return modelCurrentConsumption(duty) * durationMs * 10e6;
+}
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 9d83420..e3844f4 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -293,4 +293,13 @@ void MotionDetection::writeRegister(uint8_t reg, uint8_t value){
     digitalWrite(34,HIGH);
     delayMicroseconds(10);
     handler->endTransaction();
-};
\ No newline at end of file
+};
+
+float MotionDetection::modelCurrentConsumption(){
+    return PowerParameters::CurrentConsumptions::CURRENT_IMU;
+}
+
+
+float MotionDetection::modelChargeConsumption(uint16_t durationMs){
+    return this->modelCurrentConsumption() * durationMs * 10e6;
+}
\ No newline at end of file
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 4b5b3e6..6b87df7 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -187,5 +187,21 @@ public:
      * @return the amount of acutally fetched packages 
     */
     uint getDataFromFIFO(FIFO_Package* buffer);
+
+    /**
+     * @brief Current consumtion of the sensor
+     * 
+     * @return 
+     */
+     float modelCurrentConsumption();
+
+
+     /**
+      * @brief Estimates charge consumption of the sensor for the given duration
+      *
+      * @param durationMs
+      * @return float
+      */
+     float modelChargeConsumption(uint16_t durationMs);
 };
 #endif //MotionDetection
\ No newline at end of file
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index 03759be..b4ec24c 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -1,142 +1,245 @@
 #include "MultiColorLight.h"
 
-MultiColorLight::MultiColorLight():rgbLeds(ledAmount,ledPin){
+MultiColorLight::MultiColorLight()
+    : rgbLeds(ledAmount, ledPin) {
 
+      };
+
+void MultiColorLight::begin(void) {
+  if (!Power::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT,
+          PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
+          MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
+      Power::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT,
+          PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
+          MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
+      Power::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::LED_RGB_BOTTOM,
+          PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
+          MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Could not get power for MultiColorLight");
+    Serial.println("Could not get power for MultiColorLight");
+  }
+  rgbLeds.begin();
+  this->turnOffLed();
 };
 
-void MultiColorLight::begin(void){
-    if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) ){
-      ESP_LOGE(TAG, "Could not get power for MultiColorLight");
-      Serial.println("Could not get power for MultiColorLight");
+void MultiColorLight::setLed(uint8_t index, uint32_t color) {
+  if (index > ledAmount - 1) {
+    // TODO: logging
+  }
+  uint32_t normalizedColor = normalizeColor(color);
+  float totalConsumption = modelCurrentConsumption(normalizedColor);
+  switch (index) {
+  case 0:
+    if (!Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT,
+            totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
+      ESP_LOGW(TAG,
+               "Power to set LED RGB TOP RIGHT to color 0x%.8X not granted in "
+               "time. Skipping.",
+               normalizedColor);
+      return;
     }
-    rgbLeds.begin();
-    this->turnOffLed();
+    break;
+  case 1:
+    if (!Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption,
+            MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
+      ESP_LOGW(TAG,
+               "Power to set LED RGB TOP LEFT to color 0x%.8X not granted in "
+               "time. Skipping.",
+               normalizedColor);
+      return;
+    }
+    break;
+  case 2:
+    if (!Power::waitForCurrentAllowance(
+            PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption,
+            MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
+      ESP_LOGW(TAG,
+               "Power to set LED RGB BOTTOM to color 0x%.8X not granted in "
+               "time. Skipping.",
+               normalizedColor);
+      return;
+    }
+    break;
+  }
+  rgbLeds.setPixelColor(index, normalizedColor);
+  rgbLeds.show();
 };
 
-
-void MultiColorLight::setLed(uint8_t index , uint32_t color){
-    if (index > ledAmount-1){
-        //TODO: logging
+void MultiColorLight::setLed(leds leds, uint32_t color) {
+  switch (leds) {
+  case TOP_LEFT:
+    MultiColorLight::setLed(1, color);
+    break;
+  case TOP_RIGHT:
+    MultiColorLight::setLed(0, color);
+    break;
+  case BOTTOM:
+    MultiColorLight::setLed(2, color);
+    break;
+  case TOP:
+    for (int index = 0; index < 2; index++) {
+      MultiColorLight::setLed(index, color);
     }
-    uint32_t normalizedColor = normalizeColor(color);
-    uint16_t colorComponentRed = (normalizedColor & 0x00FF0000) >> 16;
-    uint16_t colorComponentGreen = (normalizedColor & 0x0000FF00) >> 8;
-    uint16_t colorComponentBlue = (normalizedColor & 0x000000FF);
-    float redChannelConsumption = (colorComponentRed/255.0) * PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
-    float greenChannelConsumption = (colorComponentGreen/255.0) * PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
-    float blueChannelConsumption = (colorComponentBlue/255.0) * PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
-    float totalConsumption = redChannelConsumption + greenChannelConsumption + blueChannelConsumption + PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE;
-    switch (index) {
-        case 0:
-            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
-                ESP_LOGW(TAG,  "Power to set LED RGB TOP RIGHT to color %d not granted in time. Skipping.", color);
-                return;
-            }
-            break;
-        case 1:
-            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
-                ESP_LOGW(TAG,  "Power to set LED RGB TOP LEFT to color %d not granted in time. Skipping.", color);
-                return;
-            }
-            break;
-        case 2:
-            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
-                ESP_LOGW(TAG,  "Power to set LED RGB BOTTOM to color %d not granted in time. Skipping.", color);
-                return;
-            }
-            break;
+    break;
+  case ALL:
+    for (int index = 0; index < ledAmount; index++) {
+      MultiColorLight::setLed(index, color);
     }
-    rgbLeds.setPixelColor(index, normalizedColor);
-    rgbLeds.show();
+    break;
+  default:
+    // TODO logging
+    break;
+  }
 };
 
-
-void MultiColorLight::setLed(leds leds, uint32_t color){
-    switch (leds){
-        case TOP_LEFT:
-            MultiColorLight::setLed(1,color);break;
-        case TOP_RIGHT:
-            MultiColorLight::setLed(0,color);break;
-        case BOTTOM:
-            MultiColorLight::setLed(2,color);break;
-        case TOP:
-            for (int index = 0; index<2; index++){
-                MultiColorLight::setLed(index,color);
-            }break;
-        case ALL:
-            for (int index = 0; index<ledAmount; index++){
-                MultiColorLight::setLed(index,color);
-            }break;
-        default:
-            //TODO logging
-            break;
-    }
-
+void MultiColorLight::setLed(leds leds, uint8_t red, uint8_t green,
+                             uint8_t blue) {
+  MultiColorLight::setLed(leds, MultiColorLight::color(red, green, blue));
 };
 
-void MultiColorLight::setLed(leds leds, uint8_t red, uint8_t green, uint8_t blue){
-    MultiColorLight::setLed(leds, MultiColorLight::color(red,green,blue));
+void MultiColorLight::setTopLeds(uint32_t color) {
+  MultiColorLight::setLed(TOP, color);
 };
 
-
-void MultiColorLight::setTopLeds(uint32_t color){
-    MultiColorLight::setLed(TOP,color);
-}; 
-
-void MultiColorLight::setTopLeds(uint8_t red, uint8_t green, uint8_t blue){
-    MultiColorLight::setTopLeds(MultiColorLight::color(red,green,blue));
-}; 
-
-void MultiColorLight::blink(uint16_t amount,uint32_t color, leds leds, uint32_t interval){
-    for(uint16_t index = 0; index < amount;index++){
-        MultiColorLight::setLed(leds, color);
-        vTaskDelay(interval);
-        MultiColorLight::turnOffLed(leds);
-        vTaskDelay(interval);
-    }
+void MultiColorLight::setTopLeds(uint8_t red, uint8_t green, uint8_t blue) {
+  MultiColorLight::setTopLeds(MultiColorLight::color(red, green, blue));
 };
 
-void MultiColorLight::turnOffLed(leds leds){
-    switch (leds){
-        case TOP_LEFT:
-            MultiColorLight::setLed(1,0);break;
-        case TOP_RIGHT:
-            MultiColorLight::setLed(0,0);break;
-        case BOTTOM:
-            MultiColorLight::setLed(2,0);break;
-        case TOP:
-            for (int index = 0; index<2; index++){
-                MultiColorLight::setLed(index,0);
-            }break;
-        case ALL:
-            for (int index = 0; index<3; index++){
-                MultiColorLight::setLed(index,0);
-            }break;
-        default:
-            //TODO logging
-            break;
-    }
+void MultiColorLight::blink(uint16_t amount, uint32_t color, leds leds,
+                            uint32_t interval) {
+  for (uint16_t index = 0; index < amount; index++) {
+    MultiColorLight::setLed(leds, color);
+    vTaskDelay(interval);
+    MultiColorLight::turnOffLed(leds);
+    vTaskDelay(interval);
+  }
 };
 
-uint32_t MultiColorLight::color(uint8_t r, uint8_t g, uint8_t b){
-    return rgbLeds.Color(r,g,b);
+void MultiColorLight::turnOffLed(leds leds) {
+  switch (leds) {
+  case TOP_LEFT:
+    MultiColorLight::setLed(1, 0);
+    break;
+  case TOP_RIGHT:
+    MultiColorLight::setLed(0, 0);
+    break;
+  case BOTTOM:
+    MultiColorLight::setLed(2, 0);
+    break;
+  case TOP:
+    for (int index = 0; index < 2; index++) {
+      MultiColorLight::setLed(index, 0);
+    }
+    break;
+  case ALL:
+    for (int index = 0; index < 3; index++) {
+      MultiColorLight::setLed(index, 0);
+    }
+    break;
+  default:
+    // TODO logging
+    break;
+  }
 };
 
-//PRIVATE
-uint32_t MultiColorLight::normalizeColor(uint32_t color,uint8_t maxBrightness){
-    uint8_t red = (color&0x00FF0000)>>16;
-    uint8_t green = (color&0x0000FF00)>>8;
-    uint8_t blue = (color&0x000000FF);
-    if (red > maxBrightness){
-        red = maxBrightness;
+uint32_t MultiColorLight::color(uint8_t r, uint8_t g, uint8_t b) {
+  return rgbLeds.Color(r, g, b);
+};
+
+// PRIVATE
+uint32_t MultiColorLight::normalizeColor(uint32_t color,
+                                         uint8_t maxBrightness) {
+  uint8_t red = (color & 0x00FF0000) >> 16;
+  uint8_t green = (color & 0x0000FF00) >> 8;
+  uint8_t blue = (color & 0x000000FF);
+  if (red > maxBrightness) {
+    red = maxBrightness;
+  }
+  if (green > maxBrightness - 70) {
+    green = maxBrightness - 70;
+  }
+  if (blue > maxBrightness - 50) {
+    blue = maxBrightness - 50;
+  }
+  return MultiColorLight::color(red, green, blue);
+};
+
+float MultiColorLight::modelCurrentConsumption(uint32_t color) {
+  uint32_t normalizedColor = normalizeColor(color);
+  uint16_t colorComponentRed = (normalizedColor & 0x00FF0000) >> 16;
+  uint16_t colorComponentGreen = (normalizedColor & 0x0000FF00) >> 8;
+  uint16_t colorComponentBlue = (normalizedColor & 0x000000FF);
+  float redChannelConsumption =
+      (colorComponentRed / 255.0) *
+      PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
+  float greenChannelConsumption =
+      (colorComponentGreen / 255.0) *
+      PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
+  float blueChannelConsumption =
+      (colorComponentBlue / 255.0) *
+      PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
+  return redChannelConsumption + greenChannelConsumption +
+         blueChannelConsumption +
+         PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE;
+};
+
+float MultiColorLight::modelCurrentConsumption(uint8_t red, uint8_t green,
+                                               uint8_t blue) {
+  return modelCurrentConsumption(MultiColorLight::color(red, green, blue));
+};
+
+float MultiColorLight::modelChargeConsumption(uint8_t index, uint32_t color,
+                                                 uint16_t durationMs) {
+  if (index > ledAmount - 1) {
+    // TODO: logging
+  }
+  uint32_t normalizedColor = normalizeColor(color);
+  float ledConsumption = modelCurrentConsumption(normalizedColor);
+  return ledConsumption * durationMs * 10e6;
+};
+
+float MultiColorLight::modelChargeConsumption(leds leds, uint32_t color,
+                                                 uint16_t durationMs) {
+  float ledsConsumption = 0;
+  switch (leds) {
+  case TOP_LEFT:
+    ledsConsumption =
+        MultiColorLight::modelChargeConsumption(1, color, durationMs);
+    break;
+  case TOP_RIGHT:
+    ledsConsumption =
+        MultiColorLight::modelChargeConsumption(0, color, durationMs);
+    break;
+  case BOTTOM:
+    ledsConsumption =
+        MultiColorLight::modelChargeConsumption(2, color, durationMs);
+    break;
+  case TOP:
+    for (int index = 0; index < 2; index++) {
+      ledsConsumption +=
+          MultiColorLight::modelChargeConsumption(index, color, durationMs);
     }
-    if(green > maxBrightness-70){
-        green = maxBrightness-70;
+    break;
+  case ALL:
+    for (int index = 0; index < ledAmount; index++) {
+      ledsConsumption +=
+          MultiColorLight::modelChargeConsumption(index, color, durationMs);
     }
-    if(blue > maxBrightness-50){
-        blue = maxBrightness-50;
-    }
-    return MultiColorLight::color(red,green,blue);
-}
\ No newline at end of file
+    break;
+  default:
+    // TODO logging
+    break;
+  }
+};
+
+float MultiColorLight::modelChargeConsumption(leds leds, uint8_t red,
+                                                 uint8_t green, uint8_t blue,
+                                                 uint16_t durationMs) {
+  return MultiColorLight::modelChargeConsumption(
+      leds, MultiColorLight::color(red, green, blue), durationMs);
+};
\ No newline at end of file
diff --git a/src/multiColorLight/MultiColorLight.h b/src/multiColorLight/MultiColorLight.h
index 562958a..76ba711 100644
--- a/src/multiColorLight/MultiColorLight.h
+++ b/src/multiColorLight/MultiColorLight.h
@@ -1,146 +1,209 @@
 /**
  * @file MultiColorLight.h
  * @author Saskia Duebener, Hans Haupt
- * @brief This component controls the ability to show multicolored light, using the RGB-LEDs
+ * @brief This component controls the ability to show multicolored light, using
+ * the RGB-LEDs
  * @version 0.2
  * @date 2023-11-25
- * 
+ *
  * @copyright Copyright (c) 2023
- * 
+ *
  */
 #ifndef MultiColorLight_h
 #define MultiColorLight_h
-#include <Adafruit_NeoPixel.h>
-#include "ColorConstants.h"
 #include "../power/Power.h"
+#include "ColorConstants.h"
+#include <Adafruit_NeoPixel.h>
 
 #define MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS 20
 
 /**
- * @brief Describes combinations of leds on the Dezibot. 
- * With the Robot in Front of you, when the robot drives away from you, the left LED is TOP_LEFT 
- * 
+ * @brief Describes combinations of leds on the Dezibot.
+ * With the Robot in Front of you, when the robot drives away from you, the left
+ * LED is TOP_LEFT
+ *
  */
-enum leds{
-    TOP_LEFT,
-    TOP_RIGHT,
-    BOTTOM,
-    TOP,
-    ALL
-};
+enum leds { TOP_LEFT, TOP_RIGHT, BOTTOM, TOP, ALL };
 
 #define TAG "MultiColorLight"
 
-class MultiColorLight{
+class MultiColorLight {
 protected:
-    static const uint16_t ledAmount = 3;
-    static const int16_t ledPin = 48;
-    static const uint8_t maxBrightness = 150;
-    Adafruit_NeoPixel rgbLeds;
-    static constexpr int maximumExecutionDelayMs = 10;
+  static const uint16_t ledAmount = 3;
+  static const int16_t ledPin = 48;
+  static const uint8_t maxBrightness = 150;
+  Adafruit_NeoPixel rgbLeds;
+  static constexpr int maximumExecutionDelayMs = 10;
+
 public:
-    
-    MultiColorLight();
-    /**
-     * @brief initialize the multicolor component
-     * 
-     */
-    void begin(void);
+  MultiColorLight();
+  /**
+   * @brief initialize the multicolor component
+   *
+   */
+  void begin(void);
 
-    /**
-     * @brief Set the specified led to the passed color
-     * @param index ranging from 0-2, 0: Right, 1: Left, 2: Bottom 
-     * @param color A 32-bit unsigned integer representing the color in the format
-    *               0x00RRGGBB, where RR is the red component, GG is the green
-    *               component, and BB is the blue component. Each color can range between 0 to 100
-     */
-    void setLed(uint8_t index , uint32_t color);
+  /**
+   * @brief Set the specified led to the passed color
+   * @param index ranging from 0-2, 0: Right, 1: Left, 2: Bottom
+   * @param color A 32-bit unsigned integer representing the color in the format
+   *               0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component. Each color can range
+   * between 0 to 100
+   */
+  void setLed(uint8_t index, uint32_t color);
 
-    /**
-     * @brief Set the specified leds to the passed color value
-     * 
-     * @param leds which leds should be updated
-     * @param color A 32-bit unsigned integer representing the color in the format
-    *               0x00RRGGBB, where RR is the red component, GG is the green
-    *               component, and BB is the blue component. Each color can range between 0 to 100
-     */
-    void setLed(leds leds, uint32_t color);
+  /**
+   * @brief Set the specified leds to the passed color value
+   *
+   * @param leds which leds should be updated
+   * @param color A 32-bit unsigned integer representing the color in the format
+   *               0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component. Each color can range
+   * between 0 to 100
+   */
+  void setLed(leds leds, uint32_t color);
 
-    /**
-     * @brief Set the specified leds to the passed color value
-     * 
-     * @param leds which leds should be updated
-     * @param red brightness of red, is normalized in the function
-     * @param green brightness of green, is normalized in the function
-     * @param blue brightness of blue, is normalized in the function
-     */
-    void setLed(leds leds, uint8_t red, uint8_t green, uint8_t blue);
-    
-    /**
-     * @brief sets the two leds on the top of the robot to the specified color
-     * 
-     * @param color A 32-bit unsigned integer representing the color in the format
-    *               0x00RRGGBB, where RR is the red component, GG is the green
-    *               component, and BB is the blue component. Each color can range between 0 to 100
-     */
-    void setTopLeds(uint32_t color);    
+  /**
+   * @brief Set the specified leds to the passed color value
+   *
+   * @param leds which leds should be updated
+   * @param red brightness of red, is normalized in the function
+   * @param green brightness of green, is normalized in the function
+   * @param blue brightness of blue, is normalized in the function
+   */
+  void setLed(leds leds, uint8_t red, uint8_t green, uint8_t blue);
 
-    /**
-     * @brief sets the two leds on the top of the robot to the specified color
-     * 
-     * @param red brightness of red, is normalized in the function
-     * @param green brightness of green, is normalized in the function
-     * @param blue brightness of blue, is normalized in the function
-     */
-    void setTopLeds(uint8_t red, uint8_t green, uint8_t blue);  
-    
-    /**
-     * @brief Let LEDs blink, returns after all blinks were executed
-     * 
-     * @param amount how often should the leds blink
-     * @param color A 32-bit unsigned integer representing the color in the format
-    *               0x00RRGGBB, where RR is the red component, GG is the green
-    *               component, and BB is the blue component.
-    *               Each color can range between 0 to 100
-    *               Defaults to blue
-     * @param leds which LEDs should blink, default is TOP
-     * @param interval how many miliseconds the led is on, defaults to 1s
-     */
-    void blink(uint16_t amount,uint32_t color = 0x00006400,leds leds=TOP, uint32_t interval=1000);
+  /**
+   * @brief calculates the current consumption of an LED with the given color
+   *
+   * @param color A 32-bit unsigned integer representing the color in the
+   * format 0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component.
+   * @return float the current consumption in mA
+   */
+  float modelCurrentConsumption(uint32_t color);
 
-    /**
-     * @brief turn off the given leds
-     * 
-     * @param leds which leds should be turned off, defaults to ALL
-     */
-    void turnOffLed(leds leds=ALL);
-    
-    /**
-     * @brief wrapper to calulate the used colorformat from a rgb-value
-     * 
-     * @param r red (0-100)
-     * @param g green (0-100)
-     * @param b blue (0-100)
-     * @return A 32-bit unsigned integer representing the color in the format
-    *          0x00RRGGBB, where RR is the red component, GG is the green
-    *          component, and BB is the blue component.
-     */
-    uint32_t color(uint8_t r, uint8_t g, uint8_t b);
+  /**
+   * @brief calculates the current consumption of an LED with the given color
+   * @note color is not normalized in this function
+   *
+   * @param red brightness of red
+   * @param green brightness of green
+   * @param blue brightness of blue
+   * @return float the current consumption in mA
+   */
+  float modelCurrentConsumption(uint8_t red, uint8_t green, uint8_t blue);
+
+  /**
+   * @brief Estimate the energy consumption of setting the specified led to the
+   * passed color
+   * @param index ranging from 0-2, 0: Right, 1: Left, 2: Bottom
+   * @param color A 32-bit unsigned integer representing the color in the
+   * format 0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component. Each color can
+   * range between 0 to 100
+   * @return consumed energy in coloumbs
+   */
+  float modelChargeConsumption(uint8_t index, uint32_t color,
+                                  uint16_t durationMs);
+
+  /**
+   * @brief Estimate the energy consumption of setting the specified leds to the
+   * passed color value
+   *
+   * @param leds which leds should be considered
+   * @param color A 32-bit unsigned integer representing the color in the
+   * format 0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component. Each color can
+   * range between 0 to 100
+   * @return consumed energy in coloumbs
+   */
+  float modelChargeConsumption(leds leds, uint32_t color,
+                                  uint16_t durationMs);
+
+  /**
+   * @brief Estimate the energy consumption of setting the specified leds to the
+   * passed color value
+   *
+   * @param leds which leds should be considered
+   * @param red brightness of red, is normalized in the function
+   * @param green brightness of green, is normalized in the function
+   * @param blue brightness of blue, is normalized in the function
+   * @return consumed energy in coloumbs
+   */
+  float modelChargeConsumption(leds leds, uint8_t red, uint8_t green,
+                                  uint8_t blue, uint16_t durationMs);
+
+  /**
+   * @brief sets the two leds on the top of the robot to the specified color
+   *
+   * @param color A 32-bit unsigned integer representing the color in the format
+   *               0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component. Each color can range
+   * between 0 to 100
+   */
+  void setTopLeds(uint32_t color);
+
+  /**
+   * @brief sets the two leds on the top of the robot to the specified color
+   *
+   * @param red brightness of red, is normalized in the function
+   * @param green brightness of green, is normalized in the function
+   * @param blue brightness of blue, is normalized in the function
+   */
+  void setTopLeds(uint8_t red, uint8_t green, uint8_t blue);
+
+  /**
+   * @brief Let LEDs blink, returns after all blinks were executed
+   *
+   * @param amount how often should the leds blink
+   * @param color A 32-bit unsigned integer representing the color in the format
+   *               0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component.
+   *               Each color can range between 0 to 100
+   *               Defaults to blue
+   * @param leds which LEDs should blink, default is TOP
+   * @param interval how many miliseconds the led is on, defaults to 1s
+   */
+  void blink(uint16_t amount, uint32_t color = 0x00006400, leds leds = TOP,
+             uint32_t interval = 1000);
+
+  /**
+   * @brief turn off the given leds
+   *
+   * @param leds which leds should be turned off, defaults to ALL
+   */
+  void turnOffLed(leds leds = ALL);
+
+  /**
+   * @brief wrapper to calulate the used colorformat from a rgb-value
+   *
+   * @param r red (0-100)
+   * @param g green (0-100)
+   * @param b blue (0-100)
+   * @return A 32-bit unsigned integer representing the color in the format
+   *          0x00RRGGBB, where RR is the red component, GG is the green
+   *          component, and BB is the blue component.
+   */
+  uint32_t color(uint8_t r, uint8_t g, uint8_t b);
 
 private:
-    /**
-     * @brief normalizes every component of color to not exeed the maxBrightness
-     * 
-     * @param color A 32-bit unsigned integer representing the color in the format
-        *               0x00RRGGBB, where RR is the red component, GG is the green
-        *               component, and BB is the blue component.
-    * @param maxBrigthness maximal level of brightness that is allowed for each color
-    * @return uint32_t A 32-bit unsigned integer representing the color in the format
-        *               0x00RRGGBB, where RR is the red component, GG is the green
-        *               component, and BB is the blue component. Where each component can be
-        *               between 0 - maxBrightness
-    */
-    uint32_t normalizeColor(uint32_t color, uint8_t maxBrigthness=maxBrightness);
+  /**
+   * @brief normalizes every component of color to not exeed the maxBrightness
+   *
+   * @param color A 32-bit unsigned integer representing the color in the format
+   *               0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component.
+   * @param maxBrigthness maximal level of brightness that is allowed for each
+   * color
+   * @return uint32_t A 32-bit unsigned integer representing the color in the
+   * format 0x00RRGGBB, where RR is the red component, GG is the green
+   *               component, and BB is the blue component. Where each component
+   * can be between 0 - maxBrightness
+   */
+  uint32_t normalizeColor(uint32_t color,
+                          uint8_t maxBrigthness = maxBrightness);
 };
 
-#endif //MultiColorLight_h
\ No newline at end of file
+#endif // MultiColorLight_h
\ No newline at end of file

From 62d4c6bdb0cd040babb0f067493201ae36c40bcc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sat, 15 Feb 2025 22:07:33 +0100
Subject: [PATCH 43/59] Fix oversights in modeling functions

---
 src/lightDetection/LightDetection.cpp | 14 +++++++++++++-
 src/lightDetection/LightDetection.h   |  9 +++++----
 src/motion/Motion.h                   |  2 +-
 src/motion/Motor.cpp                  |  4 ++--
 4 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index 2d07cff..3bbf253 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -135,4 +135,16 @@ uint16_t LightDetection::readDLPT(photoTransistors sensor){
     Power::releaseCurrent(PowerParameters::PowerConsumers::PT_DL);
     digitalWrite(DL_PT_ENABLE,LOW);
     return result;
-};
\ No newline at end of file
+};
+
+float LightDetection::modelCurrentConsumption(photoTransistors sensor){
+    if(sensor == DL_FRONT || sensor == DL_BOTTOM){
+        return PowerParameters::CurrentConsumptions::CURRENT_PT * 2;
+    } else {
+        return PowerParameters::CurrentConsumptions::CURRENT_PT * 4;
+    }
+};
+
+float LightDetection::modelChargeConsumptionOn(photoTransistors sensor, uint16_t durationMs) {
+    return LightDetection::modelCurrentConsumption(sensor) * durationMs * 10e6;
+}
\ No newline at end of file
diff --git a/src/lightDetection/LightDetection.h b/src/lightDetection/LightDetection.h
index 49f02bc..b79fdac 100644
--- a/src/lightDetection/LightDetection.h
+++ b/src/lightDetection/LightDetection.h
@@ -84,15 +84,16 @@ public:
      *
      * @return float the current consumption of the PTs
      */
-    static float modelCurrentConsumption(void);
+    static float modelCurrentConsumption(photoTransistors sensor);
 
     /**
-     * @brief Estimate the energy consumption of setting the specified led to
-     * the passed color
+     * @brief Estimate the energy consumption of enabling the selected PT for
+     * the given duration
      * @param durationMs time the led will be on
      * @return consumed energy in coloumbs
      */
-    float modelChargeConsumptionOn(uint16_t durationMs);
+    static float modelChargeConsumptionOn(photoTransistors sensor,
+                                   uint16_t durationMs);
 
   protected: 
     static const uint8_t IR_PT_FRONT_ADC = 3;
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index 33650b5..195c077 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -71,7 +71,7 @@ class Motor{
          * @param durationMs time the display will be on
          * @return consumed energy in coloumbs
          */
-        float modelChargeConsumptionOn(uint16_t duty, uint16_t durationMs);
+        float modelChargeConsumption(uint16_t duty, uint16_t durationMs);
 
 
     protected:
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index e58ca95..825620c 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -68,11 +68,11 @@ bool Motor::setSpeed(uint16_t duty) {
 
 uint16_t Motor::getSpeed(void) { return this->duty; };
 
-float modelCurrentConsumption(uint16_t duty) {
+float Motor::modelCurrentConsumption(uint16_t duty) {
   const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
   return PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
 }
 
-float modelChargeConsumptionOn(uint16_t duty, uint16_t durationMs) {
+float Motor::modelChargeConsumption(uint16_t duty, uint16_t durationMs) {
   return modelCurrentConsumption(duty) * durationMs * 10e6;
 }

From ca37aa972b92cb566687b1181015a71b41edc4b5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sat, 15 Feb 2025 22:07:55 +0100
Subject: [PATCH 44/59] Add test case for modeling functions

---
 .../Estimation_Test/Estimation_Test.ino       | 46 +++++++++++++++++++
 1 file changed, 46 insertions(+)
 create mode 100644 example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino

diff --git a/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino b/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino
new file mode 100644
index 0000000..bd86500
--- /dev/null
+++ b/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino
@@ -0,0 +1,46 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+long randomDurationMs = 0;
+
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.begin();
+  Serial.begin(115200);
+  // init RNG
+  randomSeed(analogRead(GPIO_NUM_42));
+}
+
+void loop() {
+  randomDurationMs = random(0, 10e4);
+  uint32_t color = random(0, 0xFFFFFF);
+  Serial.printf("Estimated current and charge consumption of turning an RGB "
+                "LED on at color 0x%06X for %d ms: %f mA, %f C\n",
+                color, randomDurationMs,
+                dezibot.multiColorLight.modelCurrentConsumption(color),
+                dezibot.multiColorLight.modelChargeConsumption(
+                    TOP_LEFT, color, randomDurationMs));
+  randomDurationMs = random(0, 10e4);
+  uint32_t randomDuty = random(0, 1 << LEDC_TIMER_10_BIT);
+  Serial.printf(
+      "Estimated current and charge consumption of turning front IR LED on for "
+      "%d ms: %f mA, %f C\n",
+      randomDurationMs,
+      dezibot.infraredLight.front.modelCurrentConsumption(randomDuty),
+      dezibot.infraredLight.front.modelChargeConsumptionOn(randomDurationMs));
+  Serial.printf(
+      "Estimated current and charge consumption of turning bottom IR LED on "
+      "for "
+      "%d ms: %f mA, %f C\n",
+      randomDurationMs,
+      dezibot.infraredLight.bottom.modelCurrentConsumption(randomDuty),
+      dezibot.infraredLight.bottom.modelChargeConsumptionOn(randomDurationMs));
+  randomDurationMs = random(0, 10e4);
+  Serial.printf(
+      "Estimated current and charge consumption of running motor at default "
+      "duty cycle for "
+      "%d ms: %f mA, %f C\n",
+      randomDurationMs, dezibot.motion.left.modelCurrentConsumption(3900),
+      dezibot.motion.left.modelChargeConsumption(3900, randomDurationMs));
+  delay(10000);
+}
\ No newline at end of file

From 7ed58afea0d43f7a3480e3b5f373112955e7f4d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sat, 15 Feb 2025 22:41:34 +0100
Subject: [PATCH 45/59] Fix charge calculation

---
 src/colorDetection/ColorDetection.cpp   | 4 ++--
 src/infraredLight/InfraredLED.cpp       | 8 ++++----
 src/lightDetection/LightDetection.cpp   | 5 +++--
 src/motion/Motor.cpp                    | 2 +-
 src/motionDetection/MotionDetection.cpp | 5 ++---
 src/multiColorLight/MultiColorLight.cpp | 3 ++-
 6 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/src/colorDetection/ColorDetection.cpp b/src/colorDetection/ColorDetection.cpp
index eddbaf2..19dcde9 100644
--- a/src/colorDetection/ColorDetection.cpp
+++ b/src/colorDetection/ColorDetection.cpp
@@ -98,6 +98,6 @@ float ColorDetection::modelCurrentConsumption() {
 };
 
 float ColorDetection::modelChargeConsumption(uint16_t durationMs) {
-  return PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW *
-         durationMs * 10e6;
+  return (PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW *
+         durationMs) / 10e6f;
 }
\ No newline at end of file
diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp
index 53473a0..a9b9b06 100644
--- a/src/infraredLight/InfraredLED.cpp
+++ b/src/infraredLight/InfraredLED.cpp
@@ -115,10 +115,10 @@ float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) {
   constexpr float resolution = 1 << DUTY_RESOLUTION;
   if (this->ledPin == IR_BOTTOM_PIN) {
     return durationMs *
-           PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM * 10e6;
+           (PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM) / 10e6f;
   } else if (this->ledPin == IR_FRONT_PIN) {
     return durationMs *
-           PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT *  10e6;
+           (PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT) / 10e6f;
   }
   return NAN;
 }
@@ -126,6 +126,6 @@ float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) {
 float InfraredLED::modelChargeConsumptionSendFrequency(uint16_t durationMs) {
   // Float to force float division without casting
 
-  return durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY) *
-         10e6;
+  return (durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY)) /
+         10e6f;
 }
diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index 3bbf253..498d2c2 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -145,6 +145,7 @@ float LightDetection::modelCurrentConsumption(photoTransistors sensor){
     }
 };
 
-float LightDetection::modelChargeConsumptionOn(photoTransistors sensor, uint16_t durationMs) {
-    return LightDetection::modelCurrentConsumption(sensor) * durationMs * 10e6;
+float LightDetection::modelChargeConsumptionOn(photoTransistors sensor,
+                                               uint16_t durationMs) {
+  return (LightDetection::modelCurrentConsumption(sensor) * durationMs) / 10e6f;
 }
\ No newline at end of file
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 825620c..ea6e9db 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -74,5 +74,5 @@ float Motor::modelCurrentConsumption(uint16_t duty) {
 }
 
 float Motor::modelChargeConsumption(uint16_t duty, uint16_t durationMs) {
-  return modelCurrentConsumption(duty) * durationMs * 10e6;
+  return (modelCurrentConsumption(duty) * durationMs) / 10e6f;
 }
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index e3844f4..178ef9f 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -299,7 +299,6 @@ float MotionDetection::modelCurrentConsumption(){
     return PowerParameters::CurrentConsumptions::CURRENT_IMU;
 }
 
-
-float MotionDetection::modelChargeConsumption(uint16_t durationMs){
-    return this->modelCurrentConsumption() * durationMs * 10e6;
+float MotionDetection::modelChargeConsumption(uint16_t durationMs) {
+  return (this->modelCurrentConsumption() * durationMs) / 10e6f;
 }
\ No newline at end of file
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index b4ec24c..28b9024 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -200,7 +200,7 @@ float MultiColorLight::modelChargeConsumption(uint8_t index, uint32_t color,
   }
   uint32_t normalizedColor = normalizeColor(color);
   float ledConsumption = modelCurrentConsumption(normalizedColor);
-  return ledConsumption * durationMs * 10e6;
+  return (ledConsumption * durationMs) / 10e6f;
 };
 
 float MultiColorLight::modelChargeConsumption(leds leds, uint32_t color,
@@ -235,6 +235,7 @@ float MultiColorLight::modelChargeConsumption(leds leds, uint32_t color,
     // TODO logging
     break;
   }
+  return ledsConsumption; 
 };
 
 float MultiColorLight::modelChargeConsumption(leds leds, uint8_t red,

From ec457d31d90054d1d2e2029352edc4efa0b74bf5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sat, 15 Feb 2025 22:42:04 +0100
Subject: [PATCH 46/59] Add test case for modeling functions

---
 .../Estimation_Test/Estimation_Test.ino         | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino b/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino
index bd86500..eac8906 100644
--- a/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino
+++ b/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino
@@ -8,11 +8,13 @@ void setup() {
   dezibot.begin();
   Serial.begin(115200);
   // init RNG
-  randomSeed(analogRead(GPIO_NUM_42));
+  int seed = analogRead(GPIO_NUM_42);
+  randomSeed(seed);
+  Serial.printf("Initialized PRNG with seed %d", seed);
 }
 
 void loop() {
-  randomDurationMs = random(0, 10e4);
+  randomDurationMs = random(0, 100);
   uint32_t color = random(0, 0xFFFFFF);
   Serial.printf("Estimated current and charge consumption of turning an RGB "
                 "LED on at color 0x%06X for %d ms: %f mA, %f C\n",
@@ -20,22 +22,23 @@ void loop() {
                 dezibot.multiColorLight.modelCurrentConsumption(color),
                 dezibot.multiColorLight.modelChargeConsumption(
                     TOP_LEFT, color, randomDurationMs));
-  randomDurationMs = random(0, 10e4);
+  randomDurationMs = random(0, 100);
   uint32_t randomDuty = random(0, 1 << LEDC_TIMER_10_BIT);
   Serial.printf(
-      "Estimated current and charge consumption of turning front IR LED on for "
+      "Estimated current and charge consumption of turning front IR LED on at %d duty for "
       "%d ms: %f mA, %f C\n",
+      randomDuty,
       randomDurationMs,
       dezibot.infraredLight.front.modelCurrentConsumption(randomDuty),
       dezibot.infraredLight.front.modelChargeConsumptionOn(randomDurationMs));
   Serial.printf(
-      "Estimated current and charge consumption of turning bottom IR LED on "
-      "for "
+      "Estimated current and charge consumption of turning bottom IR LED on at %d duty for "
       "%d ms: %f mA, %f C\n",
+      randomDuty,
       randomDurationMs,
       dezibot.infraredLight.bottom.modelCurrentConsumption(randomDuty),
       dezibot.infraredLight.bottom.modelChargeConsumptionOn(randomDurationMs));
-  randomDurationMs = random(0, 10e4);
+  randomDurationMs = random(0, 100);
   Serial.printf(
       "Estimated current and charge consumption of running motor at default "
       "duty cycle for "

From 5efa7a5efdf344bfad0e7ea6bce2982548e31d35 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 01:10:33 +0100
Subject: [PATCH 47/59] Rename Power.* to PowerManager.* for clarity

---
 src/Dezibot.h                             |   2 +-
 src/colorDetection/ColorDetection.cpp     |   4 +-
 src/colorDetection/ColorDetection.h       |   2 +-
 src/communication/Communication.cpp       |   8 +-
 src/communication/Communication.h         |   2 +-
 src/display/Display.cpp                   |  14 +--
 src/display/Display.h                     |   2 +-
 src/infraredLight/InfraredLED.cpp         |  16 +--
 src/infraredLight/InfraredLight.h         |   2 +-
 src/lightDetection/LightDetection.cpp     |  18 +--
 src/lightDetection/LightDetection.h       |   2 +-
 src/motion/Motion.h                       |   2 +-
 src/motion/Motor.cpp                      |   6 +-
 src/motionDetection/MotionDetection.cpp   |  38 +++---
 src/motionDetection/MotionDetection.h     |   2 +-
 src/multiColorLight/MultiColorLight.cpp   |  16 +--
 src/multiColorLight/MultiColorLight.h     |   2 +-
 src/power/{Power.cpp => PowerManager.cpp} | 146 +++++++++++-----------
 src/power/{Power.h => PowerManager.h}     |  12 +-
 src/power/PowerScheduler.cpp              |   8 +-
 20 files changed, 152 insertions(+), 152 deletions(-)
 rename src/power/{Power.cpp => PowerManager.cpp} (70%)
 rename src/power/{Power.h => PowerManager.h} (98%)

diff --git a/src/Dezibot.h b/src/Dezibot.h
index 4d4274c..ef3f691 100644
--- a/src/Dezibot.h
+++ b/src/Dezibot.h
@@ -19,7 +19,7 @@
 #include "infraredLight/InfraredLight.h"
 #include "communication/Communication.h"
 #include "display/Display.h"
-#include "power/Power.h"
+#include "power/PowerManager.h"
 
 
 class Dezibot {
diff --git a/src/colorDetection/ColorDetection.cpp b/src/colorDetection/ColorDetection.cpp
index 19dcde9..06b3bcf 100644
--- a/src/colorDetection/ColorDetection.cpp
+++ b/src/colorDetection/ColorDetection.cpp
@@ -1,7 +1,7 @@
 #include "ColorDetection.h"
 
 void ColorDetection::begin(void) {
-  if (!Power::waitForCurrentAllowance(
+  if (!PowerManager::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::RGBW_SENSOR,
           PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW,
           COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -100,4 +100,4 @@ float ColorDetection::modelCurrentConsumption() {
 float ColorDetection::modelChargeConsumption(uint16_t durationMs) {
   return (PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW *
          durationMs) / 10e6f;
-}
\ No newline at end of file
+}
diff --git a/src/colorDetection/ColorDetection.h b/src/colorDetection/ColorDetection.h
index 4829e79..4323723 100644
--- a/src/colorDetection/ColorDetection.h
+++ b/src/colorDetection/ColorDetection.h
@@ -11,7 +11,7 @@
 
 #ifndef ColorDetection_h
 #define ColorDetection_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include <Arduino.h>
 #include <Wire.h>
 #include <stdint.h>
diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index 352e265..146e8ca 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -45,7 +45,7 @@ void nodeTimeAdjustedCallback(int32_t offset) {
 
 void vTaskUpdate(void *pvParameters) {
   for (;;) {
-    if (Power::waitForCurrentAllowance(
+    if (PowerManager::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::WIFI,
             PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
                 PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
@@ -54,7 +54,7 @@ void vTaskUpdate(void *pvParameters) {
     } else {
       ESP_LOGW(TAG, "Skipping mesh update after not being granted power");
     }
-    Power::waitForCurrentAllowance(
+    PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::WIFI,
         PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
         MESH_MAX_EXECUTION_DELAY_MS, NULL);
@@ -76,7 +76,7 @@ void Communication::begin(void) {
   mesh.setDebugMsgTypes(
       ERROR |
       STARTUP); // set before init() so that you can see startup messages
-  if (!Power::waitForCurrentAllowance(
+  if (!PowerManager::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::WIFI,
           PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
           MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -95,4 +95,4 @@ void Communication::begin(void) {
   xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass,
               tskIDLE_PRIORITY, &xHandle);
   configASSERT(xHandle);
-};
\ No newline at end of file
+};
diff --git a/src/communication/Communication.h b/src/communication/Communication.h
index 6f75d5c..090d99f 100644
--- a/src/communication/Communication.h
+++ b/src/communication/Communication.h
@@ -1,7 +1,7 @@
 
 #ifndef Communication_h
 #define Communication_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include <Arduino.h>
 #include <painlessMesh.h>
 #include <stdint.h>
diff --git a/src/display/Display.cpp b/src/display/Display.cpp
index ef906d9..342c48c 100644
--- a/src/display/Display.cpp
+++ b/src/display/Display.cpp
@@ -4,7 +4,7 @@
  * @brief Adds the ability to print to the display of the robot.
  * @version 0.1
  * @date 2024-06-05
- * 
+ *
  * @copyright Copyright (c) 2024
  */
 
@@ -14,7 +14,7 @@
 
 
 void Display::begin(void){
-    if(!Power::waitForCurrentAllowance(
+    if(!PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::DISPLAY_OLED, PowerParameters::CurrentConsumptions::CURRENT_DISPLAY
         ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL)){
             ESP_LOGE(TAG,"Could not get power for Display");
@@ -29,7 +29,7 @@ void Display::begin(void){
     sendDisplayCMD(stopCompleteOn);
     /*which pixels are bright: normal = 1s are bright, inverese= 0s are bright*/
     sendDisplayCMD( setNormalMode);
- 
+
     sendDisplayCMD( setOscFreq);
     sendDisplayCMD(0x80);
 
@@ -74,7 +74,7 @@ void Display::updateLine(uint charAmount)
     if(charAmount+this->charsOnCurrLine>16)
     {
         this->currLine = (this->currLine+((charAmount+this->charsOnCurrLine)/16))%8;
-        this->charsOnCurrLine = (charAmount+this->charsOnCurrLine)%17; //there can be 0-16 chars on one line, so the 17th char is on next line 
+        this->charsOnCurrLine = (charAmount+this->charsOnCurrLine)%17; //there can be 0-16 chars on one line, so the 17th char is on next line
     }
     else
     {
@@ -85,7 +85,7 @@ void Display::updateLine(uint charAmount)
 void Display::print(char *value){
     char *nextchar;
 	/* write data to the buffer */
-    while(value && *value != '\0') //check if pointer is still valid and string is not terminated 
+    while(value && *value != '\0') //check if pointer is still valid and string is not terminated
         {
 	        //check if next character is a linebreak
             if(*value=='\n')
@@ -114,7 +114,7 @@ void Display::print(char *value){
                 }
                 Wire.endTransmission();
             }
-            value++;     
+            value++;
         }
 };
 
@@ -186,4 +186,4 @@ float modelCurrentConsumption() {
 
 float modelChargeConsumptionOn(uint16_t durationMs) {
     return PowerParameters::CurrentConsumptions::CURRENT_DISPLAY * durationMs * 10e6;
-};
\ No newline at end of file
+};
diff --git a/src/display/Display.h b/src/display/Display.h
index 387cc35..8c9bc47 100644
--- a/src/display/Display.h
+++ b/src/display/Display.h
@@ -11,7 +11,7 @@
 
 #ifndef Display_h
 #define Display_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include "DisplayCMDs.h"
 #include <Arduino.h>
 #include <stdint.h>
diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp
index a9b9b06..a09f1a3 100644
--- a/src/infraredLight/InfraredLED.cpp
+++ b/src/infraredLight/InfraredLED.cpp
@@ -13,7 +13,7 @@ InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel)
 };
 
 void InfraredLED::begin(void){
-   //we want to change frequency instead of 
+   //we want to change frequency instead of
     pwmTimer = ledc_timer_config_t{
         .speed_mode = pwmSpeedMode,
         .duty_resolution = DUTY_RESOLUTION,
@@ -47,7 +47,7 @@ void InfraredLED::setState(bool state){
     ledc_set_freq(pwmSpeedMode,timer,1);
     if (state) {
       if (this->ledPin == IR_BOTTOM_PIN) {
-        if (!Power::waitForCurrentAllowance(
+        if (!PowerManager::waitForCurrentAllowance(
                 PowerParameters::PowerConsumers::LED_IR_BOTTOM,
                 PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM,
                 IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -56,7 +56,7 @@ void InfraredLED::setState(bool state){
           return;
         }
       } else if (this->ledPin == IR_FRONT_PIN) {
-        if (!Power::waitForCurrentAllowance(
+        if (!PowerManager::waitForCurrentAllowance(
                 PowerParameters::PowerConsumers::LED_IR_FRONT,
                 PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT,
                 IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -68,14 +68,14 @@ void InfraredLED::setState(bool state){
         ledc_set_duty(pwmSpeedMode,channel,1023);
     } else {
       if (this->ledPin == IR_BOTTOM_PIN) {
-        Power::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM);
+        PowerManager::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM);
       } else {
-        Power::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT);
+        PowerManager::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT);
       }
         ledc_set_duty(pwmSpeedMode,channel,0);
     }
     ledc_update_duty(pwmSpeedMode,channel);
-    
+
 };
 
 void InfraredLED::sendFrequency(uint16_t frequency){
@@ -83,12 +83,12 @@ void InfraredLED::sendFrequency(uint16_t frequency){
   // Float to force float division without casting
   constexpr float resolution = 1 << DUTY_RESOLUTION;
   if (this->ledPin == IR_BOTTOM_PIN) {
-    Power::waitForCurrentAllowance(
+    PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::LED_IR_BOTTOM,
         this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
         NULL);
   } else if (this->ledPin == IR_FRONT_PIN) {
-    Power::waitForCurrentAllowance(
+    PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::LED_IR_FRONT,
         this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
         NULL);
diff --git a/src/infraredLight/InfraredLight.h b/src/infraredLight/InfraredLight.h
index fae6df3..9ef00c5 100644
--- a/src/infraredLight/InfraredLight.h
+++ b/src/infraredLight/InfraredLight.h
@@ -10,7 +10,7 @@
  */
 #ifndef InfraredLight_h
 #define InfraredLight_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include "driver/ledc.h"
 #include <Arduino.h>
 #include <stdint.h>
diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index 498d2c2..1fbe3fa 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -27,7 +27,7 @@ photoTransistors LightDetection::getBrightest(ptType type){
     photoTransistors maxSensor;
     uint16_t maxReading = 0;
     uint16_t currentReading = 0;
-    
+
     if (type == IR){
         maxSensor = IR_FRONT;
        for(const auto pt : allIRPTs){
@@ -47,15 +47,15 @@ photoTransistors LightDetection::getBrightest(ptType type){
             }
         }
     }
-    
+
     return maxSensor;
 };
 
 uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measurments, uint32_t timeBetween){
-    
+
     TickType_t xLastWakeTime = xTaskGetTickCount();
     TickType_t frequency = timeBetween / portTICK_PERIOD_MS;
-    uint64_t cumulatedResult = 0; 
+    uint64_t cumulatedResult = 0;
     for(int i = 0; i < measurments; i++){
         cumulatedResult += LightDetection::getValue(sensor);
         xTaskDelayUntil(&xLastWakeTime,frequency);
@@ -64,7 +64,7 @@ uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measu
 };
 
 void LightDetection::beginInfrared(void){
-    if(!Power::waitForCurrentAllowance(
+    if(!PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::PT_IR,
         PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
         LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -80,7 +80,7 @@ void LightDetection::beginInfrared(void){
 };
 
 void LightDetection::beginDaylight(void){
-    if(!Power::waitForCurrentAllowance(
+    if(!PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::PT_DL,
         PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
         LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -113,7 +113,7 @@ uint16_t LightDetection::readIRPT(photoTransistors sensor){
     default:
         break;
     }
-    //Power::releaseCurrent(PowerParameters::PowerConsumers::PT_IR);
+    //PowerManager::releaseCurrent(PowerParameters::PowerConsumers::PT_IR);
     //digitalWrite(IR_PT_ENABLE,LOW);
     return result;
 };
@@ -132,7 +132,7 @@ uint16_t LightDetection::readDLPT(photoTransistors sensor){
     default:
         break;
     }
-    Power::releaseCurrent(PowerParameters::PowerConsumers::PT_DL);
+    PowerManager::releaseCurrent(PowerParameters::PowerConsumers::PT_DL);
     digitalWrite(DL_PT_ENABLE,LOW);
     return result;
 };
@@ -148,4 +148,4 @@ float LightDetection::modelCurrentConsumption(photoTransistors sensor){
 float LightDetection::modelChargeConsumptionOn(photoTransistors sensor,
                                                uint16_t durationMs) {
   return (LightDetection::modelCurrentConsumption(sensor) * durationMs) / 10e6f;
-}
\ No newline at end of file
+}
diff --git a/src/lightDetection/LightDetection.h b/src/lightDetection/LightDetection.h
index b79fdac..7597e78 100644
--- a/src/lightDetection/LightDetection.h
+++ b/src/lightDetection/LightDetection.h
@@ -11,7 +11,7 @@
 
 #ifndef LightDetection_h
 #define LightDetection_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include <Arduino.h>
 #include <stdint.h>
 
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index c4c6f7d..e1bb259 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -17,7 +17,7 @@
 #include <freertos/task.h>
 #include "driver/ledc.h"
 #include "motionDetection/MotionDetection.h"
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #define LEDC_MODE          LEDC_LOW_SPEED_MODE
 #define TIMER              LEDC_TIMER_2
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index 6482ded..78fc1f2 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -1,5 +1,5 @@
 #include "Motion.h"
-#include "power/Power.h"
+#include "power/PowerManager.h"
 
 #define MOTOR_LEFT_PIN 12
 #define MOTOR_RIGHT_PIN 11
@@ -30,7 +30,7 @@ void Motor::begin(void){
 bool Motor::setSpeed(uint16_t duty) {
   const float current = this->modelCurrentConsumption(duty);
   if (this->pin == MOTOR_LEFT_PIN) {
-    if (!Power::waitForCurrentAllowance(
+    if (!PowerManager::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::MOTOR_LEFT, current,
             MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
       ESP_LOGW(TAG,
@@ -40,7 +40,7 @@ bool Motor::setSpeed(uint16_t duty) {
       return false;
     }
   } else {
-    if (!Power::waitForCurrentAllowance(
+    if (!PowerManager::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
             MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
       ESP_LOGW(TAG,
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index 178ef9f..cfe2c77 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -6,7 +6,7 @@ MotionDetection::MotionDetection(){
 };
 
 void MotionDetection::begin(void){
-  if (!Power::waitForCurrentAllowance(
+  if (!PowerManager::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::IMU,
           PowerParameters::CurrentConsumptions::CURRENT_IMU,
           IMU_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -18,7 +18,7 @@ void MotionDetection::begin(void){
     handler->begin(36,37,35,34);
     // set Accel and Gyroscop to Low Noise
     this->writeRegister(PWR_MGMT0,0x1F);
-    //busy Wait for startup 
+    //busy Wait for startup
     delayMicroseconds(250);
     //set accelconfig
     this->writeRegister(0x21,0x05);
@@ -28,7 +28,7 @@ void MotionDetection::begin(void){
     this->writeRegister(0x23,0x37);
     //Enable Gyro and Acceldata in FIFO
     this->initFIFO();
-    // TODO: Accelerometer Startup Time From sleep mode to valid data 10 
+    // TODO: Accelerometer Startup Time From sleep mode to valid data 10
 };
 void MotionDetection::end(void){
     this->writeRegister(PWR_MGMT0,0x00);
@@ -53,12 +53,12 @@ IMUResult MotionDetection::getRotation(){
 };
 float MotionDetection::getTemperature(){
     int16_t raw_temperatur = readRegister(REG_TEMP_HIGH)<<8;
-    raw_temperatur |= readRegister(REG_TEMP_LOW); 
+    raw_temperatur |= readRegister(REG_TEMP_LOW);
     return raw_temperatur/128 +25;
 };
 
 int8_t MotionDetection::getWhoAmI(){
-    return readRegister(WHO_AM_I);    
+    return readRegister(WHO_AM_I);
 };
 
 bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
@@ -122,7 +122,7 @@ Orientation MotionDetection::getTilt(){
         }
         //yAngle = -1*yAngle-90;
     }
-          
+
 
     return Orientation{xAngle,yAngle};
 
@@ -175,7 +175,7 @@ uint8_t MotionDetection::readRegister(uint8_t reg){
     result = handler->transfer(cmdRead(reg));
     result = handler->transfer(0x00);
     digitalWrite(34,HIGH);
-    handler->endTransaction(); 
+    handler->endTransaction();
     return result;
 };
 
@@ -207,7 +207,7 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_
     }
     uint8_t result = this->readRegister(PWR_MGMT0);
     Serial.print("MADDR_W: ");
-    Serial.println(readRegister(MADDR_W)); 
+    Serial.println(readRegister(MADDR_W));
     //set Idle Bit
     this->writeRegister(PWR_MGMT0,result|0x10);
     switch(bank){
@@ -254,7 +254,7 @@ void MotionDetection::initFIFO(){
 uint MotionDetection::getDataFromFIFO(FIFO_Package* buffer){
     int16_t fifocount = 0;
     int8_t fifohigh = this->readRegister(FIFO_COUNTH);
-    int8_t fifolow = this->readRegister(FIFO_COUNTL);  
+    int8_t fifolow = this->readRegister(FIFO_COUNTL);
     fifocount = (fifohigh<<8)|fifolow;
     //fifocount |= this->readRegister(FIFO_COUNTL);
     //fifocount = (this->readRegister(FIFO_COUNTH)<<8);
@@ -267,20 +267,20 @@ uint MotionDetection::getDataFromFIFO(FIFO_Package* buffer){
     handler->transfer(buf,16*fifocount);
     digitalWrite(34,HIGH);
     handler->endTransaction();
-    
+
     writeRegister(0x02,0x04);
     delayMicroseconds(10);
 
     for(int i = 0; i<fifocount;i++){
         buffer[i].header = buf[0x00+16*i];
-        buffer[i].accel.x = (buf[0x02+16*i]<<8)|buf[0x01+16*i]; 
-        buffer[i].accel.y = (buf[0x04+16*i]<<8)|buf[0x03+16*i]; 
-        buffer[i].accel.z = (buf[0x06+16*i]<<8)|buf[0x05+16*i]; 
-        buffer[i].gyro.x = (buf[0x08+16*i]<<8)|buf[0x07+16*i]; 
-        buffer[i].gyro.y = (buf[0x0A+16*i]<<8)|buf[0x09+16*i]; 
-        buffer[i].gyro.z = (buf[0x0C+16*i]<<8)|buf[0x0B+16*i]; 
-        buffer[i].temperature = buf[0x0D+16*i]; 
-        buffer[i].timestamp = (buf[0x0F+16*i]<<8)|buf[0x0E +16*i]; 
+        buffer[i].accel.x = (buf[0x02+16*i]<<8)|buf[0x01+16*i];
+        buffer[i].accel.y = (buf[0x04+16*i]<<8)|buf[0x03+16*i];
+        buffer[i].accel.z = (buf[0x06+16*i]<<8)|buf[0x05+16*i];
+        buffer[i].gyro.x = (buf[0x08+16*i]<<8)|buf[0x07+16*i];
+        buffer[i].gyro.y = (buf[0x0A+16*i]<<8)|buf[0x09+16*i];
+        buffer[i].gyro.z = (buf[0x0C+16*i]<<8)|buf[0x0B+16*i];
+        buffer[i].temperature = buf[0x0D+16*i];
+        buffer[i].timestamp = (buf[0x0F+16*i]<<8)|buf[0x0E +16*i];
     }
     return fifocount;
 };
@@ -301,4 +301,4 @@ float MotionDetection::modelCurrentConsumption(){
 
 float MotionDetection::modelChargeConsumption(uint16_t durationMs) {
   return (this->modelCurrentConsumption() * durationMs) / 10e6f;
-}
\ No newline at end of file
+}
diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h
index 6b87df7..8bdd63b 100644
--- a/src/motionDetection/MotionDetection.h
+++ b/src/motionDetection/MotionDetection.h
@@ -10,7 +10,7 @@
  */
 #ifndef MotionDetection_h
 #define MotionDetection_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include "IMU_CMDs.h"
 #include <Arduino.h>
 #include <SPI.h>
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index 28b9024..e439252 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -6,15 +6,15 @@ MultiColorLight::MultiColorLight()
       };
 
 void MultiColorLight::begin(void) {
-  if (!Power::waitForCurrentAllowance(
+  if (!PowerManager::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT,
           PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
           MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
-      Power::waitForCurrentAllowance(
+      PowerManager::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT,
           PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
           MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
-      Power::waitForCurrentAllowance(
+      PowerManager::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::LED_RGB_BOTTOM,
           PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
           MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
@@ -33,7 +33,7 @@ void MultiColorLight::setLed(uint8_t index, uint32_t color) {
   float totalConsumption = modelCurrentConsumption(normalizedColor);
   switch (index) {
   case 0:
-    if (!Power::waitForCurrentAllowance(
+    if (!PowerManager::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT,
             totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
       ESP_LOGW(TAG,
@@ -44,7 +44,7 @@ void MultiColorLight::setLed(uint8_t index, uint32_t color) {
     }
     break;
   case 1:
-    if (!Power::waitForCurrentAllowance(
+    if (!PowerManager::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption,
             MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
       ESP_LOGW(TAG,
@@ -55,7 +55,7 @@ void MultiColorLight::setLed(uint8_t index, uint32_t color) {
     }
     break;
   case 2:
-    if (!Power::waitForCurrentAllowance(
+    if (!PowerManager::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption,
             MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
       ESP_LOGW(TAG,
@@ -235,7 +235,7 @@ float MultiColorLight::modelChargeConsumption(leds leds, uint32_t color,
     // TODO logging
     break;
   }
-  return ledsConsumption; 
+  return ledsConsumption;
 };
 
 float MultiColorLight::modelChargeConsumption(leds leds, uint8_t red,
@@ -243,4 +243,4 @@ float MultiColorLight::modelChargeConsumption(leds leds, uint8_t red,
                                                  uint16_t durationMs) {
   return MultiColorLight::modelChargeConsumption(
       leds, MultiColorLight::color(red, green, blue), durationMs);
-};
\ No newline at end of file
+};
diff --git a/src/multiColorLight/MultiColorLight.h b/src/multiColorLight/MultiColorLight.h
index 76ba711..b526e86 100644
--- a/src/multiColorLight/MultiColorLight.h
+++ b/src/multiColorLight/MultiColorLight.h
@@ -11,7 +11,7 @@
  */
 #ifndef MultiColorLight_h
 #define MultiColorLight_h
-#include "../power/Power.h"
+#include "../power/PowerManager.h"
 #include "ColorConstants.h"
 #include <Adafruit_NeoPixel.h>
 
diff --git a/src/power/Power.cpp b/src/power/PowerManager.cpp
similarity index 70%
rename from src/power/Power.cpp
rename to src/power/PowerManager.cpp
index 09ce96c..760cfb6 100644
--- a/src/power/Power.cpp
+++ b/src/power/PowerManager.cpp
@@ -1,5 +1,5 @@
 /**
- * @file Power.h
+ * @file PowerManager.h
  * @author Phillip Kühne
  * @brief This component provides utilities for keeping track of power usage
  * consumption.
@@ -7,19 +7,19 @@
  * @date 2024-11-23
  */
 
-#include "Power.h"
+#include "PowerManager.h"
 
-SemaphoreHandle_t Power::powerMutex = NULL;
+SemaphoreHandle_t PowerManager::powerMutex = NULL;
 
 void vTaskUpdatePowerState(void *pvParameters) {
   for (;;) {
     ESP_LOGV(TAG, "Updating Power State...");
-    Power::updatePowerStateHandler();
+    PowerManager::updatePowerStateHandler();
     vTaskDelay(pdMS_TO_TICKS(10));
   }
 }
 
-void Power::begin() {
+void PowerManager::begin() {
   // Check if another instance of us already initialized the power scheduler,
   // if not, we will do it.
   ESP_LOGI(TAG, "Initializing Power Management");
@@ -38,8 +38,8 @@ void Power::begin() {
     powerScheduler = &PowerScheduler::getPowerScheduler(
         PowerParameters::Battery::CELL_CURRENT_1C_MA,
         PowerParameters::Battery::CELL_CURRENT_2C_MA);
-    Power::initPowerState();
-    Power::recalculateCurrentBudgets();
+    PowerManager::initPowerState();
+    PowerManager::recalculateCurrentBudgets();
     TaskHandle_t xHandle = NULL;
     xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
                 tskIDLE_PRIORITY, &xHandle);
@@ -54,26 +54,26 @@ void Power::begin() {
   }
 }
 
-float Power::getFreeLimitCurrentBudget() {
+float PowerManager::getFreeLimitCurrentBudget() {
   return powerScheduler->getFreeLimitCurrentBudget();
 }
 
-float Power::getFreeMaximumCurrentBudget() {
+float PowerManager::getFreeMaximumCurrentBudget() {
   return powerScheduler->getFreeMaximumCurrentBudget();
 }
 
-bool Power::tryAccquireCurrentAllowance(
+bool PowerManager::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
     uint16_t requestedDurationMs) {
   return powerScheduler->tryAccquireCurrentAllowance(consumer, neededCurrent,
                                                      requestedDurationMs);
 }
 
-void Power::releaseCurrent(PowerParameters::PowerConsumers consumer) {
+void PowerManager::releaseCurrent(PowerParameters::PowerConsumers consumer) {
   powerScheduler->releaseCurrent(consumer);
 }
 
-bool Power::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
+bool PowerManager::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                     uint16_t neededCurrent,
                                     uint16_t maxSlackTimeMs,
                                     uint16_t requestedDurationMs) {
@@ -81,15 +81,15 @@ bool Power::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
       consumer, neededCurrent, maxSlackTimeMs, requestedDurationMs);
 }
 
-void Power::beginPermanentDeepSleep(void) {
+void PowerManager::beginPermanentDeepSleep(void) {
   return powerScheduler->beginPermanentDeepSleep();
 }
 
-float Power::getCurrentCurrent(void) {
+float PowerManager::getCurrentCurrent(void) {
   return powerScheduler->getCurrentCurrent();
 }
 
-float Power::getBatteryCurrent(void) {
+float PowerManager::getBatteryCurrent(void) {
   const float i_3v3 = getCurrentCurrent();
   const float u_3v3 = 3.3;
   const float u_bat = getBatteryVoltage();
@@ -97,15 +97,15 @@ float Power::getBatteryCurrent(void) {
   return (u_3v3 * i_3v3) / (u_bat * eta);
 }
 
-void Power::recalculateCurrentBudgets(void) {
+void PowerManager::recalculateCurrentBudgets(void) {
   return powerScheduler->recalculateCurrentBudgets();
 }
 
-float Power::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
+float PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
   return powerScheduler->getConsumerCurrent(consumer);
 }
 
-float Power::getBatteryVoltage() {
+float PowerManager::getBatteryVoltage() {
   // Get the battery voltage from the ADC and convert it to a voltage
   // using the voltage divider.
   pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT);
@@ -127,14 +127,14 @@ float Power::getBatteryVoltage() {
   return batteryVoltage;
 }
 
-float Power::getBatteryChargePercent() { return percentRemaining; }
+float PowerManager::getBatteryChargePercent() { return percentRemaining; }
 
-float Power::getBatteryChargeCoulombs() { return coloumbsRemaining; }
+float PowerManager::getBatteryChargeCoulombs() { return coloumbsRemaining; }
 
-float Power::getBatteryVoltageChargePercent() {
+float PowerManager::getBatteryVoltageChargePercent() {
   // Directly get the battery voltage, correct the curve with an offset and
   // calculate the charge state based on the discharge curve.
-  float batteryVoltage = getBatteryVoltage() + Power::fullVoltageOffset;
+  float batteryVoltage = getBatteryVoltage() + PowerManager::fullVoltageOffset;
   float chargeState = 0;
   // Clamp edge cases
   if (batteryVoltage >=
@@ -163,16 +163,16 @@ float Power::getBatteryVoltageChargePercent() {
   }
 }
 
-void Power::updatePowerStateHandler() {
+void PowerManager::updatePowerStateHandler() {
 
   // Update supply and charge state flags
-  Power::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS);
-  Power::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
+  PowerManager::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS);
+  PowerManager::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
 
   // If the battery is charging and fully charged
-  if (Power::busPowered && !Power::chargingState) {
+  if (PowerManager::busPowered && !PowerManager::chargingState) {
     // Calibrate voltage offset on full Battery
-    Power::fullVoltageOffset = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0] - getBatteryVoltage();
+    PowerManager::fullVoltageOffset = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0] - getBatteryVoltage();
   }
 
   ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState);
@@ -184,7 +184,7 @@ void Power::updatePowerStateHandler() {
   float coloumbsConsumedSinceLastUpdate;
 
   // Calculate remaining battery charge in Coulombs based on current and time
-  if (!Power::busPowered) {
+  if (!PowerManager::busPowered) {
     coloumbsConsumedSinceLastUpdate =
         (currentCurrent / 1000) *
         ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0);
@@ -196,7 +196,7 @@ void Power::updatePowerStateHandler() {
 
   // If current flow is close enough to reference, get battery charge state via
   // voltage curve
-  if (!Power::busPowered) {
+  if (!PowerManager::busPowered) {
     if ((currentCurrent > (referenceCurrentMa * 0.6)) &&
         (currentCurrent < (referenceCurrentMa * 1.4))) {
       // Get battery charge state from voltage curve
@@ -233,21 +233,21 @@ void Power::updatePowerStateHandler() {
   for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) {
     sampleSum += lastSOC[i];
   }
-  Power::percentRemaining =
+  PowerManager::percentRemaining =
       sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
 
   // Update last update time
-  Power::lastPowerStateUpdate = xTaskGetTickCount();
+  PowerManager::lastPowerStateUpdate = xTaskGetTickCount();
 
   // Update the available current (changes based on battery state of charge)
-  Power::powerScheduler->recalculateCurrentBudgets();
+  PowerManager::powerScheduler->recalculateCurrentBudgets();
   ESP_LOGD(TAG, "Current: %f mA, Charge: %f Coulombs, %f %%", currentCurrent,
            coloumbsRemaining, percentRemaining);
 
   return;
 }
 
-float Power::getMax3V3Current() {
+float PowerManager::getMax3V3Current() {
   // Conversion from Thesis
   float u_bat = getBatteryVoltage();
   float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA;
@@ -256,7 +256,7 @@ float Power::getMax3V3Current() {
   return (u_bat * i_bat * eta) / u_3v3;
 }
 
-void Power::addSoCSample(float soc) {
+void PowerManager::addSoCSample(float soc) {
   PowerMutex lock(powerMutex);
   if (!lock.isLocked()) {
     ESP_LOGE(TAG, "Could not take power to add SoC sample");
@@ -267,7 +267,7 @@ void Power::addSoCSample(float soc) {
   lastSOC[latestSoCIndex] = soc;
 }
 
-void Power::initPowerState(void) {
+void PowerManager::initPowerState(void) {
   // Initialize the power state
   lastPowerStateUpdate = xTaskGetTickCount();
   // TODO: Get initial battery charge state based on voltage, set coloumbs based
@@ -288,88 +288,88 @@ void Power::initPowerState(void) {
   chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
 }
 
-void Power::dumpPowerStatistics() {
+void PowerManager::dumpPowerStatistics() {
   Serial.printf("======== Dezibot Power Statistics ========\r\n");
-  Serial.printf("Current: %f mA\r\n", Power::getCurrentCurrent());
-  Serial.printf("Battery Voltage: %f V\r\n", Power::getBatteryVoltage());
-  Serial.printf("Battery Charge: %f %%\r\n", Power::getBatteryChargePercent());
+  Serial.printf("Current: %f mA\r\n", PowerManager::getCurrentCurrent());
+  Serial.printf("Battery Voltage: %f V\r\n", PowerManager::getBatteryVoltage());
+  Serial.printf("Battery Charge: %f %%\r\n", PowerManager::getBatteryChargePercent());
   Serial.printf("Battery Charge: %f Coulombs\r\n",
-                Power::getBatteryChargeCoulombs());
+                PowerManager::getBatteryChargeCoulombs());
   Serial.printf("Max 3.3V Current in this state (1C, 2C): %f mA, %f mA \r\n",
-                Power::getMax3V3Current(), Power::getMax3V3Current() * 2);
+                PowerManager::getMax3V3Current(), PowerManager::getMax3V3Current() * 2);
   Serial.printf("=========================================\r\n");
 }
 
-void Power::dumpConsumerStatistics() {
+void PowerManager::dumpConsumerStatistics() {
   Serial.printf("======== Dezibot Consumer Statistics ========\r\n");
-  Serial.printf("ESP: %f mA\r\n", Power::getConsumerCurrent(
+  Serial.printf("ESP: %f mA\r\n", PowerManager::getConsumerCurrent(
                                       PowerParameters::PowerConsumers::ESP));
-  Serial.printf("WIFI: %f mA\r\n", Power::getConsumerCurrent(
+  Serial.printf("WIFI: %f mA\r\n", PowerManager::getConsumerCurrent(
                                        PowerParameters::PowerConsumers::WIFI));
   Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n",
-                Power::getConsumerCurrent(
+                PowerManager::getConsumerCurrent(
                     PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT));
   Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n",
-                Power::getConsumerCurrent(
+                PowerManager::getConsumerCurrent(
                     PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT));
   Serial.printf("LED_RGB_BOTTOM: %f mA\r\n",
-                Power::getConsumerCurrent(
+                PowerManager::getConsumerCurrent(
                     PowerParameters::PowerConsumers::LED_RGB_BOTTOM));
   Serial.printf(
       "RGBW_SENSOR: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
   Serial.printf("LED_IR_BOTTOM: %f mA\r\n",
-                Power::getConsumerCurrent(
+                PowerManager::getConsumerCurrent(
                     PowerParameters::PowerConsumers::LED_IR_BOTTOM));
   Serial.printf(
       "LED_IR_FRONT: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
   Serial.printf(
       "PT_IR: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
   Serial.printf(
       "PT_DL: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
   Serial.printf(
       "LED_UV: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
   Serial.printf(
       "DISPLAY_OLED: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
   Serial.printf(
       "MOTOR_LEFT: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
   Serial.printf(
       "MOTOR_RIGHT: %f mA\r\n",
-      Power::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
-  Serial.printf("IMU: %f mA\r\n", Power::getConsumerCurrent(
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
+  Serial.printf("IMU: %f mA\r\n", PowerManager::getConsumerCurrent(
                                       PowerParameters::PowerConsumers::IMU));
   Serial.printf("=============================================\r\n");
 }
 
-bool Power::isUSBPowered() { return busPowered; }
+bool PowerManager::isUSBPowered() { return busPowered; }
 
-bool Power::isBatteryPowered() { return !busPowered; }
+bool PowerManager::isBatteryPowered() { return !busPowered; }
 
-bool Power::isBatteryCharging() { return chargingState && busPowered; }
+bool PowerManager::isBatteryCharging() { return chargingState && busPowered; }
 
-bool Power::isBatteryDischarging() { return !chargingState && !busPowered; }
+bool PowerManager::isBatteryDischarging() { return !chargingState && !busPowered; }
 
-bool Power::isBatteryFullyCharged() { return !chargingState && busPowered; }
-int Power::latestSoCIndex = 0;
-float Power::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0};
-TickType_t Power::lastPowerStateUpdate = 0;
-float Power::coloumbsRemaining =
+bool PowerManager::isBatteryFullyCharged() { return !chargingState && busPowered; }
+int PowerManager::latestSoCIndex = 0;
+float PowerManager::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0};
+TickType_t PowerManager::lastPowerStateUpdate = 0;
+float PowerManager::coloumbsRemaining =
     PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
-float Power::percentRemaining = 100.0;
-PowerScheduler *Power::powerScheduler = nullptr;
+float PowerManager::percentRemaining = 100.0;
+PowerScheduler *PowerManager::powerScheduler = nullptr;
 
-bool Power::busPowered = false;
+bool PowerManager::busPowered = false;
 
-bool Power::chargingState = false;
+bool PowerManager::chargingState = false;
 
-float Power::fullVoltageOffset = 0;
+float PowerManager::fullVoltageOffset = 0;
 
-Power::Power() {}
+PowerManager::PowerManager() {}
 
-Power::~Power() {}
\ No newline at end of file
+PowerManager::~PowerManager() {}
diff --git a/src/power/Power.h b/src/power/PowerManager.h
similarity index 98%
rename from src/power/Power.h
rename to src/power/PowerManager.h
index 34be7cd..c5222ec 100644
--- a/src/power/Power.h
+++ b/src/power/PowerManager.h
@@ -1,5 +1,5 @@
 /**
- * @file Power.h
+ * @file PowerManager.h
  * @author Phillip Kühne
  * @brief This component provides utilities for keeping track of power usage
  * consumption.
@@ -21,7 +21,7 @@
 
 enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
-class Power {
+class PowerManager {
 
 private:
   static SemaphoreHandle_t powerMutex;
@@ -86,8 +86,8 @@ protected:
 
 public:
   static void begin(void);
-  Power();
-  ~Power();
+  PowerManager();
+  ~PowerManager();
   /// @brief Get the current free current budget (to C1 discharge)
   /// @return the amount of power that is currently available (in mA)
   static float getFreeLimitCurrentBudget(void);
@@ -193,6 +193,6 @@ public:
 
 };
 
-extern Power power;
+extern PowerManager power;
 
-#endif // Power
\ No newline at end of file
+#endif // Power
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 2166073..0dbbbef 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -11,7 +11,7 @@
  */
 
 #include "PowerScheduler.h"
-#include "Power.h"
+#include "PowerManager.h"
 
 bool PowerScheduler::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, float neededCurrent,
@@ -192,9 +192,9 @@ void PowerScheduler::recalculateCurrentBudgets(void) {
 
   // Get the respective maximums and subtract currently flowing currents
   ESP_LOGI(TAG, "Recalculating current budgets...");
-  float tempFreeLimitCurrentBudget = Power::getMax3V3Current();
+  float tempFreeLimitCurrentBudget = PowerManager::getMax3V3Current();
   ESP_LOGI(TAG, "Got max 3V3 current: %.2f", tempFreeLimitCurrentBudget);
-  float tempFreeMaximumCurrentBudget = Power::getMax3V3Current() * 2;
+  float tempFreeMaximumCurrentBudget = PowerManager::getMax3V3Current() * 2;
   PowerSchedulerMutex lock(powerSchedulerMutex);
   if (lock.isLocked() == false) {
     return;
@@ -315,4 +315,4 @@ PowerScheduler::~PowerScheduler() {
   if (powerSchedulerMutex != NULL) {
     vSemaphoreDelete(powerSchedulerMutex);
   }
-}
\ No newline at end of file
+}

From 7a7139360cae1e5f8dabba8bff8aee3ef91ea923 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 01:43:19 +0100
Subject: [PATCH 48/59] Fix mistakes

---
 src/Dezibot.h                         | 2 +-
 src/colorDetection/ColorDetection.h   | 4 ++--
 src/infraredLight/InfraredLED.cpp     | 8 +-------
 src/motion/Motion.h                   | 1 -
 src/multiColorLight/MultiColorLight.h | 4 ++--
 src/power/PowerManager.h              | 8 ++++----
 src/power/PowerScheduler.cpp          | 5 ++---
 src/power/PowerScheduler.h            | 2 +-
 8 files changed, 13 insertions(+), 21 deletions(-)

diff --git a/src/Dezibot.h b/src/Dezibot.h
index ef3f691..93353cd 100644
--- a/src/Dezibot.h
+++ b/src/Dezibot.h
@@ -34,7 +34,7 @@ public:
     InfraredLight infraredLight;
     Communication communication;
     Display display;
-    Power power;
+    PowerManager power;
     void begin(void);
 };
 
diff --git a/src/colorDetection/ColorDetection.h b/src/colorDetection/ColorDetection.h
index 4323723..4f2a7ce 100644
--- a/src/colorDetection/ColorDetection.h
+++ b/src/colorDetection/ColorDetection.h
@@ -73,7 +73,7 @@ public:
      * 
      * @return 
      */
-    float modelCurrentConsumption();
+    static float modelCurrentConsumption();
 
 
     /**
@@ -84,7 +84,7 @@ public:
      * @param durationMs
      * @return float
      */
-    float modelChargeConsumption(uint16_t durationMs);
+    static float modelChargeConsumption(uint16_t durationMs);
 
   protected:
     uint16_t readDoubleRegister(uint8_t regAddr);
diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp
index a09f1a3..386821d 100644
--- a/src/infraredLight/InfraredLED.cpp
+++ b/src/infraredLight/InfraredLED.cpp
@@ -80,8 +80,6 @@ void InfraredLED::setState(bool state){
 
 void InfraredLED::sendFrequency(uint16_t frequency){
   constexpr uint32_t duty = DUTY_CYCLE_FREQUENCY;
-  // Float to force float division without casting
-  constexpr float resolution = 1 << DUTY_RESOLUTION;
   if (this->ledPin == IR_BOTTOM_PIN) {
     PowerManager::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::LED_IR_BOTTOM,
@@ -111,8 +109,6 @@ float InfraredLED::modelCurrentConsumption(uint32_t duty){
 };
 
 float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) {
-  // Float to force float division without casting
-  constexpr float resolution = 1 << DUTY_RESOLUTION;
   if (this->ledPin == IR_BOTTOM_PIN) {
     return durationMs *
            (PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM) / 10e6f;
@@ -124,8 +120,6 @@ float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) {
 }
 
 float InfraredLED::modelChargeConsumptionSendFrequency(uint16_t durationMs) {
-  // Float to force float division without casting
-
-  return (durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY)) /
+    return (durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY)) /
          10e6f;
 }
diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index e1bb259..eb56d21 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -79,7 +79,6 @@ class Motor{
         uint8_t pin;
         ledc_timer_t timer;
         ledc_channel_t channel;
-        Power* powerManager;
         uint16_t duty;
         // The phase of the pwm signal, expressed as a number betweeen 0 and
         // the maximum value representable by the pwm timer resolution.
diff --git a/src/multiColorLight/MultiColorLight.h b/src/multiColorLight/MultiColorLight.h
index b526e86..9731145 100644
--- a/src/multiColorLight/MultiColorLight.h
+++ b/src/multiColorLight/MultiColorLight.h
@@ -31,7 +31,7 @@ class MultiColorLight {
 protected:
   static const uint16_t ledAmount = 3;
   static const int16_t ledPin = 48;
-  static const uint8_t maxBrightness = 150;
+  static const uint8_t defaultMaxBrightness = 150;
   Adafruit_NeoPixel rgbLeds;
   static constexpr int maximumExecutionDelayMs = 10;
 
@@ -203,7 +203,7 @@ private:
    * can be between 0 - maxBrightness
    */
   uint32_t normalizeColor(uint32_t color,
-                          uint8_t maxBrigthness = maxBrightness);
+                          uint8_t maxBrightness = defaultMaxBrightness);
 };
 
 #endif // MultiColorLight_h
\ No newline at end of file
diff --git a/src/power/PowerManager.h b/src/power/PowerManager.h
index c5222ec..362e7b7 100644
--- a/src/power/PowerManager.h
+++ b/src/power/PowerManager.h
@@ -14,8 +14,8 @@
 #include "driver/adc.h"
 #include "esp_adc/adc_oneshot.h"
 #include "freertos/semphr.h"
-#ifndef Power_h
-#define Power_h
+#ifndef PowerManager_h
+#define PowerManager_h
 
 #define TAG "Power"
 
@@ -47,7 +47,7 @@ private:
         xSemaphoreGive(mutex);
       }
     }
-    bool isLocked() { return locked; }
+    bool isLocked() const { return locked; }
   };
 
 protected:
@@ -103,7 +103,7 @@ public:
   /// @return whether the current could be successfully allocated
   static bool
   tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                              uint16_t neededcurrent,
+                              uint16_t neededCurrent,
                               uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
   /// @param consumer the active consumer to release the current for
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 0dbbbef..396e84b 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -304,9 +304,8 @@ float PowerScheduler::getConsumerCurrent(
   return currentSum;
 }
 
-PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma) {
-  this->limitCurrent = i_limit_ma;
-  this->maximumCurrent = i_max_ma;
+PowerScheduler::PowerScheduler(float i_limit_ma, float i_max_ma)
+    : freeLimitCurrentBudget(i_limit_ma), freeMaximumCurrentBudget(i_max_ma) {
   this->currentAllowances = std::vector<CurrentAllowance>();
   this->powerSchedulerMutex = xSemaphoreCreateMutex();
 }
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index c32e6a7..aa7e66f 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -91,7 +91,7 @@ public:
   /// account, so requesting the same or less power will always succeed. Also,
   /// small amounts of power (below 1 mA) will always be granted.
   bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
-                                   float neededcurrent,
+                                   float neededCurrent,
                                    uint16_t requestedDurationMs = 0);
   /// @brief "Return" the current currently allocated to a consumer
   /// @param consumer the active consumer to release the current for

From 33c23cb4f86e4daef76630c8107ce8662d06722b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 12:19:38 +0100
Subject: [PATCH 49/59] Update picking of next current allowance to take more
 than one outstanding allowance into consideration

---
 src/power/PowerScheduler.cpp | 72 ++++++++++++++++++++++--------------
 src/power/PowerScheduler.h   |  2 +-
 2 files changed, 45 insertions(+), 29 deletions(-)

diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 396e84b..beb3d5d 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -16,20 +16,23 @@
 bool PowerScheduler::tryAccquireCurrentAllowance(
     PowerParameters::PowerConsumers consumer, float neededCurrent,
     uint16_t requestedDurationMs) {
-      float existingConsumption = getConsumerCurrent(consumer);
-      const bool currentAvailableBelowLimit =
+  float existingConsumption = getConsumerCurrent(consumer);
+  const bool currentLimitNotExceeded =
       this->freeLimitCurrentBudget + existingConsumption > 0;
-      const bool currentAvailableBelowMaximum =
+  const bool neededCurrentAvailableBelowMaximum =
       this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
-      const bool currentIsInsignificant = neededCurrent < 1;
-      if (currentIsInsignificant ||
-        (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
+  const bool currentIsInsignificant =
+      neededCurrent < PowerParameters::CURRENT_INSIGNIFICANT;
+  if (currentIsInsignificant ||
+      (currentLimitNotExceeded && neededCurrentAvailableBelowMaximum)) {
     if (existingConsumption > 0) {
       releaseCurrent(consumer);
     }
     PowerSchedulerMutex lock(powerSchedulerMutex);
     if (lock.isLocked() == false) {
-      ESP_LOGE(TAG, "Failed to Acquire PowerScheduler Mutex during Current Allocation");
+      ESP_LOGE(
+          TAG,
+          "Failed to Acquire PowerScheduler Mutex during Current Allocation");
       return false;
     }
     this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{
@@ -110,14 +113,14 @@ bool PowerScheduler::waitForCurrentAllowance(
         // Exclude existing consumption from the same consumer, as it will be
         // gone if this is granted
         float existingConsumption = getConsumerCurrent(consumer);
-        const bool currentAvailableBelowLimit =
+        const bool currentLimitNotExceeded =
             this->freeLimitCurrentBudget + existingConsumption > 0;
-        const bool currentAvailableBelowMaximum =
+        const bool neededCurrentAvailableBelowMaximum =
             this->freeMaximumCurrentBudget + existingConsumption >=
             neededCurrent;
         const bool currentIsInsignificant = neededCurrent < 1;
         if (currentIsInsignificant ||
-            (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
+            (currentLimitNotExceeded && neededCurrentAvailableBelowMaximum)) {
           PowerSchedulerMutex lock(powerSchedulerMutex);
           if (lock.isLocked() == false) {
             return false;
@@ -148,7 +151,7 @@ bool PowerScheduler::waitForCurrentAllowance(
     }
     if (notificationStatus == pdFALSE) {
       // We waited long enough...
-      // Remove the task from the list of waiting tasks
+      // Give up and remove the task from the list of waiting tasks
       PowerSchedulerMutex lock(powerSchedulerMutex);
       if (lock.isLocked() == false) {
         return false;
@@ -178,8 +181,7 @@ void PowerScheduler::checkWaitingTasks(void) {
   // If there are requested allowances, try to grant the one expiring next
   if (this->currentAllowances.size() > 0) {
 
-    PowerScheduler::CurrentAllowance *nextAllowance =
-        getNextExpiringAllowance();
+    PowerScheduler::CurrentAllowance *nextAllowance = getNextAllowance();
     if (nextAllowance != nullptr) {
       xTaskNotify(nextAllowance->taskHandle,
                   PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE,
@@ -237,33 +239,47 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance *
-PowerScheduler::getNextExpiringAllowance(void) {
-  PowerSchedulerMutex lock(powerSchedulerMutex);
+PowerScheduler::CurrentAllowance *PowerScheduler::getNextAllowance(void) {
   TickType_t minTicks = UINT32_MAX;
   CurrentAllowance *nextAllowance = nullptr;
-  if (lock.isLocked() == false) {
-    return nullptr;
-  }
+
+  std::vector<CurrentAllowance> sortedAllowances;
+  sortedAllowances.reserve(currentAllowances.size());
   for (auto &allowance : currentAllowances) {
     if (!(allowance.granted)) {
-      TickType_t ticks =
-          allowance.requestedAt + pdMS_TO_TICKS(allowance.maxSlackTimeMs);
-      if (ticks < minTicks) {
-        minTicks = ticks;
-        nextAllowance = &allowance;
-      }
+      sortedAllowances.push_back(allowance);
+    }
+  }
+  // Sort the not yet granted requests by how much time they have left before
+  // expiring
+  std::sort(sortedAllowances.begin(), sortedAllowances.end(),
+            [](const CurrentAllowance &a, const CurrentAllowance &b) {
+              return a.requestedAt + pdMS_TO_TICKS(a.maxSlackTimeMs) <
+                     b.requestedAt + pdMS_TO_TICKS(b.maxSlackTimeMs);
+            });
+
+  // Get the first one whose power requirement can be met
+  for (CurrentAllowance &allowance : sortedAllowances) {
+    const float existingConsumerConsumption =
+        getConsumerCurrent(allowance.consumer);
+    const bool currentAvailableBelowMaximum =
+        this->freeMaximumCurrentBudget + existingConsumerConsumption >=
+        allowance.neededCurrent;
+    const bool currentInsignificant =
+        allowance.neededCurrent < PowerParameters::CURRENT_INSIGNIFICANT;
+    if (currentInsignificant || currentAvailableBelowMaximum) {
+      return &allowance;
     }
   }
   // Will be nullptr if no allowance was found
-  return nextAllowance;
+  return nullptr;
 }
 
 PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,
                                                   float i_max_ma) {
   if (powerSchedulerInstance == nullptr) {
-      powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma);
-    }
+    powerSchedulerInstance = new PowerScheduler(i_limit_ma, i_max_ma);
+  }
   return *powerSchedulerInstance;
 }
 
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index aa7e66f..92127b9 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -147,7 +147,7 @@ public:
   CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
 
   // @brief Retrieve the allowance that will expire next
-  CurrentAllowance *getNextExpiringAllowance(void);
+  CurrentAllowance *getNextAllowance(void);
 
 protected:
   // Current above which there will be no new scheduling

From b410338109be77a89abb0f239b96a9eace3b538a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 12:22:23 +0100
Subject: [PATCH 50/59] Improve naming

---
 src/power/PowerScheduler.cpp | 4 ++--
 src/power/PowerScheduler.h   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index beb3d5d..05c48a2 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -181,7 +181,7 @@ void PowerScheduler::checkWaitingTasks(void) {
   // If there are requested allowances, try to grant the one expiring next
   if (this->currentAllowances.size() > 0) {
 
-    PowerScheduler::CurrentAllowance *nextAllowance = getNextAllowance();
+    PowerScheduler::CurrentAllowance *nextAllowance = getNextCurrentAllowance();
     if (nextAllowance != nullptr) {
       xTaskNotify(nextAllowance->taskHandle,
                   PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE,
@@ -239,7 +239,7 @@ PowerScheduler::getCurrentAllowance(TaskHandle_t taskHandle) {
   }
   return nullptr;
 }
-PowerScheduler::CurrentAllowance *PowerScheduler::getNextAllowance(void) {
+PowerScheduler::CurrentAllowance *PowerScheduler::getNextCurrentAllowance(void) {
   TickType_t minTicks = UINT32_MAX;
   CurrentAllowance *nextAllowance = nullptr;
 
diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h
index 92127b9..c0e93aa 100644
--- a/src/power/PowerScheduler.h
+++ b/src/power/PowerScheduler.h
@@ -147,7 +147,7 @@ public:
   CurrentAllowance *getCurrentAllowance(TaskHandle_t taskHandle);
 
   // @brief Retrieve the allowance that will expire next
-  CurrentAllowance *getNextAllowance(void);
+  CurrentAllowance *getNextCurrentAllowance(void);
 
 protected:
   // Current above which there will be no new scheduling

From a48dd7c9ddb8e6156239ed2f180f8f60e08554e9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 13:08:25 +0100
Subject: [PATCH 51/59] Safeguard serial error prints

---
 src/communication/Communication.cpp     |  4 +++-
 src/display/Display.cpp                 |  4 +++-
 src/lightDetection/LightDetection.cpp   | 10 ++++++++--
 src/motion/Motion.cpp                   | 13 ++++++++++---
 src/motionDetection/MotionDetection.cpp |  4 +++-
 src/multiColorLight/MultiColorLight.cpp |  4 +++-
 src/power/PowerScheduler.cpp            |  2 +-
 7 files changed, 31 insertions(+), 10 deletions(-)

diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp
index 146e8ca..1669ecf 100644
--- a/src/communication/Communication.cpp
+++ b/src/communication/Communication.cpp
@@ -81,7 +81,9 @@ void Communication::begin(void) {
           PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
           MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
     ESP_LOGE(TAG, "Failed to get power for mesh initialization");
-    Serial.println("Failed to get power for mesh initialization");
+    if(Serial) {
+      Serial.println("Failed to get power for mesh initialization");
+    }
   }
   mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
   mesh.onReceive(&receivedCallback);
diff --git a/src/display/Display.cpp b/src/display/Display.cpp
index 342c48c..6fe4a6c 100644
--- a/src/display/Display.cpp
+++ b/src/display/Display.cpp
@@ -18,7 +18,9 @@ void Display::begin(void){
         PowerParameters::PowerConsumers::DISPLAY_OLED, PowerParameters::CurrentConsumptions::CURRENT_DISPLAY
         ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL)){
             ESP_LOGE(TAG,"Could not get power for Display");
-            Serial.println("Could not get power for Display");
+            if (Serial) {
+              Serial.println("Could not get power for Display");
+            }
     }
     //set Mux Ratio
     sendDisplayCMD(muxRatio);
diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp
index 1fbe3fa..46c52da 100644
--- a/src/lightDetection/LightDetection.cpp
+++ b/src/lightDetection/LightDetection.cpp
@@ -69,7 +69,10 @@ void LightDetection::beginInfrared(void){
         PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
         LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
             ESP_LOGE(TAG,"Could not get power for Infrared Phototransistors");
-            Serial.println("Could not get power for Infrared Phototransistors");
+            if(Serial){
+              Serial.println(
+                  "Could not get power for Infrared Phototransistors");
+            }
     }
     digitalWrite(IR_PT_ENABLE,true);
     pinMode(IR_PT_ENABLE, OUTPUT);
@@ -85,7 +88,10 @@ void LightDetection::beginDaylight(void){
         PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
         LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
             ESP_LOGE(TAG,"Could not get power for Daylight Phototransistors");
-            Serial.println("Could not get power for Daylight Phototransistors");
+            if(Serial){
+              Serial.println(
+                  "Could not get power for Daylight Phototransistors");
+            }
     }
     digitalWrite(DL_PT_ENABLE,true);
     pinMode(DL_PT_ENABLE, OUTPUT);
diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp
index 0db820c..2852efa 100644
--- a/src/motion/Motion.cpp
+++ b/src/motion/Motion.cpp
@@ -32,7 +32,9 @@ void Motion::moveTask(void * args) {
     uint32_t runtime = (uint32_t)args;
 
     if (!Motion::left.setSpeed(LEFT_MOTOR_DUTY) || !Motion::right.setSpeed(RIGHT_MOTOR_DUTY)) {
-        Serial.println("ailed to start motors due to power constraints");
+        if(Serial){
+            Serial.println("Failed to start motors due to power constraints");
+        }
         Motion::stop();  // Use to clean up
         return;
     }
@@ -123,7 +125,9 @@ void Motion::leftMotorTask(void * args) {
     }
     Motion::right.setSpeed(0);
     if(!Motion::left.setSpeed(LEFT_MOTOR_DUTY)){
-        Serial.println("Can not rotate Clockwise due to power constraints");
+        if(Serial){
+          Serial.println("Can not rotate Clockwise due to power constraints");
+        }
         Motion::stop();
         return;
     }
@@ -166,7 +170,10 @@ void Motion::rightMotorTask(void * args) {
         xClockwiseTaskHandle = NULL;
     }
     if(!Motion::left.setSpeed(RIGHT_MOTOR_DUTY)){
-        Serial.println("Can not rotate Antilockwise due to power constraints");
+        if(Serial){
+          Serial.println(
+              "Can not rotate Antilcockwise due to power constraints");
+        }
         Motion::stop();
         return;
     }
diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp
index cfe2c77..19cdd7c 100644
--- a/src/motionDetection/MotionDetection.cpp
+++ b/src/motionDetection/MotionDetection.cpp
@@ -11,7 +11,9 @@ void MotionDetection::begin(void){
           PowerParameters::CurrentConsumptions::CURRENT_IMU,
           IMU_MAX_EXECUTION_DELAY_MS, NULL)) {
     ESP_LOGE(TAG, "Could not get power for MotionDetection");
-    Serial.println("Could not get power for MotionDetection");
+    if(Serial){
+      Serial.println("Could not get power for MotionDetection");
+    }
   }
     pinMode(34,OUTPUT);
     digitalWrite(34,HIGH);
diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp
index e439252..90e0ef8 100644
--- a/src/multiColorLight/MultiColorLight.cpp
+++ b/src/multiColorLight/MultiColorLight.cpp
@@ -19,7 +19,9 @@ void MultiColorLight::begin(void) {
           PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
           MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)) {
     ESP_LOGE(TAG, "Could not get power for MultiColorLight");
-    Serial.println("Could not get power for MultiColorLight");
+    if(Serial){
+      Serial.println("Could not get power for MultiColorLight");
+    }
   }
   rgbLeds.begin();
   this->turnOffLed();
diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp
index 05c48a2..e2c6aa3 100644
--- a/src/power/PowerScheduler.cpp
+++ b/src/power/PowerScheduler.cpp
@@ -137,7 +137,7 @@ bool PowerScheduler::waitForCurrentAllowance(
                .requestedAt = initialTickCount,
                .grantedAt = xTaskGetTickCount(),
                .granted = true});
-          ESP_LOGV(TAG, "%d mA granted to consumer %d after %d ms",
+          ESP_LOGI(TAG, "%d mA granted to consumer %d with a delay of %d ms",
                    neededCurrent, static_cast<int>(consumer),
                    xTaskGetTickCount() - initialTickCount);
           return true;

From fd1be4ffeb3c5d6b5786d6761c2cca81688c3988 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 19:17:36 +0100
Subject: [PATCH 52/59] Improve Power State Update Task timing

---
 src/power/PowerManager.cpp  | 5 ++++-
 src/power/PowerParameters.h | 4 ++++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/power/PowerManager.cpp b/src/power/PowerManager.cpp
index 760cfb6..420598d 100644
--- a/src/power/PowerManager.cpp
+++ b/src/power/PowerManager.cpp
@@ -14,8 +14,11 @@ SemaphoreHandle_t PowerManager::powerMutex = NULL;
 void vTaskUpdatePowerState(void *pvParameters) {
   for (;;) {
     ESP_LOGV(TAG, "Updating Power State...");
+    TickType_t executionStart = xTaskGetTickCount();
     PowerManager::updatePowerStateHandler();
-    vTaskDelay(pdMS_TO_TICKS(10));
+    vTaskDelayUntil(
+        &executionStart,
+        pdMS_TO_TICKS(PowerParameters::POWER_STATE_UPDATE_INTERVAL_MS));
   }
 }
 
diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h
index bf16281..24ea9f0 100644
--- a/src/power/PowerParameters.h
+++ b/src/power/PowerParameters.h
@@ -12,6 +12,7 @@
 #ifndef PowerParameters_h
 #define PowerParameters_h
 
+#include <cstdint>
 namespace PowerParameters {
 
 struct Battery {
@@ -114,6 +115,9 @@ enum PowerConsumers {
   MOTOR_RIGHT,
   IMU
 };
+
+static constexpr uint32_t POWER_STATE_UPDATE_INTERVAL_MS = 10;
+
 }; // namespace PowerParameters
 
 #endif // Consumptions_h
\ No newline at end of file

From 21f313af5c4159fffefed93f6739b1cee0bf427f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 19:18:20 +0100
Subject: [PATCH 53/59] Fix "adc1 already in use!" log spam.

---
 src/power/PowerManager.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/power/PowerManager.cpp b/src/power/PowerManager.cpp
index 420598d..6e79e99 100644
--- a/src/power/PowerManager.cpp
+++ b/src/power/PowerManager.cpp
@@ -36,6 +36,10 @@ void PowerManager::begin() {
     }
   }
 
+  // Initialize IO for battery voltage measurement
+  pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT);
+  pinMode(PowerParameters::PinConfig::BAT_ADC, INPUT);
+
   if (powerScheduler == nullptr) {
     ESP_LOGI(TAG, "Creating Power Scheduler");
     powerScheduler = &PowerScheduler::getPowerScheduler(
@@ -111,8 +115,6 @@ float PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers consumer)
 float PowerManager::getBatteryVoltage() {
   // Get the battery voltage from the ADC and convert it to a voltage
   // using the voltage divider.
-  pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT);
-  pinMode(PowerParameters::PinConfig::BAT_ADC, INPUT);
   // Enable voltage divider
   digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH);
   // Allow voltage to stabilize

From fdd59a83e3d562e314b6a5ce827e3e25c01ebb65 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Sun, 16 Feb 2025 20:44:58 +0100
Subject: [PATCH 54/59] Reset batttery charge in coloumbs on charge completion

---
 src/power/PowerManager.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/power/PowerManager.cpp b/src/power/PowerManager.cpp
index 6e79e99..1f012b6 100644
--- a/src/power/PowerManager.cpp
+++ b/src/power/PowerManager.cpp
@@ -178,6 +178,7 @@ void PowerManager::updatePowerStateHandler() {
   if (PowerManager::busPowered && !PowerManager::chargingState) {
     // Calibrate voltage offset on full Battery
     PowerManager::fullVoltageOffset = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0] - getBatteryVoltage();
+    PowerManager::coloumbsRemaining = PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB;
   }
 
   ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState);

From 59e2ec3d101c24129807c4b8c45ca01a86cba40c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Mon, 17 Feb 2025 19:23:31 +0100
Subject: [PATCH 55/59] Fix Motor cleanup

---
 src/motion/Motion.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp
index 2852efa..7f316ca 100644
--- a/src/motion/Motion.cpp
+++ b/src/motion/Motion.cpp
@@ -218,6 +218,8 @@ void Motion::stop(void){
         vTaskDelete(xClockwiseTaskHandle);
         xClockwiseTaskHandle = NULL;
     }
+    PowerManager::releaseCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT);
+    PowerManager::releaseCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT);
     Motion::left.setSpeed(0);
     Motion::right.setSpeed(0);
 }

From 2f2c6f2860b59ce1cec189c60ab25ed67b2eb6f1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Mon, 17 Feb 2025 19:23:50 +0100
Subject: [PATCH 56/59] Add power test case

---
 .../Varied_Power_Consumption.ino              | 102 ++++++++++++++++++
 1 file changed, 102 insertions(+)
 create mode 100644 example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino

diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
new file mode 100644
index 0000000..fb3b830
--- /dev/null
+++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
@@ -0,0 +1,102 @@
+#include "Dezibot.h"
+
+/*
+ * Test case of varying power consumption, with logging of modeled state on
+ * secondary UART.
+ */
+
+Dezibot dezibot;
+
+// Using alternate Serial pins to not be powered by the USB port
+#define RXD_HEADER 16
+#define TXD_HEADER 17
+
+// Task for running through LED brightness levels
+void powerChange(void *pvParameters) {
+  while (true) {
+    dezibot.infraredLight.bottom.turnOn();
+    delay(1000);
+    dezibot.infraredLight.bottom.turnOff();
+    delay(1000);
+  }
+}
+
+// Task for periodic logging of power state
+void outputCsvLine(void *pvParameters) {
+  while (true) {
+    Serial1.printf(
+        "%d,%f,%f,%f,%f,%f,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%"
+        "f,%f,%f,%f\r\n",
+        millis(), dezibot.power.getCurrentCurrent(),
+        dezibot.power.getMax3V3Current(),
+        dezibot.power.getBatteryChargePercent(),
+        dezibot.power.getBatteryChargeCoulombs(),
+        dezibot.power.getBatteryVoltage(), dezibot.power.isUSBPowered(),
+        dezibot.power.isBatteryPowered(), dezibot.power.isBatteryCharging(),
+        dezibot.power.isBatteryDischarging(),
+        dezibot.power.isBatteryFullyCharged(),
+        dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::ESP),
+        dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::WIFI),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::LED_RGB_BOTTOM),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::RGBW_SENSOR),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::LED_IR_FRONT),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::LED_IR_BOTTOM),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::PT_IR),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::PT_DL),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::LED_UV),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::DISPLAY_OLED),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::MOTOR_LEFT),
+        dezibot.power.getConsumerCurrent(
+            PowerParameters::PowerConsumers::MOTOR_RIGHT),
+        dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::IMU));
+    delay(100);
+  }
+}
+
+void setup() {
+  dezibot.begin();
+  Serial1.begin(115200, SERIAL_8N1, RXD_HEADER, TXD_HEADER);
+  // Output CSV-Header for Timestamp and modelled current of all components
+  Serial1.printf(
+      "Timestamp (ms),Current (mA),calculated max current (mA),charge "
+      "(%%),charge (C),voltage "
+      "(V),isUSBPowered,isBatteryPowered,isBatteryCharging,"
+      "isBatteryDischarging,isBatteryFullyCharged,ESP (mA),WIFI "
+      "(mA),LED_RGB_TOP_LEFT (mA),"
+      "LED_RGB_TOP_RIGHT (mA),LED_RGB_BOTTOM (mA),RGBW_SENSOR "
+      "(mA),LED_IR_FRONT (mA),LED_IR_BOTTOM (mA),PT_IR (mA),PT_DL (mA),"
+      "LED_UV (mA),DISPLAY_OLED (mA),MOTOR_LEFT (mA),MOTOR_RIGHT "
+      "(mA),IMU (mA)\r\n");
+  // Start logging task
+  xTaskCreate(outputCsvLine, "outputCsvLine", 4096, NULL, tskIDLE_PRIORITY, NULL);
+
+  // Start power consumption task
+  xTaskCreate(powerChange, "powerChange", 4096, NULL, tskIDLE_PRIORITY, NULL);
+}
+
+void loop() {
+  while (true) {
+    dezibot.multiColorLight.turnOffLed();
+    for (leds led : {TOP_LEFT, TOP_RIGHT, BOTTOM, TOP, ALL}) {
+      dezibot.multiColorLight.setLed(led, 255, 0, 0);
+      delay(1000);
+      dezibot.multiColorLight.setLed(led, 0, 255, 0);
+      delay(1000);
+      dezibot.multiColorLight.setLed(led, 0, 0, 255);
+      delay(1000);
+    }
+  }
+}
\ No newline at end of file

From 92d63d98fa1e4988868fee26f49d466011681b8f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Mon, 17 Feb 2025 19:30:25 +0100
Subject: [PATCH 57/59] Update power test case

---
 .../Varied_Power_Consumption.ino                 | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
index fb3b830..ec19ccb 100644
--- a/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
+++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
@@ -21,6 +21,16 @@ void powerChange(void *pvParameters) {
   }
 }
 
+// Task for running through LED brightness levels
+void powerChange2(void *pvParameters) {
+  while (true) {
+    dezibot.infraredLight.front.turnOn();
+    delay(1000);
+    dezibot.infraredLight.front.turnOff();
+    delay(1000);
+  }
+}
+
 // Task for periodic logging of power state
 void outputCsvLine(void *pvParameters) {
   while (true) {
@@ -81,10 +91,14 @@ void setup() {
       "LED_UV (mA),DISPLAY_OLED (mA),MOTOR_LEFT (mA),MOTOR_RIGHT "
       "(mA),IMU (mA)\r\n");
   // Start logging task
-  xTaskCreate(outputCsvLine, "outputCsvLine", 4096, NULL, tskIDLE_PRIORITY, NULL);
+  xTaskCreate(outputCsvLine, "outputCsvLine", 4096, NULL, tskIDLE_PRIORITY,
+              NULL);
 
   // Start power consumption task
   xTaskCreate(powerChange, "powerChange", 4096, NULL, tskIDLE_PRIORITY, NULL);
+
+  // Start second power consumption task
+  xTaskCreate(powerChange2, "powerChange2", 4096, NULL, tskIDLE_PRIORITY, NULL);
 }
 
 void loop() {

From 064ed959c71b2d0fa54068e43e4372dd1c6c3fc0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Mon, 17 Feb 2025 21:23:02 +0100
Subject: [PATCH 58/59] Add Power Test Case Program without Logging

---
 .../Varied_Power_Consumption_no_logging.ino   | 57 +++++++++++++++++++
 1 file changed, 57 insertions(+)
 create mode 100644 example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino

diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino
new file mode 100644
index 0000000..c2284e7
--- /dev/null
+++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino
@@ -0,0 +1,57 @@
+#include "Dezibot.h"
+
+/*
+ * Test case of varying power consumption, with logging of modeled state on
+ * secondary UART.
+ */
+
+Dezibot dezibot;
+
+// Using alternate Serial pins to not be powered by the USB port
+#define RXD_HEADER 16
+#define TXD_HEADER 17
+
+// Task for running through LED brightness levels
+void powerChange(void *pvParameters) {
+  while (true) {
+    dezibot.infraredLight.bottom.turnOn();
+    delay(1000);
+    dezibot.infraredLight.bottom.turnOff();
+    delay(1000);
+  }
+}
+
+// Task for running through LED brightness levels
+void powerChange2(void *pvParameters) {
+  while (true) {
+    dezibot.infraredLight.front.turnOn();
+    delay(1000);
+    dezibot.infraredLight.front.turnOff();
+    delay(1000);
+  }
+}
+
+
+void setup() {
+  dezibot.begin();
+  // Output CSV-Header for Timestamp and modelled current of all components
+  // Start power consumption task
+  xTaskCreate(powerChange, "powerChange", 4096, NULL, tskIDLE_PRIORITY, NULL);
+
+  // Start second power consumption task
+  xTaskCreate(powerChange2, "powerChange2", 4096, NULL, tskIDLE_PRIORITY, NULL);
+}
+
+void loop() {
+  while (true) {
+    dezibot.multiColorLight.turnOffLed();
+    for (leds led : {TOP_LEFT, TOP_RIGHT, BOTTOM, TOP, ALL}) {
+      dezibot.multiColorLight.setLed(led, 255, 0, 0);
+      delay(1000);
+      dezibot.multiColorLight.setLed(led, 0, 255, 0);
+      delay(1000);
+      dezibot.multiColorLight.setLed(led, 0, 0, 255);
+      delay(1000);
+    }
+  }
+}
\ No newline at end of file

From 0d7fcf477ccf988e97786852260a98862b709a28 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Mon, 17 Feb 2025 21:26:18 +0100
Subject: [PATCH 59/59] Update comments for clarification

---
 .../Varied_Power_Consumption.ino              |  4 +--
 .../Varied_Power_Consumption_no_logging.ino   | 32 +++++++++----------
 2 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
index ec19ccb..54bff7d 100644
--- a/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
+++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino
@@ -11,7 +11,7 @@ Dezibot dezibot;
 #define RXD_HEADER 16
 #define TXD_HEADER 17
 
-// Task for running through LED brightness levels
+// Task for independently toggling the bottom IR LED
 void powerChange(void *pvParameters) {
   while (true) {
     dezibot.infraredLight.bottom.turnOn();
@@ -21,7 +21,7 @@ void powerChange(void *pvParameters) {
   }
 }
 
-// Task for running through LED brightness levels
+// Tast for independently toggling the front IR LED
 void powerChange2(void *pvParameters) {
   while (true) {
     dezibot.infraredLight.front.turnOn();
diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino
index c2284e7..7e680fe 100644
--- a/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino
+++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino
@@ -11,25 +11,25 @@ Dezibot dezibot;
 #define RXD_HEADER 16
 #define TXD_HEADER 17
 
-// Task for running through LED brightness levels
+// Task for independently toggling the bottom IR LED
 void powerChange(void *pvParameters) {
-  while (true) {
-    dezibot.infraredLight.bottom.turnOn();
-    delay(1000);
-    dezibot.infraredLight.bottom.turnOff();
-    delay(1000);
+    while (true) {
+      dezibot.infraredLight.bottom.turnOn();
+      delay(1000);
+      dezibot.infraredLight.bottom.turnOff();
+      delay(1000);
+    }
   }
-}
-
-// Task for running through LED brightness levels
-void powerChange2(void *pvParameters) {
-  while (true) {
-    dezibot.infraredLight.front.turnOn();
-    delay(1000);
-    dezibot.infraredLight.front.turnOff();
-    delay(1000);
+  
+  // Tast for independently toggling the front IR LED
+  void powerChange2(void *pvParameters) {
+    while (true) {
+      dezibot.infraredLight.front.turnOn();
+      delay(1000);
+      dezibot.infraredLight.front.turnOff();
+      delay(1000);
+    }
   }
-}
 
 
 void setup() {