Power management progress

example/advanced/Power_Measurements/Display/Display.ino

@@ -0,0 +1,26 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+
+// How many times to run a command on the display consecutively;
+const uint16_t iterations = 5000;
+
+void setup() {
+  dezibot.begin();
+}
+
+void loop() {
+  //Typical output
+  dezibot.display.println("Typical output");
+  for(uint16_t iter=0; iter<iterations; iter++) {
+    dezibot.display.println(iter);
+  }
+  delay(10);
+  //Lots of pixels turned on
+  dezibot.display.invertColor();
+  dezibot.display.println("Inverted output");
+  for(uint16_t iter=0; iter<iterations; iter++) {
+    dezibot.display.println(iter);
+  }
+  delay(10);
+}

example/advanced/Power_Measurements/ESP32_baseline/ESP32_baseline.ino

@@ -0,0 +1,136 @@
+#include "Dezibot.h"
+#include "esp_pm.h"
+#include "esp_task_wdt.h"
+
+Dezibot dezibot;
+
+const uint16_t cycleTime = 2e4;  //20000 ms = 20 s
+
+esp_pm_lock_handle_t cpuFreqLock;
+esp_pm_lock_handle_t apbFreqLock;
+esp_pm_lock_handle_t lightSleepLock;
+static bool pmLocksCreated = false;
+
+
+void stress_task0(void *pvParameters) {
+  // Register ourselves with the task watchdog
+  if (esp_task_wdt_add(NULL) != ESP_OK) {
+    Serial.println("Failed to add task 0 to TWDT");
+  }
+  while (1) {
+    volatile uint32_t x = 0;
+    for (uint32_t i = 0; i < 10000; i++) {
+      x += i;
+    }
+    esp_task_wdt_reset();
+  }
+}
+
+void stress_task1(void *pvParameters) {
+  // Register ourselves with the task watchdog
+  if (esp_task_wdt_add(NULL) != ESP_OK) {
+    Serial.println("Failed to add task 0 to TWDT");
+  }
+  while (1) {
+    volatile uint32_t x = 0;
+    for (uint32_t i = 0; i < 10000; i++) {
+      x += i;
+    }
+    esp_task_wdt_reset();
+  }
+}
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) {
+    ;
+  }
+  uint32_t Freq = getCpuFrequencyMhz();
+  Serial.print("CPU Freq = ");
+  Serial.print(Freq);
+  Serial.println(" MHz");
+  Freq = getXtalFrequencyMhz();
+  Serial.print("XTAL Freq = ");
+  Serial.print(Freq);
+  Serial.println(" MHz");
+  Freq = getApbFrequency();
+  Serial.print("APB Freq = ");
+  Serial.print(Freq);
+  Serial.println(" Hz");
+  // Create lock handles for controlling power management features
+  // Parameter arg is always unused and should thus be set to 0
+  // Ref: https://docs.espressif.com/projects/esp-idf/en/v3.3.6/api-reference/system/power_management.html#enumerations
+  if (!pmLocksCreated) {
+    bool cpuFreqLockSuccess = esp_pm_lock_create(ESP_PM_CPU_FREQ_MAX, 0, "CPU_FREQ_MAX", &cpuFreqLock);
+    bool apbFreqLockSuccess = esp_pm_lock_create(ESP_PM_APB_FREQ_MAX, 0, "APB_FREQ_MAX", &apbFreqLock);
+    bool lightSleepLockSuccess = esp_pm_lock_create(ESP_PM_NO_LIGHT_SLEEP, 0, "NO_LIGHT_SLEEP", &lightSleepLock);
+    if ((cpuFreqLockSuccess && apbFreqLockSuccess && lightSleepLockSuccess)) {
+      pmLocksCreated = true;
+      Serial.println("Sucessfully created handles to control power management features.");
+    } else {
+      Serial.println("Failed to create handles to control power management features!");
+    }
+  } else {
+    Serial.println("Powermanagement lock handles already created, skipping creation!");
+  }
+  // Configure Task Watchdog so it does not interfere
+  esp_task_wdt_config_t twdt_config = {
+    .timeout_ms = 30000,
+    .idle_core_mask = (1 << portNUM_PROCESSORS) - 1,
+    .trigger_panic = true
+  };
+  if (esp_task_wdt_reconfigure(&twdt_config) != ESP_OK) {
+    Serial.println("Failed to reconfigure task watchdog!");
+  }
+  delay(500);
+  Serial.flush();
+  Serial.end();
+}
+
+/*
+ * A function that prints to serial, setting up the serial peripheral beforehand, and shutting it down afterwards.
+ * Do not use this on a regular basis, as it is probably very slow. It is useful for not having the serial peripheral
+ * active at all times.
+ *
+ * @param str The text to print
+ */
+void setupAndCleanupSerialPrint(char *str) {
+  Serial.begin(115200);
+  while (!Serial) {
+    ;
+    ;
+  }
+  delay(10);
+  Serial.print(str);
+  delay(10);
+  Serial.flush();
+  delay(10);
+  Serial.end();
+}
+
+void loop() {
+  // Alternate between 20 Seconds at 100% CPU load, no artificial load but not sleeping, and normal behaviour (light sleep when there is nothing to do).
+  setupAndCleanupSerialPrint("Beginning stress phase\n");
+  TaskHandle_t core0StressTask = NULL;
+  TaskHandle_t core1StressTask = NULL;
+  xTaskCreatePinnedToCore(stress_task0, "CPU0Stress", 4096, NULL, 1, &core0StressTask, 0);
+  xTaskCreatePinnedToCore(stress_task1, "CPU1Stress", 4096, NULL, 1, &core1StressTask, 1);
+  vTaskDelay(pdMS_TO_TICKS(cycleTime));
+  setupAndCleanupSerialPrint("Still alive after waiting cycleTime after setting up stress tasks...\n");
+  esp_task_wdt_delete(core0StressTask);
+  esp_task_wdt_delete(core1StressTask);
+  vTaskDelete(core0StressTask);
+  vTaskDelete(core1StressTask);
+  // Now disable light sleep
+  setupAndCleanupSerialPrint("Beginning idle with power management disabled\n");
+  esp_pm_lock_acquire(cpuFreqLock);
+  esp_pm_lock_acquire(apbFreqLock);
+  esp_pm_lock_acquire(lightSleepLock);
+  vTaskDelay(pdMS_TO_TICKS(cycleTime));
+  // Restore auto light sleep and dynamic frequency scaling
+  setupAndCleanupSerialPrint("Beginning idle with power management reenabled\n");
+  esp_pm_lock_release(cpuFreqLock);
+  esp_pm_lock_release(apbFreqLock);
+  esp_pm_lock_release(lightSleepLock);
+  vTaskDelay(pdMS_TO_TICKS(cycleTime));
+}

