Merge branch 'main' of gitlab.dit.htwk-leipzig.de:phillip.kuehne/dezibot

Introduce Power Management

See merge request phillip.kuehne/dezibot!1

.gitignore

@@ -39,3 +39,10 @@ docs/*
 .history/
 *.vsix
 *.workspace
+
+# Tool-generated files
+.cache/
+compile_commands.json
+
+# Build directories
+build/

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);
+}

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);
+}

example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino

@@ -0,0 +1,49 @@
+#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
+  int seed = analogRead(GPIO_NUM_42);
+  randomSeed(seed);
+  Serial.printf("Initialized PRNG with seed %d", seed);
+}
+
+void loop() {
+  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",
+                color, randomDurationMs,
+                dezibot.multiColorLight.modelCurrentConsumption(color),
+                dezibot.multiColorLight.modelChargeConsumption(
+                    TOP_LEFT, color, randomDurationMs));
+  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 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 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, 100);
+  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);
+}

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());
+  }
+}

example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino

@@ -0,0 +1,50 @@
+#include "Dezibot.h"
+
+/*
+ * 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();
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;
+    ;
+  }
+  Serial.println("Starting Power Management Test");
+  Serial.println("Status on end of setup:");
+
+  dezibot.power.dumpPowerStatistics();
+  dezibot.power.dumpConsumerStatistics();
+}
+
+void dumpInfoAndWait() {
+  delay(100);
+  dezibot.power.dumpPowerStatistics();
+  dezibot.power.dumpConsumerStatistics();
+  delay(900);
+}
+
+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();
+  Serial.println("Turned off all LEDs and motors");
+  dumpInfoAndWait();
+}

example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino

@@ -0,0 +1,116 @@
+#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 independently toggling the bottom IR LED
+void powerChange(void *pvParameters) {
+  while (true) {
+    dezibot.infraredLight.bottom.turnOn();
+    delay(1000);
+    dezibot.infraredLight.bottom.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);
+  }
+}
+
+// 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);
+
+  // 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);
+    }
+  }
+}

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 independently toggling the bottom IR LED
+void powerChange(void *pvParameters) {
+    while (true) {
+      dezibot.infraredLight.bottom.turnOn();
+      delay(1000);
+      dezibot.infraredLight.bottom.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() {
+  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);
+    }
+  }
+}

example/advanced/Power_Measurements/Display/Display.ino

@@ -3,7 +3,7 @@
 Dezibot dezibot = Dezibot();
 
 // How many times to run a command on the display consecutively;
-const uint16_t iterations = 5000;
+const uint16_t iterations = 1000;
 
 void setup() {
   dezibot.begin();
@@ -23,4 +23,10 @@ void loop() {
     dezibot.display.println(iter);
   }
   delay(10);
+  //Completely off
+  dezibot.display.clear();
+  sleep(10);
+  //Completely on
+  dezibot.display.invertColor();
+  sleep(10);
 }

example/advanced/Power_Measurements/ESP32_baseline/ESP32_baseline.ino

@@ -4,7 +4,7 @@
 
 Dezibot dezibot;
 
-const uint16_t cycleTime = 2e4;  //20000 ms = 20 s
+const uint16_t cycleTime = 20e3; 
 
 esp_pm_lock_handle_t cpuFreqLock;
 esp_pm_lock_handle_t apbFreqLock;
@@ -12,39 +12,39 @@ 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) {
+void stress_task(void *pvParameters) {
+    // Register with task watchdog
+    ESP_ERROR_CHECK(esp_task_wdt_add(NULL));
+    
+    // Variables for load generation
     volatile uint32_t x = 0;
+    TickType_t last_wake_time = xTaskGetTickCount();
+    
+    while (1) {
+        // Compute-intensive period
         for (uint32_t i = 0; i < 10000; i++) {
             x += i;
+            // Mix in some memory operations
+            if (i % 100 == 0) {
+                // Force cache misses occasionally
+                void* temp = malloc(32);
+                if (temp) {
+                    free(temp);
                 }
-    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();
+        // Reset watchdog
+        ESP_ERROR_CHECK(esp_task_wdt_reset());
+        
     }
 }
 
 void setup() {
   Serial.begin(115200);
-  while (!Serial) {
-    ;
-  }
+  // while (!Serial) {
+  //   ;
+  // }
   uint32_t Freq = getCpuFrequencyMhz();
   Serial.print("CPU Freq = ");
   Serial.print(Freq);
@@ -87,6 +87,17 @@ void setup() {
   Serial.end();
 }
 
+void blip_io(int times) {
+  constexpr int ioPin = 17;
+  pinMode(ioPin, OUTPUT);
+  for(int i = 0; i<times; i++) {
+    digitalWrite(ioPin,1);
+    delay(1);
+    digitalWrite(ioPin,0);
+    delay(1);
+  }
+}
+
 /*
  * 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
@@ -96,10 +107,6 @@ void setup() {
  */
 void setupAndCleanupSerialPrint(char *str) {
   Serial.begin(115200);
-  while (!Serial) {
-    ;
-    ;
-  }
   delay(10);
   Serial.print(str);
   delay(10);
@@ -111,10 +118,11 @@ void setupAndCleanupSerialPrint(char *str) {
 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");
+  blip_io(1);
   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);
+  xTaskCreatePinnedToCore(stress_task, "CPU0Stress", 4096, NULL, 1, &core0StressTask, 0);
+  xTaskCreatePinnedToCore(stress_task, "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);
@@ -123,12 +131,14 @@ void loop() {
   vTaskDelete(core1StressTask);
   // Now disable light sleep
   setupAndCleanupSerialPrint("Beginning idle with power management disabled\n");
+  blip_io(2);
   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");
+  blip_io(3);
   esp_pm_lock_release(cpuFreqLock);
   esp_pm_lock_release(apbFreqLock);
   esp_pm_lock_release(lightSleepLock);

example/advanced/Power_Measurements/IR_LED/IR_LED.ino

@@ -9,15 +9,26 @@ void setup() {
   dezibot.infraredLight.begin();
 }
 
+void blink_times(int times) {
+  constexpr int ioPin = 17;
+  pinMode(ioPin, OUTPUT);
+  for(int i = 0; i<times; i++) {
+    digitalWrite(ioPin,1);
+    delay(300);
+    digitalWrite(ioPin,0);
+    delay(300);
+  }
+}
+
 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();
+  blink_times(1);
   delay(cycleMillis);
   dezibot.infraredLight.front.turnOff();
   dezibot.infraredLight.bottom.turnOn();
   delay(cycleMillis);
-  //Turn off and wait a little to measure baseline consumption
+  blink_times(2);
   dezibot.infraredLight.bottom.turnOff();
-  delay(cycleMillis);
 }

example/advanced/Power_Measurements/Motors/Motors.ino

@@ -14,8 +14,7 @@ void loop() {
   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);  
+  dezibot.motion.move();
   delay(20000);
   dezibot.motion.stop();
   delay(20000);

example/advanced/Power_Measurements/RGB_LED_direct/RGB_LED_direct.ino

@@ -0,0 +1,80 @@
+#include <Adafruit_NeoPixel.h>
+
+/*
+╔══════════════════════════════════════════════════════════════════════════════╗
+║ WARNING: This example controls the RGB-LEDs directly, bypassing the          ║
+║ MultiColorLight component. This is not recommended for normal use, as it     ║
+║ bypasses the safety checks and color normalization. This is only intended    ║
+║ for diagnostic purposes.                                                     ║
+╚══════════════════════════════════════════════════════════════════════════════╝
+*/
+
+Adafruit_NeoPixel rgbLeds(3, 48);
+
+void setup() { rgbLeds.begin(); }
+
+void loop() {
+  // Ramp up the brightness of each color channel
+  // Allows for the PWM behaviour to quickly be observed on an oscilloscope
+  // Red
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(bri, 0, 0));
+    rgbLeds.show();
+    delay(10);
+  }
+  // Red
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(0, bri, 0));
+    rgbLeds.show();
+    delay(10);
+  }
+  // Blue
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(0, 0, bri));
+    rgbLeds.show();
+    delay(10);
+  }
+  // Combinations of the color channels
+  // Yellow (Red + Green)
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(bri, bri, 0));
+    rgbLeds.show();
+    delay(10);
+  }
+  // Purple (Red + Blue)
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(bri, 0, bri));
+    rgbLeds.show();
+    delay(10);
+  }
+  // Cyan (Green + Blue)
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(0, bri, bri));
+    rgbLeds.show();
+    delay(10);
+  }
+  // White (Red + Green + Blue)
+  for (int bri = 0; bri < 0xFF; bri += 0x1) {
+    rgbLeds.setPixelColor(0, rgbLeds.Color(bri, bri, bri));
+    rgbLeds.show();
+    delay(10);
+  }
+
+  // Now some constant states for comparable measurements
+  // Full brightness R+G+B
+  rgbLeds.setPixelColor(0, rgbLeds.Color(255, 255, 255));
+  rgbLeds.show();
+  sleep(5);
+  // Half brightness R+G+B
+  rgbLeds.setPixelColor(0, rgbLeds.Color(127, 127, 127));
+  rgbLeds.show();
+  sleep(5);
+  // Minimum brightness R+G+B
+  rgbLeds.setPixelColor(0, rgbLeds.Color(1, 1, 1));
+  rgbLeds.show();
+  sleep(5);
+  // Off (baseline)
+  rgbLeds.setPixelColor(0, rgbLeds.Color(0, 0, 0));
+  rgbLeds.show();
+  sleep(5);
+}

