Handle denials of power appropriately

src/colorDetection/ColorDetection.cpp

@@ -1,8 +1,15 @@
 #include  "ColorDetection.h"
 
 void ColorDetection::begin(void){
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::RGBW_SENSOR, PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW, COLOR_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
+  if(!Power::waitForCurrentAllowance(
-    ColorDetection::configure(VEML_CONFIG{.mode = AUTO,.enabled = true,.exposureTime=MS40});
+      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;

src/colorDetection/ColorDetection.h

@@ -31,6 +31,7 @@
 
 #define COLOR_DETECTION_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "ColorDetection"
 
 enum duration{
     MS40,

src/communication/Communication.cpp

@@ -45,12 +45,16 @@ void nodeTimeAdjustedCallback(int32_t offset) {
 
 void vTaskUpdate(void *pvParameters) {
   for (;;) {
-    Power::waitForCurrentAllowance(
+    if (Power::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::WIFI,
             PowerParameters::CurrentConsumptions::CURRENT_WIFI_PEAK +
                 PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-        MESH_MAX_EXECUTION_DELAY_MS, NULL);
+            MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
+
       mesh.update();
+    } else {
+      ESP_LOGW(TAG, "Skipping mesh update after not being granted power");
+    }
     Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::WIFI,
         PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
@@ -73,10 +77,13 @@ void Communication::begin(void) {
   mesh.setDebugMsgTypes(
       ERROR |
       STARTUP); // set before init() so that you can see startup messages
-  Power::waitForCurrentAllowance(
+  if (!Power::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::WIFI,
           PowerParameters::CurrentConsumptions::CURRENT_WIFI_BASE,
-      MESH_MAX_EXECUTION_DELAY_MS, NULL);
+          MESH_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Failed to get power for mesh initialization");
+    throw "Failed to get power for mesh initialization";
+  }
   mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
   mesh.onReceive(&receivedCallback);
   mesh.onNewConnection(&newConnectionCallback);

src/communication/Communication.h

@@ -1,18 +1,17 @@
 
 #ifndef Communication_h
 #define Communication_h
-#include <stdint.h>
+#include "../power/Power.h"
 #include <Arduino.h>
 #include <painlessMesh.h>
-#include "../power/Power.h"
+#include <stdint.h>
 
 #define   MESH_PREFIX     "DEZIBOT_MESH"
 #define   MESH_PASSWORD   "somethingSneaky"
 #define   MESH_PORT       5555
 #define   MESH_MAX_EXECUTION_DELAY_MS 10
 
-
+#define TAG "Communication"
-
 
 class Communication{
     public: 

src/display/Display.cpp

@@ -14,9 +14,12 @@
 
 
 void Display::begin(void){
-    Power::waitForCurrentAllowance(
+    if(!Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::DISPLAY_OLED, PowerParameters::CurrentConsumptions::CURRENT_DISPLAY
-        ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL);
+        ,DISPLAY_MAX_EXECUTION_DELAY_MS, NULL)){
+            ESP_LOGE(TAG,"Could not get power for Display");
+            throw "Could not get power for Display";
+    }
     //set Mux Ratio
     sendDisplayCMD(muxRatio);
     sendDisplayCMD(0x3f);

src/display/Display.h

@@ -19,6 +19,8 @@
 // This execution delay is basically only used for initial activation, so it can be set to a higher value
 #define DISPLAY_MAX_EXECUTION_DELAY_MS 100
 
+#define TAG "Display"
+
 class Display{
     protected:
         //how many chars are on current line

src/infraredLight/InfraredLED.cpp

@@ -46,15 +46,23 @@ void InfraredLED::setState(bool state){
     ledc_set_freq(pwmSpeedMode,timer,1);
     if (state) {
       if (this->ledPin == IR_BOTTOM_PIN) {
-        Power::waitForCurrentAllowance(
+        if (!Power::waitForCurrentAllowance(
                 PowerParameters::PowerConsumers::LED_IR_BOTTOM,
                 PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM,
-            IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+                IR_LED_MAX_EXECUTION_DELAY_MS, NULL)) {
+          ESP_LOGE(TAG,
+                   "Could not get power for Bottom IR LED. Not turning on.");
+          return;
+        }
       } else if (this->ledPin == IR_FRONT_PIN) {
-        Power::waitForCurrentAllowance(
+        if (!Power::waitForCurrentAllowance(
                 PowerParameters::PowerConsumers::LED_IR_FRONT,
                 PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT,
-            IR_LED_MAX_EXECUTION_DELAY_MS, NULL);
+                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 {

src/infraredLight/InfraredLight.h

@@ -17,6 +17,7 @@
 
 #define IR_LED_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "InfraredLight"
 
 class InfraredLED{
     public: 

src/lightDetection/LightDetection.cpp

@@ -64,10 +64,13 @@ uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measu
 };
 
 void LightDetection::beginInfrared(void){
-    Power::waitForCurrentAllowance(
+    if(!Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::PT_IR,
         PowerParameters::CurrentConsumptions::CURRENT_PT * 4,
-        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+            ESP_LOGE(TAG,"Could not get power for Infrared Phototransistors");
+            throw "Could not get power for Infrared Phototransistors";
+    }
     digitalWrite(IR_PT_ENABLE,true);
     pinMode(IR_PT_ENABLE, OUTPUT);
     pinMode(IR_PT_FRONT_ADC, INPUT);
@@ -77,10 +80,13 @@ void LightDetection::beginInfrared(void){
 };
 
 void LightDetection::beginDaylight(void){
-    Power::waitForCurrentAllowance(
+    if(!Power::waitForCurrentAllowance(
         PowerParameters::PowerConsumers::PT_DL,
         PowerParameters::CurrentConsumptions::CURRENT_PT * 2,
-        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL);
+        LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS, NULL)) {
+            ESP_LOGE(TAG,"Could not get power for Daylight Phototransistors");
+            throw "Could not get power for Daylight Phototransistors";
+    }
     digitalWrite(DL_PT_ENABLE,true);
     pinMode(DL_PT_ENABLE, OUTPUT);
     pinMode(DL_PT_BOTTOM_ADC, INPUT);

src/lightDetection/LightDetection.h

@@ -17,6 +17,8 @@
 
 #define LIGHT_DETECTION_MAX_EXECUTION_DELAY_MS 1
 
+#define TAG "LightDetection"
+
 enum photoTransistors{
     IR_LEFT,
     IR_RIGHT,

src/motion/Motion.h

@@ -28,6 +28,8 @@
 
 #define MOTOR_MAX_EXECUTION_DELAY_MS 100
 
+#define TAG "Motion"
+
 class Motor{
     public:
         Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);

src/motion/Motor.cpp

@@ -29,10 +29,24 @@ void Motor::begin(void){
 void Motor::setSpeed(uint16_t duty){
     const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
     const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
-    if (this->pin == MOTOR_LEFT_PIN){
+    if (this->pin == MOTOR_LEFT_PIN) {
-        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_LEFT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+      if (!Power::waitForCurrentAllowance(
+              PowerParameters::PowerConsumers::MOTOR_LEFT, current,
+              MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
+        ESP_LOGW(TAG,
+                 "Power to set LEFT MOTOR to speed %d not granted in time. "
+                 "Skipping.",
+                 duty);
+      }
     } else {
-        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_RIGHT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+      if (!Power::waitForCurrentAllowance(
+              PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
+              MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
+        ESP_LOGW(TAG,
+                 "Power to set RIGHT MOTOR to speed %d not granted in time. "
+                 "Skipping.",
+                 duty);
+      }
     }
     int difference = duty-this->getSpeed();
     if (difference > 0){

src/motionDetection/MotionDetection.cpp

@@ -6,10 +6,13 @@ MotionDetection::MotionDetection(){
 };
 
 void MotionDetection::begin(void){
-    Power::waitForCurrentAllowance(
+  if (!Power::waitForCurrentAllowance(
           PowerParameters::PowerConsumers::IMU,
           PowerParameters::CurrentConsumptions::CURRENT_IMU,
-        IMU_MAX_EXECUTION_DELAY_MS, NULL);
+          IMU_MAX_EXECUTION_DELAY_MS, NULL)) {
+    ESP_LOGE(TAG, "Could not get power for MotionDetection");
+    throw "Could not get power for MotionDetection";
+  }
     pinMode(34,OUTPUT);
     digitalWrite(34,HIGH);
     handler->begin(36,37,35,34);

src/multiColorLight/MultiColorLight.cpp

@@ -5,13 +5,17 @@ MultiColorLight::MultiColorLight():rgbLeds(ledAmount,ledPin){
 };
 
 void MultiColorLight::begin(void){
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+    if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) &&
-    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+    Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL) ){
+      ESP_LOGE(TAG, "Could not get power for MultiColorLight");
+      throw "Could not get power for MultiColorLight";
+    }
     rgbLeds.begin();
     this->turnOffLed();
 };
 
+
 void MultiColorLight::setLed(uint8_t index , uint32_t color){
     if (index > ledAmount-1){
         //TODO: logging
@@ -26,13 +30,22 @@ void MultiColorLight::setLed(uint8_t index , uint32_t color){
     float totalConsumption = redChannelConsumption + greenChannelConsumption + blueChannelConsumption + PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE;
     switch (index) {
         case 0:
-            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
+                ESP_LOGW(TAG,  "Power to set LED RGB TOP RIGHT to color %d not granted in time. Skipping.", color);
+                return;
+            }
             break;
         case 1:
-            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
+                ESP_LOGW(TAG,  "Power to set LED RGB TOP LEFT to color %d not granted in time. Skipping.", color);
+                return;
+            }
             break;
         case 2:
-            Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL);
+            if(!Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::LED_RGB_BOTTOM, totalConsumption, MULTI_COLOR_LIGHT_MAX_EXECUTION_DELAY_MS, NULL)){
+                ESP_LOGW(TAG,  "Power to set LED RGB BOTTOM to color %d not granted in time. Skipping.", color);
+                return;
+            }
             break;
     }
     rgbLeds.setPixelColor(index, normalizedColor);

src/multiColorLight/MultiColorLight.h

@@ -29,6 +29,8 @@ enum leds{
     ALL
 };
 
+#define TAG "MultiColorLight"
+
 class MultiColorLight{
 protected:
     static const uint16_t ledAmount = 3;

src/power/Power.cpp

@@ -19,9 +19,8 @@ void Power::begin() {
     if (!(powerScheduler->tryAccquireCurrentAllowance(
             PowerParameters::PowerConsumers::ESP,
             PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) {
-      Serial.println("Alledgedly not enough power available to reserve the "
+      ESP_LOGE(TAG, "Could not get power for ESP");
-                     "ESP32s base power consumption. Something is wrong.");
+      throw "Could not get power for ESP";
-      return;
     }
   }
 }

src/power/Power.h

@@ -11,6 +11,8 @@
 #ifndef Power_h
 #define Power_h
 
+#define TAG "Power"
+
 enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT };
 
 class Power {

src/power/PowerScheduler.cpp

@@ -22,7 +22,7 @@ bool PowerScheduler::tryAccquireCurrentAllowance(
       this->freeLimitCurrentBudget + existingConsumption > 0;
   const bool currentAvailableBelowMaximum =
       this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent;
-  const bool currentIsInsignificant = neededCurrent < 0.1;
+  const bool currentIsInsignificant = neededCurrent < 1;
   if (currentIsInsignificant ||
       (currentAvailableBelowLimit && currentAvailableBelowMaximum)) {
     if (existingConsumption > 0) {
@@ -115,6 +115,9 @@ bool PowerScheduler::waitForCurrentAllowance(
                .requestedAt = initialTickCount,
                .grantedAt = xTaskGetTickCount(),
                .granted = true});
+          ESP_LOGV(TAG, "%d mA granted to consumer %d after %d ms",
+                   neededCurrent, static_cast<int>(consumer),
+                   xTaskGetTickCount() - initialTickCount);
           return true;
         } else {
           // Still not enough power available for us. Wait the remaining ticks.

src/power/PowerScheduler.h

@@ -19,6 +19,8 @@
 #ifndef PowerScheduler_h
 #define PowerScheduler_h
 
+#define TAG "PowerScheduler"
+
 class PowerScheduler {
 private:
   static constexpr uint16_t DEFAULT_SLACK_TIME_MS = 100;
@@ -43,6 +45,9 @@ public:
   /// @param neededCurrent the amount of current we want to be accounted for (in
   /// mA)
   /// @return whether the current could be successfully allocated
+  /// @note This takes existing power consumption by the same consumer into account,
+  /// so requesting the same or less power will always succeed. Also, small amounts
+  /// of power (below 1 mA) will always be granted.
   bool tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer,
                                    uint16_t neededcurrent,
                                    uint16_t requestedDurationMs = 0);