example/advanced/Power_Measurements/IMU/IMU.ino

@@ -0,0 +1,43 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+// How many times to run a command on the IMU consecutively;
+const uint16_t iterations = 5000;
+
+void setup() {
+  dezibot.motion.detection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  // Test if IMU is working correctly
+  char imu_whoami = dezibot.motion.detection.getWhoAmI();
+  if (imu_whoami == 0x67) {
+    Serial.println("IMU seems to be working. Starting measurements...");
+  } else {
+    Serial.println("IMU does not seemm to be working correctly.");
+    exit(0);
+  }
+  dezibot.motion.detection.end();
+  Serial.println("Returned IMU to low-power mode. Killing Serial peripheral. Goodbye!");
+  delay(1000);
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // This puts both accelerometer and gyroscope into low noise mode, which is their highest
+  // power consumption state
+  dezibot.motion.detection.begin();
+  for (uint16_t iter = 0; iter < iterations; iter++) {
+    dezibot.motion.detection.getRotation();
+    dezibot.motion.detection.getAcceleration();
+    delay(1);
+  }
+  // Turn everything back off to measure baseline power consumption
+  delay(iterations);
+}

example/advanced/Power_Measurements/IR_LED/IR_LED.ino

@@ -0,0 +1,23 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+const uint16_t cycleMillis = 2e4;
+
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.infraredLight.begin();
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  // Just turn the lights on alternating each five seconds, to get the consumption
+  dezibot.infraredLight.front.turnOn();
+  delay(cycleMillis);
+  dezibot.infraredLight.front.turnOff();
+  dezibot.infraredLight.bottom.turnOn();
+  delay(cycleMillis);
+  //Turn off and wait a little to measure baseline consumption
+  dezibot.infraredLight.bottom.turnOff();
+  delay(cycleMillis);
+}