example/advanced/Power_Measurements/VEML6040/VEML6040.ino

@@ -3,7 +3,7 @@
 Dezibot dezibot;
 
 void setup() {
-  dezibot.lightDetection.begin();
+  dezibot.colorDetection.begin();
   //dezibot.motion.detection.end();
   // put your setup code here, to run once:
   Serial.begin(115200);
@@ -13,7 +13,7 @@ void setup() {
   }
   delay(1000);
   // Test if VEML6040 is working correctly
-  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  char light_value = dezibot.colorDetection.getColorValue(VEML_WHITE);
   if (light_value != UINT16_MAX) {
     Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
   } else {
@@ -29,6 +29,6 @@ void setup() {
 
 void loop() {
   // put your main code here, to run repeatedly:
-  dezibot.lightDetection.getValue(DL_FRONT);
+  dezibot.colorDetection.getColorValue(VEML_WHITE);
   delay(10);
 }

example/advanced/Power_Measurements/WLAN/WLAN.ino

@@ -3,40 +3,46 @@
 
 Dezibot dezibot = Dezibot();
 
-const uint16_t cycleTime = 5e3;  //5000 ms = 5 s
+void blip_io(int times) {
+  constexpr int ioPin = 17;
+  pinMode(ioPin, OUTPUT);
+  for(int i = 0; i<times; i++) {
+    digitalWrite(ioPin,1);
+    delay(500);
+    digitalWrite(ioPin,0);
+    delay(500);
+  }
+}
 
 void setup() {
   #ifdef DEBUG
   Serial.begin(112500);
-  while (!Serial) {
-    ; /* Wait for USB-CDC Serial init to complete. */
-  }
-  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");
+  Serial.println("Mesh set up");
   /* Set up receive handler */
   dezibot.communication.onReceive(handle_receive);
-  dezibot.display.println("Set up receive. Printing incoming messages:");
+  Serial.println("Set up receive. Printing incoming messages:");
   Serial.println("Sending broadcast messages to generate TX power consumption:");
   #endif
+  blip_io(1);
+  delay(5000);
+  #ifdef DEBUG
+  Serial.println("Starting Transmission...");
+  #endif
+  blip_io(2);
 }
 
 void handle_receive(String &message) {
-  dezibot.display.println(message);
+  Serial.println(message);
 }
 
 void loop() {
     /* Continuously send to consume power on TX */
-    for(int i=0; i<cycleTime; i++) {
     dezibot.communication.sendMessage("Power Test Message");
-      delay(1);
-    }
-    delay(cycleTime);
-
+    sleep(2);
 }

src/Dezibot.cpp

@@ -9,6 +9,8 @@ Dezibot::Dezibot() : multiColorLight() {};
 
 void Dezibot::begin(void)
 {
+    ESP_LOGI("Dezibot", "Initializing Dezibot");
+    power.begin();
     Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
     lightDetection.begin();
@@ -17,10 +19,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";
-    }
 };

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 {
@@ -34,7 +34,7 @@ public:
     InfraredLight infraredLight;
     Communication communication;
     Display display;
-    Power power;
+    PowerManager power;
     void begin(void);
 };
 

src/colorDetection/ColorDetection.cpp

@@ -1,32 +1,43 @@
 #include "ColorDetection.h"
 
 void ColorDetection::begin(void) {
-    ColorDetection::configure(VEML_CONFIG{.mode = AUTO,.enabled = true,.exposureTime=MS40});
+  if (!PowerManager::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)
-    {
+  switch (config.exposureTime) {
   case MS40:
-            configRegister = 0x00;break;
+    configRegister = 0x00;
+    break;
   case MS80:
-            configRegister = 0x01;break;
+    configRegister = 0x01;
+    break;
   case MS160:
-            configRegister = 0x02;break;
+    configRegister = 0x02;
+    break;
   case MS320:
-            configRegister = 0x03;break;
+    configRegister = 0x03;
+    break;
   case MS640:
-            configRegister = 0x04;break;
+    configRegister = 0x04;
+    break;
   case MS1280:
-            configRegister = 0x05;break;
+    configRegister = 0x05;
+    break;
   }
   configRegister = configRegister << 4;
-    if(config.mode == MANUAL)
-    {
+  if (config.mode == MANUAL) {
     configRegister = configRegister | (0x01 << 1);
   }
-    if(!config.enabled)
-    {
+  if (!config.enabled) {
     configRegister = configRegister | 1;
   }
   ColorDetection::writeDoubleRegister(CMD_CONFIG, (uint16_t)configRegister);
@@ -34,8 +45,7 @@ void ColorDetection::configure(VEML_CONFIG config){
 
 uint16_t ColorDetection::getColorValue(color color) {
 
-    switch(color)
-    {
+  switch (color) {
   case VEML_RED:
     return readDoubleRegister(REG_RED);
     break;
@@ -81,3 +91,13 @@ void ColorDetection::writeDoubleRegister(uint8_t regAddr, uint16_t data){
     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) / 10e6f;
+}

src/colorDetection/ColorDetection.h

@@ -11,9 +11,10 @@
 
 #ifndef ColorDetection_h
 #define ColorDetection_h
-#include <stdint.h>
-#include <Wire.h>
+#include "../power/PowerManager.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,9 @@
 #define REG_BLUE                    0x0A
 #define REG_WHITE                   0x0B
 
+#define COLOR_DETECTION_MAX_EXECUTION_DELAY_MS 1
+
+#define TAG "ColorDetection"
 
 enum duration{
     MS40,
@@ -61,6 +65,27 @@ public:
     void begin(void);
     void configure(VEML_CONFIG config);
     uint16_t getColorValue(color color);
+
+    /**
+     * @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 
+     */
+    static 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
+     */
+    static float modelChargeConsumption(uint16_t durationMs);
+
   protected:
     uint16_t readDoubleRegister(uint8_t regAddr);
     void writeDoubleRegister(uint8_t regAddr, uint16_t data);

src/communication/Communication.cpp

@@ -7,24 +7,24 @@ uint32_t Communication::groupNumber = 0;
 // User-defined callback function pointer
 void (*Communication::userCallback)(String &msg) = nullptr;
 
-void Communication::sendMessage(String msg)
-{
+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)
-{
+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) {
@@ -33,46 +33,58 @@ void Communication::receivedCallback(uint32_t from, String &msg)
   }
 }
 
-void newConnectionCallback(uint32_t 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)
-{
+void nodeTimeAdjustedCallback(int32_t offset) {
   Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(), offset);
 }
 
-void vTaskUpdate(void *pvParameters)
-{
-    for (;;)
-    {
+void vTaskUpdate(void *pvParameters) {
+  for (;;) {
+    if (PowerManager::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");
+    }
+    PowerManager::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::WIFI,
+        PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
+        MESH_MAX_EXECUTION_DELAY_MS, NULL);
   }
 }
 
-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))
-{
+void Communication::onReceive(void (*callbackFunc)(String &msg)) {
   userCallback = callbackFunc;
 }
 
-void Communication::begin(void)
-{
+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
+  if (!PowerManager::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");
+    if(Serial) {
+      Serial.println("Failed to get power for mesh initialization");
+    }
+  }
   mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
   mesh.onReceive(&receivedCallback);
   mesh.onNewConnection(&newConnectionCallback);
@@ -82,6 +94,7 @@ void Communication::begin(void)
   static uint8_t ucParameterToPass;
   TaskHandle_t xHandle = NULL;
 
-    xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass, tskIDLE_PRIORITY, &xHandle);
+  xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass,
+              tskIDLE_PRIORITY, &xHandle);
   configASSERT(xHandle);
 };

src/communication/Communication.h

@@ -1,14 +1,17 @@
 
 #ifndef Communication_h
 #define Communication_h
-#include <stdint.h>
+#include "../power/PowerManager.h"
 #include <Arduino.h>
 #include <painlessMesh.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: 
@@ -23,6 +26,7 @@ public:
     void sendMessage(String msg);
     
     void onReceive(void (*callbackFunc)(String &msg));
+
     private:
     static void (*userCallback)(String &msg);
     static void receivedCallback(uint32_t from, String &msg);

src/display/Display.cpp

@@ -14,6 +14,14 @@
 
 
 void Display::begin(void){
+    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");
+            if (Serial) {
+              Serial.println("Could not get power for Display");
+            }
+    }
     //set Mux Ratio
     sendDisplayCMD(muxRatio);
     sendDisplayCMD(0x3f);
@@ -173,3 +181,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;
+};

src/display/Display.h

@@ -11,9 +11,15 @@
 
 #ifndef Display_h
 #define Display_h
-#include <stdint.h>
-#include <Arduino.h>
+#include "../power/PowerManager.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
+
+#define TAG "Display"
 
 class Display{
     protected:
@@ -121,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);
 };
 
 

src/infraredLight/InfraredLED.cpp

@@ -1,6 +1,10 @@
 #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
+#define DUTY_CYCLE_FREQUENCY 512
 
 InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel){
     this->ledPin = pin;
@@ -12,7 +16,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 +46,32 @@ void InfraredLED::turnOff(void){
 void InfraredLED::setState(bool state){
     ledc_set_freq(pwmSpeedMode,timer,1);
     if (state) {
+      if (this->ledPin == IR_BOTTOM_PIN) {
+        if (!PowerManager::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) {
+        if (!PowerManager::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 {
+      if (this->ledPin == IR_BOTTOM_PIN) {
+        PowerManager::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_BOTTOM);
+      } else {
+        PowerManager::releaseCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT);
+      }
         ledc_set_duty(pwmSpeedMode,channel,0);
     }
     ledc_update_duty(pwmSpeedMode,channel);
@@ -51,7 +79,47 @@ void InfraredLED::setState(bool state){
 };
 
 void InfraredLED::sendFrequency(uint16_t frequency){
+  constexpr uint32_t duty = DUTY_CYCLE_FREQUENCY;
+  if (this->ledPin == IR_BOTTOM_PIN) {
+    PowerManager::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::LED_IR_BOTTOM,
+        this->modelCurrentConsumption(duty), IR_LED_MAX_EXECUTION_DELAY_MS,
+        NULL);
+  } else if (this->ledPin == IR_FRONT_PIN) {
+    PowerManager::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::LED_IR_FRONT,
+        this->modelCurrentConsumption(duty), 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);
 };
+
+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) {
+  if (this->ledPin == IR_BOTTOM_PIN) {
+    return durationMs *
+           (PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM) / 10e6f;
+  } else if (this->ledPin == IR_FRONT_PIN) {
+    return durationMs *
+           (PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT) / 10e6f;
+  }
+  return NAN;
+}
+
+float InfraredLED::modelChargeConsumptionSendFrequency(uint16_t durationMs) {
+    return (durationMs * this->modelCurrentConsumption(DUTY_CYCLE_FREQUENCY)) /
+         10e6f;
+}

src/infraredLight/InfraredLight.h

@@ -10,10 +10,14 @@
  */
 #ifndef InfraredLight_h
 #define InfraredLight_h
-#include <stdint.h>
-#include <Arduino.h>
+#include "../power/PowerManager.h"
 #include "driver/ledc.h"
+#include <Arduino.h>
+#include <stdint.h>
 
+#define IR_LED_MAX_EXECUTION_DELAY_MS 1
+
+#define TAG "InfraredLight"
 
 class InfraredLED{
     public: 
@@ -44,6 +48,31 @@ class InfraredLED{
          * @param frequency 
          */
         void sendFrequency(uint16_t frequency);
+
+        /**
+         * @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;

src/lightDetection/LightDetection.cpp

@@ -64,6 +64,16 @@ uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measu
 };
 
 void LightDetection::beginInfrared(void){
+    if(!PowerManager::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::PT_IR,
+        PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+            ESP_LOGE(TAG,"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);
     pinMode(IR_PT_FRONT_ADC, INPUT);
@@ -73,6 +83,16 @@ void LightDetection::beginInfrared(void){
 };
 
 void LightDetection::beginDaylight(void){
+    if(!PowerManager::waitForCurrentAllowance(
+        PowerParameters::PowerConsumers::PT_DL,
+        PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+            ESP_LOGE(TAG,"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);
     pinMode(DL_PT_BOTTOM_ADC, INPUT);
@@ -99,6 +119,7 @@ uint16_t LightDetection::readIRPT(photoTransistors sensor){
     default:
         break;
     }
+    //PowerManager::releaseCurrent(PowerParameters::PowerConsumers::PT_IR);
     //digitalWrite(IR_PT_ENABLE,LOW);
     return result;
 };
@@ -117,6 +138,20 @@ uint16_t LightDetection::readDLPT(photoTransistors sensor){
     default:
         break;
     }
+    PowerManager::releaseCurrent(PowerParameters::PowerConsumers::PT_DL);
     digitalWrite(DL_PT_ENABLE,LOW);
     return result;
 };
+
+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) / 10e6f;
+}

src/lightDetection/LightDetection.h

@@ -11,10 +11,13 @@
 
 #ifndef LightDetection_h
 #define LightDetection_h
-#include <stdint.h>
+#include "../power/PowerManager.h"
 #include <Arduino.h>
+#include <stdint.h>
 
+#define LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "LightDetection"
 
 enum photoTransistors{
     IR_LEFT,
@@ -75,6 +78,23 @@ public:
      * @return the average of all taken meaurments
      */
     static uint32_t getAverageValue(photoTransistors sensor, uint32_t measurments, uint32_t timeBetween);
+
+    /**
+     * @brief Get current consumption of the selected PTs
+     *
+     * @return float the current consumption of the PTs
+     */
+    static float modelCurrentConsumption(photoTransistors sensor);
+
+    /**
+     * @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
+     */
+    static float modelChargeConsumptionOn(photoTransistors sensor,
+                                   uint16_t durationMs);
+
   protected: 
     static const uint8_t IR_PT_FRONT_ADC = 3;
     static const uint8_t IR_PT_LEFT_ADC = 4;

src/motion/Motion.cpp

@@ -31,8 +31,13 @@ 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)) {
+        if(Serial){
+            Serial.println("Failed 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 +77,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 +124,13 @@ void Motion::leftMotorTask(void * args) {
         xAntiClockwiseTaskHandle = NULL;
     }
     Motion::right.setSpeed(0);
-    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+    if(!Motion::left.setSpeed(LEFT_MOTOR_DUTY)){
+        if(Serial){
+          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 +169,14 @@ void Motion::rightMotorTask(void * args) {
         vTaskDelete(xClockwiseTaskHandle);
         xClockwiseTaskHandle = NULL;
     }
-    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+    if(!Motion::left.setSpeed(RIGHT_MOTOR_DUTY)){
+        if(Serial){
+          Serial.println(
+              "Can not rotate Antilcockwise due to power constraints");
+        }
+        Motion::stop();
+        return;
+    }
     Motion::left.setSpeed(0);
     while(1){
         if(runtime>40||runtime==0){
@@ -197,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);
 }

src/motion/Motion.h

@@ -17,17 +17,23 @@
 #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 
 #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
+
+#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);
+        Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase=0);
         
         /**
          * @brief Initializes the motor 
@@ -40,8 +46,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
@@ -49,18 +57,40 @@ 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 modelChargeConsumption(uint16_t duty, uint16_t durationMs);
+
+
     protected:
         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.
+        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);
@@ -76,8 +106,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;

src/motion/Motor.cpp

@@ -1,11 +1,15 @@
 #include "Motion.h"
+#include "power/PowerManager.h"
 
-Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
+#define MOTOR_LEFT_PIN 12
+#define MOTOR_RIGHT_PIN 11
+
+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->powerManager = *Power::getPowerManager();
+    this->phase = phase;
 };
 
 void Motor::begin(void){
@@ -17,19 +21,34 @@ 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");
 };
 
-void Motor::setSpeed(uint16_t duty){
-    if(duty>0) {
-        powerManager.waitForPowerAllowance(CONSUMPTION_MOTOR, portMAX_DELAY);
-        Serial.println("Motor got power");
+bool Motor::setSpeed(uint16_t duty) {
+  const float current = this->modelCurrentConsumption(duty);
+  if (this->pin == MOTOR_LEFT_PIN) {
+    if (!PowerManager::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;
+    }
   } else {
-        powerManager.releasePower(CONSUMPTION_MOTOR);
-        Serial.println("Motor released power");
+    if (!PowerManager::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) {
@@ -47,9 +66,16 @@ void Motor::setSpeed(uint16_t duty){
       delayMicroseconds(5);
     }
   }
-
+  return true;
 };
 
-uint16_t Motor::getSpeed(void){
-    return this->duty;
-};
+uint16_t Motor::getSpeed(void) { return this->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 Motor::modelChargeConsumption(uint16_t duty, uint16_t durationMs) {
+  return (modelCurrentConsumption(duty) * durationMs) / 10e6f;
+}

src/motionDetection/MotionDetection.cpp

@@ -6,6 +6,15 @@ MotionDetection::MotionDetection(){
 };
 
 void MotionDetection::begin(void){
+  if (!PowerManager::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::IMU,
+          PowerParameters::CurrentConsumptions::CURRENT_IMU,
+          IMU_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Could not get power for MotionDetection");
+    if(Serial){
+      Serial.println("Could not get power for MotionDetection");
+    }
+  }
     pinMode(34,OUTPUT);
     digitalWrite(34,HIGH);
     handler->begin(36,37,35,34);
@@ -21,9 +30,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;
@@ -124,8 +135,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;
     }
@@ -249,9 +260,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));
@@ -285,3 +296,11 @@ void MotionDetection::writeRegister(uint8_t reg, uint8_t value){
     delayMicroseconds(10);
     handler->endTransaction();
 };
+
+float MotionDetection::modelCurrentConsumption(){
+    return PowerParameters::CurrentConsumptions::CURRENT_IMU;
+}
+
+float MotionDetection::modelChargeConsumption(uint16_t durationMs) {
+  return (this->modelCurrentConsumption() * durationMs) / 10e6f;
+}

src/motionDetection/MotionDetection.h

@@ -10,9 +10,13 @@
  */
 #ifndef MotionDetection_h
 #define MotionDetection_h
-#include <SPI.h>
-#include <Arduino.h>
+#include "../power/PowerManager.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;
@@ -183,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

src/multiColorLight/MultiColorLight.cpp

@@ -1,10 +1,28 @@
 #include "MultiColorLight.h"
 
-MultiColorLight::MultiColorLight():rgbLeds(ledAmount,ledPin){
+MultiColorLight::MultiColorLight()
+    : rgbLeds(ledAmount, ledPin) {
 
       };
 
 void MultiColorLight::begin(void) {
+  if (!PowerManager::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT,
+          PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
+          MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
+      PowerManager::waitForCurrentAllowance(
+          PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT,
+          PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE,
+          MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
+      PowerManager::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");
+    if(Serial){
+      Serial.println("Could not get power for MultiColorLight");
+    }
+  }
   rgbLeds.begin();
   this->turnOffLed();
 };
@@ -13,39 +31,79 @@ 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);
+  float totalConsumption = modelCurrentConsumption(normalizedColor);
+  switch (index) {
+  case 0:
+    if (!PowerManager::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;
+    }
+    break;
+  case 1:
+    if (!PowerManager::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 (!PowerManager::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(leds leds, uint32_t color) {
   switch (leds) {
   case TOP_LEFT:
-            MultiColorLight::setLed(1,color);break;
+    MultiColorLight::setLed(1, color);
+    break;
   case TOP_RIGHT:
-            MultiColorLight::setLed(0,color);break;
+    MultiColorLight::setLed(0, color);
+    break;
   case BOTTOM:
-            MultiColorLight::setLed(2,color);break;
+    MultiColorLight::setLed(2, color);
+    break;
   case TOP:
     for (int index = 0; index < 2; index++) {
       MultiColorLight::setLed(index, color);
-            }break;
+    }
+    break;
   case ALL:
     for (int index = 0; index < ledAmount; index++) {
       MultiColorLight::setLed(index, color);
-            }break;
+    }
+    break;
   default:
     // TODO logging
     break;
   }
-
 };
 
-void MultiColorLight::setLed(leds leds, uint8_t red, uint8_t green, uint8_t 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);
 };
@@ -54,7 +112,8 @@ 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){
+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);
@@ -66,19 +125,24 @@ void MultiColorLight::blink(uint16_t amount,uint32_t color, leds leds, uint32_t
 void MultiColorLight::turnOffLed(leds leds) {
   switch (leds) {
   case TOP_LEFT:
-            MultiColorLight::setLed(1,0);break;
+    MultiColorLight::setLed(1, 0);
+    break;
   case TOP_RIGHT:
-            MultiColorLight::setLed(0,0);break;
+    MultiColorLight::setLed(0, 0);
+    break;
   case BOTTOM:
-            MultiColorLight::setLed(2,0);break;
+    MultiColorLight::setLed(2, 0);
+    break;
   case TOP:
     for (int index = 0; index < 2; index++) {
       MultiColorLight::setLed(index, 0);
-            }break;
+    }
+    break;
   case ALL:
     for (int index = 0; index < 3; index++) {
       MultiColorLight::setLed(index, 0);
-            }break;
+    }
+    break;
   default:
     // TODO logging
     break;
@@ -90,7 +154,8 @@ uint32_t MultiColorLight::color(uint8_t r, uint8_t g, uint8_t b){
 };
 
 // PRIVATE
-uint32_t MultiColorLight::normalizeColor(uint32_t color,uint8_t maxBrightness){
+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);
@@ -104,4 +169,80 @@ uint32_t MultiColorLight::normalizeColor(uint32_t color,uint8_t maxBrightness){
     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) / 10e6f;
+};
+
+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);
+    }
+    break;
+  case ALL:
+    for (int index = 0; index < ledAmount; index++) {
+      ledsConsumption +=
+          MultiColorLight::modelChargeConsumption(index, color, durationMs);
+    }
+    break;
+  default:
+    // TODO logging
+    break;
+  }
+  return ledsConsumption;
+};
+
+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);
+};

src/multiColorLight/MultiColorLight.h

@@ -1,7 +1,8 @@
 /**
  * @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
  *
@@ -10,29 +11,31 @@
  */
 #ifndef MultiColorLight_h
 #define MultiColorLight_h
-#include <Adafruit_NeoPixel.h>
+#include "../power/PowerManager.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 
+ * 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 {
 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;
-public:
+  static constexpr int maximumExecutionDelayMs = 10;
 
+public:
   MultiColorLight();
   /**
    * @brief initialize the multicolor component
@@ -45,7 +48,8 @@ public:
    * @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
+   *               component, and BB is the blue component. Each color can range
+   * between 0 to 100
    */
   void setLed(uint8_t index, uint32_t color);
 
@@ -55,7 +59,8 @@ public:
    * @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
+   *               component, and BB is the blue component. Each color can range
+   * between 0 to 100
    */
   void setLed(leds leds, uint32_t color);
 
@@ -69,12 +74,74 @@ public:
    */
   void setLed(leds leds, uint8_t red, uint8_t green, uint8_t blue);
 
+  /**
+   * @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 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
+   *               component, and BB is the blue component. Each color can range
+   * between 0 to 100
    */
   void setTopLeds(uint32_t color);
 
@@ -99,7 +166,8 @@ public:
    * @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);
+  void blink(uint16_t amount, uint32_t color = 0x00006400, leds leds = TOP,
+             uint32_t interval = 1000);
 
   /**
    * @brief turn off the given leds
@@ -127,13 +195,15 @@ private:
    * @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
+   * @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);
+  uint32_t normalizeColor(uint32_t color,
+                          uint8_t maxBrightness = defaultMaxBrightness);
 };
 
 #endif // MultiColorLight_h

src/power/Consumptions.h

@@ -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

src/power/Power.cpp

@@ -1,122 +0,0 @@
-/**
- * @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"
-static portMUX_TYPE mux;
-
-Power::Power()
-{
-    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->freePowerBudget >= neededPower)
-    {
-        this->freePowerBudget -= neededPower;
-        return true;
-    }
-    else
-    {
-        return false;
-    }
-}
-
-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);
-    }
-}

src/power/Power.h

@@ -1,60 +0,0 @@
-/**
- * @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 <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  POWER_BUDGET
-
-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();
-
-    public:
-    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 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

src/power/PowerManager.cpp

@@ -0,0 +1,381 @@
+/**
+ * @file PowerManager.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 "PowerManager.h"
+
+SemaphoreHandle_t PowerManager::powerMutex = NULL;
+
+void vTaskUpdatePowerState(void *pvParameters) {
+  for (;;) {
+    ESP_LOGV(TAG, "Updating Power State...");
+    TickType_t executionStart = xTaskGetTickCount();
+    PowerManager::updatePowerStateHandler();
+    vTaskDelayUntil(
+        &executionStart,
+        pdMS_TO_TICKS(PowerParameters::POWER_STATE_UPDATE_INTERVAL_MS));
+  }
+}
+
+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");
+
+  // 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");
+    }
+  }
+
+  // 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(
+        PowerParameters::Battery::CELL_CURRENT_1C_MA,
+        PowerParameters::Battery::CELL_CURRENT_2C_MA);
+    PowerManager::initPowerState();
+    PowerManager::recalculateCurrentBudgets();
+    TaskHandle_t xHandle = NULL;
+    xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL,
+                tskIDLE_PRIORITY, &xHandle);
+    configASSERT(xHandle);
+
+    if (!(powerScheduler->tryAccquireCurrentAllowance(
+            PowerParameters::PowerConsumers::ESP,
+            PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
+      ESP_LOGE(TAG, "Could not get power for ESP");
+      Serial.println("Could not get power for ESP");
+    }
+  }
+}
+
+float PowerManager::getFreeLimitCurrentBudget() {
+  return powerScheduler->getFreeLimitCurrentBudget();
+}
+
+float PowerManager::getFreeMaximumCurrentBudget() {
+  return powerScheduler->getFreeMaximumCurrentBudget();
+}
+
+bool PowerManager::tryAccquireCurrentAllowance(
+    PowerParameters::PowerConsumers consumer, uint16_t neededCurrent,
+    uint16_t requestedDurationMs) {
+  return powerScheduler->tryAccquireCurrentAllowance(consumer, neededCurrent,
+                                                     requestedDurationMs);
+}
+
+void PowerManager::releaseCurrent(PowerParameters::PowerConsumers consumer) {
+  powerScheduler->releaseCurrent(consumer);
+}
+
+bool PowerManager::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer,
+                                    uint16_t neededCurrent,
+                                    uint16_t maxSlackTimeMs,
+                                    uint16_t requestedDurationMs) {
+  return powerScheduler->waitForCurrentAllowance(
+      consumer, neededCurrent, maxSlackTimeMs, requestedDurationMs);
+}
+
+void PowerManager::beginPermanentDeepSleep(void) {
+  return powerScheduler->beginPermanentDeepSleep();
+}
+
+float PowerManager::getCurrentCurrent(void) {
+  return powerScheduler->getCurrentCurrent();
+}
+
+float PowerManager::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 PowerManager::recalculateCurrentBudgets(void) {
+  return powerScheduler->recalculateCurrentBudgets();
+}
+
+float PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers consumer) {
+  return powerScheduler->getConsumerCurrent(consumer);
+}
+
+float PowerManager::getBatteryVoltage() {
+  // Get the battery voltage from the ADC and convert it to a voltage
+  // using the voltage divider.
+  // 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);
+  // Disable voltage divider
+  digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW);
+  // Convert ADC value to voltage
+  float batteryVoltage =
+      (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;
+}
+
+float PowerManager::getBatteryChargePercent() { return percentRemaining; }
+
+float PowerManager::getBatteryChargeCoulombs() { return coloumbsRemaining; }
+
+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() + PowerManager::fullVoltageOffset;
+  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;
+    }
+  }
+}
+
+void PowerManager::updatePowerStateHandler() {
+
+  // Update supply and charge state flags
+  PowerManager::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS);
+  PowerManager::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT);
+
+  // If the battery is charging and fully charged
+  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);
+
+  float currentCurrent = powerScheduler->getCurrentCurrent();
+  int referenceCurrentMa =
+      PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000;
+
+  float coloumbsConsumedSinceLastUpdate;
+
+  // Calculate remaining battery charge in Coulombs based on current and time
+  if (!PowerManager::busPowered) {
+    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 (!PowerManager::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];
+      chargeState = oldChargeState -
+                    ((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);
+  }
+
+  // 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++) {
+    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];
+  }
+  PowerManager::percentRemaining =
+      sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES;
+
+  // Update last update time
+  PowerManager::lastPowerStateUpdate = xTaskGetTickCount();
+
+  // Update the available current (changes based on battery state of charge)
+  PowerManager::powerScheduler->recalculateCurrentBudgets();
+  ESP_LOGD(TAG, "Current: %f mA, Charge: %f Coulombs, %f %%", currentCurrent,
+           coloumbsRemaining, percentRemaining);
+
+  return;
+}
+
+float PowerManager::getMax3V3Current() {
+  // Conversion from Thesis
+  float u_bat = getBatteryVoltage();
+  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;
+}
+
+void PowerManager::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;
+}
+
+void PowerManager::initPowerState(void) {
+  // Initialize the power state
+  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;
+  // Set up flags and pins for them
+  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);
+}
+
+void PowerManager::dumpPowerStatistics() {
+  Serial.printf("======== Dezibot Power Statistics ========\r\n");
+  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",
+                PowerManager::getBatteryChargeCoulombs());
+  Serial.printf("Max 3.3V Current in this state (1C, 2C): %f mA, %f mA \r\n",
+                PowerManager::getMax3V3Current(), PowerManager::getMax3V3Current() * 2);
+  Serial.printf("=========================================\r\n");
+}
+
+void PowerManager::dumpConsumerStatistics() {
+  Serial.printf("======== Dezibot Consumer Statistics ========\r\n");
+  Serial.printf("ESP: %f mA\r\n", PowerManager::getConsumerCurrent(
+                                      PowerParameters::PowerConsumers::ESP));
+  Serial.printf("WIFI: %f mA\r\n", PowerManager::getConsumerCurrent(
+                                       PowerParameters::PowerConsumers::WIFI));
+  Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n",
+                PowerManager::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT));
+  Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n",
+                PowerManager::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT));
+  Serial.printf("LED_RGB_BOTTOM: %f mA\r\n",
+                PowerManager::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_RGB_BOTTOM));
+  Serial.printf(
+      "RGBW_SENSOR: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR));
+  Serial.printf("LED_IR_BOTTOM: %f mA\r\n",
+                PowerManager::getConsumerCurrent(
+                    PowerParameters::PowerConsumers::LED_IR_BOTTOM));
+  Serial.printf(
+      "LED_IR_FRONT: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT));
+  Serial.printf(
+      "PT_IR: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR));
+  Serial.printf(
+      "PT_DL: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL));
+  Serial.printf(
+      "LED_UV: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV));
+  Serial.printf(
+      "DISPLAY_OLED: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED));
+  Serial.printf(
+      "MOTOR_LEFT: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT));
+  Serial.printf(
+      "MOTOR_RIGHT: %f mA\r\n",
+      PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT));
+  Serial.printf("IMU: %f mA\r\n", PowerManager::getConsumerCurrent(
+                                      PowerParameters::PowerConsumers::IMU));
+  Serial.printf("=============================================\r\n");
+}
+
+bool PowerManager::isUSBPowered() { return busPowered; }
+
+bool PowerManager::isBatteryPowered() { return !busPowered; }
+
+bool PowerManager::isBatteryCharging() { return chargingState && busPowered; }
+
+bool PowerManager::isBatteryDischarging() { return !chargingState && !busPowered; }
+
+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 PowerManager::percentRemaining = 100.0;
+PowerScheduler *PowerManager::powerScheduler = nullptr;
+
+bool PowerManager::busPowered = false;
+
+bool PowerManager::chargingState = false;
+
+float PowerManager::fullVoltageOffset = 0;
+
+PowerManager::PowerManager() {}
+
+PowerManager::~PowerManager() {}

src/power/PowerManager.h

@@ -0,0 +1,198 @@
+/**
+ * @file PowerManager.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 "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 PowerManager_h
+#define PowerManager_h
+
+#define TAG "Power"
+
+enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
+
+class PowerManager {
+
+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() const { return locked; }
+  };
+
+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;
+
+  static bool busPowered;
+
+  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];
+  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);
+  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);
+  /// @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 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,
+                                      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 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);
+
+  // @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 float getBatteryChargePercent();
+
+  /// @brief Get estimated battery charge state as percentage based on
+  // voltage directly
+  /// @return Battery charge state in percent
+  static float getBatteryVoltageChargePercent();
+
+  /// @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 getMax3V3Current();
+
+  /// @brief update Power State
+  /// @note needs to be public for task creation
+  static void updatePowerStateHandler();
+
+  /// @brief dump power statistics to serial
+  static void dumpPowerStatistics();
+
+  /// @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 PowerManager power;
+
+#endif // Power

src/power/PowerParameters.h

@@ -0,0 +1,123 @@
+/**
+ * @file PowerParameters.h
+ * @author Phillip Kühne
+ * @brief
+ * @version 0.1
+ * @date 2024-11-28
+ *
+ * @copyright (c) 2024
+ *
+ */
+
+#ifndef PowerParameters_h
+#define PowerParameters_h
+
+#include <cstdint>
+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.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};
+  };
+
+  // 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 = 20;
+};
+
+// 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
+};
+
+static constexpr uint32_t POWER_STATE_UPDATE_INTERVAL_MS = 10;
+
+}; // namespace PowerParameters
+
+#endif // Consumptions_h

src/power/PowerScheduler.cpp

@@ -0,0 +1,333 @@
+/**
+ * @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"
+#include "PowerManager.h"
+
+bool PowerScheduler::tryAccquireCurrentAllowance(
+    PowerParameters::PowerConsumers consumer, float neededCurrent,
+    uint16_t requestedDurationMs) {
+  float existingConsumption = getConsumerCurrent(consumer);
+  const bool currentLimitNotExceeded =
+      this->freeLimitCurrentBudget + existingConsumption > 0;
+  const bool neededCurrentAvailableBelowMaximum =
+      this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
+  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");
+      return false;
+    }
+    this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{
+        .consumer = consumer,
+        .maxSlackTimeMs = 0,
+        .requestedDurationMs = requestedDurationMs,
+        .taskHandle = xTaskGetCurrentTaskHandle(),
+        .neededCurrent = neededCurrent,
+        .requestedAt = xTaskGetTickCount(),
+        .grantedAt = xTaskGetTickCount(),
+        .granted = true});
+    this->recalculateCurrentBudgets();
+    lock.unlock();
+    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) {
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return;
+  }
+  for (auto it = currentAllowances.begin(); it != currentAllowances.end();
+       ++it) {
+    if (it->consumer == consumer) {
+      currentAllowances.erase(it);
+      break;
+    }
+  }
+  lock.unlock();
+  recalculateCurrentBudgets();
+  // Check if there are tasks waiting for power
+  checkWaitingTasks();
+}
+
+bool PowerScheduler::waitForCurrentAllowance(
+    PowerParameters::PowerConsumers consumer, float 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};
+    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));
+    // 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
+        // Exclude existing consumption from the same consumer, as it will be
+        // gone if this is granted
+        float existingConsumption = getConsumerCurrent(consumer);
+        const bool currentLimitNotExceeded =
+            this->freeLimitCurrentBudget + existingConsumption > 0;
+        const bool neededCurrentAvailableBelowMaximum =
+            this->freeMaximumCurrentBudget + existingConsumption >=
+            neededCurrent;
+        const bool currentIsInsignificant = neededCurrent < 1;
+        if (currentIsInsignificant ||
+            (currentLimitNotExceeded && neededCurrentAvailableBelowMaximum)) {
+          PowerSchedulerMutex lock(powerSchedulerMutex);
+          if (lock.isLocked() == false) {
+            return false;
+          }
+          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});
+          ESP_LOGI(TAG, "%d mA granted to consumer %d with a delay of %d ms",
+                   neededCurrent, static_cast<int>(consumer),
+                   xTaskGetTickCount() - initialTickCount);
+          return true;
+        } else {
+          // Still not enough power available for us. Wait the remaining ticks.
+          xTaskNotifyWait(0, 0, &notificationValue,
+                          pdMS_TO_TICKS(maxSlackTimeMs) -
+                              (xTaskGetTickCount() - initialTickCount));
+        }
+      }
+    }
+    if (notificationStatus == pdFALSE) {
+      // We waited long enough...
+      // Give up and 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) {
+          currentAllowances.erase(it);
+          break;
+        }
+      }
+      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(),
+               this->maximumCurrent);
+      return false;
+    } else {
+      // Should be impossible to reach
+      throw "Reached impossible state";
+    }
+  }
+}
+
+void PowerScheduler::checkWaitingTasks(void) {
+  // If there are requested allowances, try to grant the one expiring next
+  if (this->currentAllowances.size() > 0) {
+
+    PowerScheduler::CurrentAllowance *nextAllowance = getNextCurrentAllowance();
+    if (nextAllowance != nullptr) {
+      xTaskNotify(nextAllowance->taskHandle,
+                  PowerScheduler::PowerWakeupReasons::POWER_AVAILABLE,
+                  eSetValueWithOverwrite);
+    }
+  }
+}
+
+void PowerScheduler::recalculateCurrentBudgets(void) {
+
+  // Get the respective maximums and subtract currently flowing currents
+  ESP_LOGI(TAG, "Recalculating current budgets...");
+  float tempFreeLimitCurrentBudget = PowerManager::getMax3V3Current();
+  ESP_LOGI(TAG, "Got max 3V3 current: %.2f", tempFreeLimitCurrentBudget);
+  float tempFreeMaximumCurrentBudget = PowerManager::getMax3V3Current() * 2;
+  PowerSchedulerMutex lock(powerSchedulerMutex);
+  if (lock.isLocked() == false) {
+    return;
+  }
+  for (auto &allowance : currentAllowances) {
+    if (allowance.granted) {
+      tempFreeLimitCurrentBudget -= allowance.neededCurrent;
+      tempFreeMaximumCurrentBudget -= allowance.neededCurrent;
+    }
+  }
+  this->freeLimitCurrentBudget = tempFreeLimitCurrentBudget;
+  this->freeMaximumCurrentBudget = tempFreeMaximumCurrentBudget;
+  ESP_LOGV(TAG, "Current budgets recalculated: %d mA, %d mA",
+           this->freeLimitCurrentBudget, this->freeMaximumCurrentBudget);
+}
+
+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;
+    }
+  }
+  return nullptr;
+}
+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;
+    }
+  }
+  return nullptr;
+}
+PowerScheduler::CurrentAllowance *PowerScheduler::getNextCurrentAllowance(void) {
+  TickType_t minTicks = UINT32_MAX;
+  CurrentAllowance *nextAllowance = nullptr;
+
+  std::vector<CurrentAllowance> sortedAllowances;
+  sortedAllowances.reserve(currentAllowances.size());
+  for (auto &allowance : currentAllowances) {
+    if (!(allowance.granted)) {
+      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 nullptr;
+}
+
+PowerScheduler &PowerScheduler::getPowerScheduler(float i_limit_ma,
+                                                  float i_max_ma) {
+  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;
+    }
+  }
+  return currentSum;
+}
+
+float PowerScheduler::getFreeLimitCurrentBudget(void) {
+  return this->freeLimitCurrentBudget;
+}
+
+float PowerScheduler::getFreeMaximumCurrentBudget(void) {
+  return this->freeMaximumCurrentBudget;
+}
+
+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) {
+      currentSum += allowance.neededCurrent;
+    }
+  }
+  return currentSum;
+}
+
+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();
+}
+
+PowerScheduler::~PowerScheduler() {
+  if (powerSchedulerMutex != NULL) {
+    vSemaphoreDelete(powerSchedulerMutex);
+  }
+}

src/power/PowerScheduler.h

@@ -0,0 +1,173 @@
+/**
+ * @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 "PowerParameters.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include <Arduino.h>
+#include <vector>
+
+#ifndef PowerScheduler_h
+#define PowerScheduler_h
+
+#define TAG "PowerScheduler"
+
+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
+  /// @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)
+  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 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
+  /// @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);
+  /// @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
+  /// "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,
+                               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
+  /// 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)
+  float getCurrentCurrent(void);
+
+  /// @brief Power consumer data structure
+  struct CurrentAllowance {
+    PowerParameters::PowerConsumers consumer;
+    uint16_t maxSlackTimeMs;
+    uint16_t requestedDurationMs;
+    TaskHandle_t taskHandle;
+    float neededCurrent;
+    TickType_t requestedAt;
+    TickType_t grantedAt;
+    bool granted;
+  };
+
+  // @brief waiting task wakeup reasons
+  enum PowerWakeupReasons {
+    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 *getNextCurrentAllowance(void);
+
+protected:
+  // 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 Mutex to protect the power scheduler from concurrent access
+
+  std::vector<PowerScheduler::CurrentAllowance> currentAllowances;
+};
+
+static PowerScheduler *powerSchedulerInstance;
+
+#endif // PowerScheduler_h

tools/generate_clangd_config.sh

@@ -0,0 +1,50 @@
+#!/bin/bash
+
+SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "$0")")" && pwd)"
+PROJECT_PATH="$(cd "$SCRIPT_DIR/.." && pwd)"
+
+# Default values that can be overridden by environment variables
+# The ':=' operator means "use the environment variable if set, otherwise use this default"
+: "${BOARD_FQBN:=esp32:esp32:esp32s3usbotg:USBMode=hwcdc}"
+: "${LIBRARY_PATH:=$PROJECT_PATH}"
+: "${BUILD_PATH:=$PROJECT_PATH/build}"
+: "${BASE_INO_SKETCH_PATH:=$PROJECT_PATH/example/start}"
+: "${COMPILE_COMMANDS:=compile_commands.json}"
+
+
+mkdir -p "$BUILD_PATH"
+
+if ! command -v arduino-cli >/dev/null 2>&1; then
+    echo "Error: arduino-cli is not installed or not in PATH"
+    echo "Please install it from https://arduino.github.io/arduino-cli/latest/installation/"
+    exit 1
+fi
+
+echo "Generating clangd configuration for $BOARD_FQBN in $BUILD_PATH"
+echo "Based on compilation of $BASE_INO_SKETCH_PATH"
+echo "This may take a while and will not produce any output until it's done."
+echo "Please be patient..."
+
+if ! arduino-cli compile \
+    --fqbn "$BOARD_FQBN" \
+    --library "$LIBRARY_PATH" \
+    --only-compilation-database \
+    --build-path "$BUILD_PATH" \
+    "$BASE_INO_SKETCH_PATH"; then
+    echo "Error: Failed to generate compilation database"
+    exit 1
+fi
+
+# Create symbolic link with error handling
+if [ -f "$BUILD_PATH/$COMPILE_COMMANDS" ]; then
+    ln -sf "$BUILD_PATH/$COMPILE_COMMANDS" "$PROJECT_PATH/$COMPILE_COMMANDS"
+    echo "Successfully placed clangd configuration in project root"
+    echo "You no longer have to use the Arduino IDE for code completion \o/"
+    echo "Feel free to use any editor that supports clangd (e.g via LSP)"
+    echo "For example, Visual Studio Code with the 'clangd' extension, or VIM (with YouCompleteMe), or Emacs (with Eglot), or ..."
+    echo "Refer to https://clangd.llvm.org/installation#editor-plugins for more information"
+
+else
+    echo "Error: Could not link \"complile_commands.json\" to the root directory. Please manually copy where needed."
+    exit 1
+fi