example/advanced/Power_Measurements/LightSensorsDaylight/LightSensorsDaylight.ino

@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if SFH325 (Q3) is working correctly
+  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (light_value != UINT16_MAX) {
+    Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
+  } else {
+    Serial.printf("Light detection does not seem to be working correctly (detected value: %d).\n",light_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(DL_FRONT);
+  delay(10);
+}

example/advanced/Power_Measurements/LightSensorsDaylightBottom/LightSensorsDaylightBottom.ino

@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if SFH320 (Q12) is working correctly
+  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (light_value != UINT16_MAX) {
+    Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
+  } else {
+    Serial.printf("Light detection does not seem to be working correctly (detected value: %d).\n",light_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(DL_BOTTOM);
+  delay(10);
+}

example/advanced/Power_Measurements/LightSensorsInfrared/LightSensorsInfrared.ino

@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if VEML6040 is working correctly
+  char ir_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (ir_value != UINT16_MAX) {
+    Serial.printf("IR detection seems to be working (detected value: %d). Starting measurements...\r\n", ir_value);
+  } else {
+    Serial.printf("IR detection does not seem to be working correctly (detected value: %d).\n",ir_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(IR_FRONT);
+  delay(10);
+}

example/advanced/Power_Measurements/Motors/Motors.ino

@@ -0,0 +1,22 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.motion.begin();
+}
+
+void loop() {
+  // Alternatingly turn on the Motors. Use the commands provided by motion for this, using the provided
+  // functions (with their duty cycle), as it represents typical usage
+  dezibot.motion.rotateClockwise();
+  delay(20000);
+  dezibot.motion.rotateAntiClockwise();
+  delay(20000);
+  // Turn on both motors at the same time
+  dezibot.motion.left.setSpeed(DEFAULT_BASE_VALUE);
+  dezibot.motion.right.setSpeed(DEFAULT_BASE_VALUE);  
+  delay(20000);
+  dezibot.motion.stop();
+  delay(20000);
+}

example/advanced/Power_Measurements/Motors_Full/Motors_Full.ino

@@ -0,0 +1,24 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+const uint duty = 8192; //Full tilt
+
+void setup() {
+  dezibot.motion.begin();
+}
+
+void loop() {
+  // Alternatingly turn on the Motors. Use the commands provided by motion for this, using the provided
+  // functions (with their duty cycle), as it represents typical usage
+  dezibot.motion.rotateClockwise(0,duty);
+  delay(20000);
+  dezibot.motion.rotateAntiClockwise(0,duty);
+  delay(20000);
+  // Turn on both motors at the same time
+  dezibot.motion.left.setSpeed(duty);
+  dezibot.motion.right.setSpeed(duty);  
+  delay(20000);
+  dezibot.motion.stop();
+  delay(20000);
+}

example/advanced/Power_Measurements/RGB_LED/RGB_LED.ino

@@ -0,0 +1,44 @@
+#include "Dezibot.h"
+
+Dezibot dezibot = Dezibot();
+
+void setup() {
+  dezibot.multiColorLight.begin();
+}
+
+void loop() {
+  /* Loop through the LEDs at full brightness for measurements, with 20 seconds to measure for each state */
+  for (int state = 0; state < 6; state++) {
+    switch (state) {
+      case 0:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        break;
+      case 1:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(TOP_LEFT,WHITE);
+        break;
+      case 2:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(TOP_RIGHT,WHITE);
+        break;
+      case 3:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(BOTTOM,WHITE);
+        break;
+      case 4:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(TOP,WHITE);
+        break;
+      case 5:
+        dezibot.multiColorLight.turnOffLed(ALL);
+        delay(10);
+        dezibot.multiColorLight.setLed(ALL,WHITE);
+        break;
+    }
+    sleep(20);
+  }
+}

example/advanced/Power_Measurements/Template/Template.ino

@@ -0,0 +1,32 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if Part under Test is working correctly
+  if (result = working) {
+    Serial.printf("Part X seems to be working (detected value: %d). Starting measurements...\r\n", result);
+  } else {
+    Serial.printf("Part X does not seem to be working correctly (detected value: %d).\n",result);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  delay(10);
+}

example/advanced/Power_Measurements/VEML6040/VEML6040.ino

@@ -0,0 +1,34 @@
+#include "Dezibot.h"
+
+Dezibot dezibot;
+
+void setup() {
+  dezibot.lightDetection.begin();
+  //dezibot.motion.detection.end();
+  // put your setup code here, to run once:
+  Serial.begin(115200);
+  // Wait for Serial to init
+  while (!Serial) {
+    ;;
+  }
+  delay(1000);
+  // Test if VEML6040 is working correctly
+  char light_value = dezibot.lightDetection.getValue(DL_FRONT);
+  if (light_value != UINT16_MAX) {
+    Serial.printf("Light detection seems to be working (detected value: %d). Starting measurements...\r\n", light_value);
+  } else {
+    Serial.printf("Light detection does not seem to be working correctly (detected value: %d).\n",light_value);
+    exit(0);
+  }
+  Serial.println("Killing Serial peripheral now to not influence anything. Goodbye!");
+  delay(1000);
+  Serial.flush();
+  Serial.end();
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  dezibot.lightDetection.getValue(DL_FRONT);
+  delay(10);
+}

example/advanced/Power_Measurements/WLAN/WLAN.ino

@@ -0,0 +1,42 @@
+#include "Dezibot.h"
+#define DEBUG
+
+Dezibot dezibot = Dezibot();
+
+const uint16_t cycleTime = 5e3;  //5000 ms = 5 s
+
+void setup() {
+  #ifdef DEBUG
+  Serial.begin(112500);
+  while (!Serial) {
+    ; /* Wait for USB-CDC Serial init to complete. */
+  }
+  dezibot.display.begin();
+  dezibot.display.println("Debug enabled.");
+  Serial.println("Debug enabled.");
+  #endif
+  dezibot.communication.begin();
+  dezibot.communication.setGroupNumber(1);
+  dezibot.communication.sendMessage("Repeated send power consumption test commencing");
+  #ifdef DEBUG
+  dezibot.display.println("Mesh set up");
+  /* Set up receive handler */
+  dezibot.communication.onReceive(handle_receive);
+  dezibot.display.println("Set up receive. Printing incoming messages:");
+  Serial.println("Sending broadcast messages to generate TX power consumption:");
+  #endif
+}
+
+void handle_receive(String &message) {
+  dezibot.display.println(message);
+}
+
+void loop() {
+    /* Continuously send to consume power on TX */
+    for(int i=0; i<cycleTime; i++) {
+      dezibot.communication.sendMessage("Power Test Message");
+      delay(1);
+    }
+    delay(cycleTime);
+
+}

src/Dezibot.cpp

@@ -5,10 +5,10 @@
 #include "Dezibot.h"
 #include <Wire.h>
 
-
 Dezibot::Dezibot() : multiColorLight() {};
 
-void Dezibot::begin(void) {
+void Dezibot::begin(void)
+{
     Wire.begin(SDA_PIN, SCL_PIN);
     infraredLight.begin();
     lightDetection.begin();
@@ -17,6 +17,10 @@ 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,6 +19,7 @@
 #include "infraredLight/InfraredLight.h"
 #include "communication/Communication.h"
 #include "display/Display.h"
+#include "power/Power.h"
 
 
 class Dezibot {
@@ -33,6 +34,7 @@ public:
     InfraredLight infraredLight;
     Communication communication;
     Display display;
+    Power power;
     void begin(void);
 };
 

src/motion/Motion.h

@@ -17,17 +17,17 @@
 #include <freertos/task.h>
 #include "driver/ledc.h"
 #include "motionDetection/MotionDetection.h"
+#include "power/Power.h"
 #define LEDC_MODE          LEDC_LOW_SPEED_MODE
 #define TIMER              LEDC_TIMER_2
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
 #define CHANNEL_RIGHT      LEDC_CHANNEL_4  
 #define DUTY_RES           LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
-#define PHASE_180_DEG      (1 << (DUTY_RES))/2 // (2**DUTY_RES)/2 Set phase to 180 degrees 
 #define FREQUENCY          (5000) // Frequency in Hertz. Set frequency at 5 kHz
 #define DEFAULT_BASE_VALUE  3900
 class Motor{
     public:
-        Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase=0);
+        Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
         
         /**
          * @brief Initializes the motor 
@@ -53,19 +53,14 @@ class Motor{
         uint8_t pin;
         ledc_timer_t timer;
         ledc_channel_t channel;
-
+        Power powerManager;
         uint16_t duty;
-        // The phase of the pwm signal, expressed as a number betweeen 0 and
-        // the maximum value representable by the pwm timer resolution.
-        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);
@@ -81,8 +76,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,LEFT_MOTOR_PHASE);
+    static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT);
-    static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT,RIGHT_MOTOR_PHASE);
+    static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT);
     
     //MotionDetection instance, for motion Correction and user (access with dezibot.motion.detection)
     static inline MotionDetection detection;

src/motion/Motor.cpp

@@ -1,11 +1,11 @@
 #include "Motion.h"
 
-Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel, int phase){
+Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
     this->pin = pin;
     this->channel = channel;
     this->timer = timer;
     this->duty = 0;
-    this->phase = phase;
+    this->powerManager = *Power::getPowerManager();
 };
 
 void Motor::begin(void){
@@ -17,14 +17,20 @@ void Motor::begin(void){
         .intr_type      = LEDC_INTR_DISABLE,
         .timer_sel      = this->timer,
         .duty           = 0, // Set duty to 0%
-        .hpoint         = this->phase
+        .hpoint         = 0
     };
     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");
+    } else {
+        powerManager.releasePower(CONSUMPTION_MOTOR);
+        Serial.println("Motor released power");
+    }
     int difference = duty-this->getSpeed();
     if (difference > 0){
         for(int i = 0;i<difference;i+=difference/20){

src/motionDetection/IMU_CMDs.h

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

src/power/Consumptions.h

@@ -0,0 +1,34 @@
+/**
+ * @file Consumptions.h
+ * @author Phillip Kühne
+ * @brief 
+ * @version 0.1
+ * @date 2024-11-28
+ * 
+ * @copyright (c) 2024
+ * 
+ */
+
+#ifndef Consumptions_h
+#define Consumptions_h
+
+// The power budget afforded by the LIR2450 button cell, 700mW
+#define POWER_BUDGET 700 
+
+// The power consumptions of the different components are defined here.
+// These are "typical worst case" values for now.
+#define CONSUMPTION_ESP_BASE 96
+#define CONSUMPTION_ESP_LOAD 100
+#define CONSUMPTION_RGB_LED 56
+#define CONSUMPTION_IR_BOTTOM 327
+#define CONSUMPTION_IR_FRONT 492
+#define CONSUMPTION_OLED 92 // Assumes lots of lit pixels
+#define CONSUMPTION_MOTOR 323
+// These are placeholders for completeness for now
+#define CONSUMPTION_RGBW_SENSOR 1
+#define CONSUMPTION_IR_PT 1
+#define CONSUMPTION_UV_LED 200
+#define CONSUMPTION_IMU 1
+
+
+#endif //Consumptions_h

src/power/Power.cpp

@@ -0,0 +1,122 @@
+/**
+ * @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

@@ -0,0 +1,60 @@
+/**
+ * @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