phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-05-19 11:01:46 +02:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'power_management' into 'main'

Browse Source 
Introduce Power Management

See merge request phillip.kuehne/dezibot!1
This commit is contained in:
Phillip Kühne 2025-02-24 22:32:17 +00:00
commit 665376886c
33 changed files with 2691 additions and 432 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -45,4 +45,4 @@ docs/*
compile_commands.json
# Build directories
build/
build/

29
example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino Normal file
View File

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

56
example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino Normal file
View File

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

49
example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino Normal file
View File

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

44
example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino Normal file
View File

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

50
example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino Normal file
View File

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

116
example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino Normal file
View File

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

57
example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino Normal file
View File

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

14
src/Dezibot.cpp
View File

@ -5,18 +5,18 @@
#include "Dezibot.h"
#include <Wire.h>
Dezibot::Dezibot() : multiColorLight() {};
Dezibot::Dezibot():multiColorLight(){};
void Dezibot::begin(void) {
Wire.begin(SDA_PIN,SCL_PIN);
void Dezibot::begin(void)
{
ESP_LOGI("Dezibot", "Initializing Dezibot");
power.begin();
Wire.begin(SDA_PIN, SCL_PIN);
infraredLight.begin();
lightDetection.begin();
lightDetection.begin();
motion.begin();
lightDetection.begin();
colorDetection.begin();
multiColorLight.begin();
display.begin();
};

2
src/Dezibot.h
View File

@ -19,6 +19,7 @@
#include "infraredLight/InfraredLight.h"
#include "communication/Communication.h"
#include "display/Display.h"
#include "power/PowerManager.h"
class Dezibot {
@ -33,6 +34,7 @@ public:
InfraredLight infraredLight;
Communication communication;
Display display;
PowerManager power;
void begin(void);
};

170
src/colorDetection/ColorDetection.cpp
View File

@ -1,83 +1,103 @@
#include "ColorDetection.h"
#include "ColorDetection.h"
void ColorDetection::begin(void){
ColorDetection::configure(VEML_CONFIG{.mode = AUTO,.enabled = true,.exposureTime=MS40});
void ColorDetection::begin(void) {
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)
{
case MS40:
configRegister = 0x00;break;
case MS80:
configRegister = 0x01;break;
case MS160:
configRegister = 0x02;break;
case MS320:
configRegister = 0x03;break;
case MS640:
configRegister = 0x04;break;
case MS1280:
configRegister = 0x05;break;
}
configRegister = configRegister << 4;
if(config.mode == MANUAL)
{
configRegister = configRegister | (0x01<<1);
}
if(!config.enabled)
{
configRegister = configRegister | 1;
}
ColorDetection::writeDoubleRegister(CMD_CONFIG,(uint16_t)configRegister);
void ColorDetection::configure(VEML_CONFIG config) {
uint8_t configRegister = 0;
switch (config.exposureTime) {
case MS40:
configRegister = 0x00;
break;
case MS80:
configRegister = 0x01;
break;
case MS160:
configRegister = 0x02;
break;
case MS320:
configRegister = 0x03;
break;
case MS640:
configRegister = 0x04;
break;
case MS1280:
configRegister = 0x05;
break;
}
configRegister = configRegister << 4;
if (config.mode == MANUAL) {
configRegister = configRegister | (0x01 << 1);
}
if (!config.enabled) {
configRegister = configRegister | 1;
}
ColorDetection::writeDoubleRegister(CMD_CONFIG, (uint16_t)configRegister);
};
uint16_t ColorDetection::getColorValue(color color){
switch(color)
{
case VEML_RED:
return readDoubleRegister(REG_RED);
break;
case VEML_GREEN:
return readDoubleRegister(REG_GREEN);
break;
case VEML_BLUE:
return readDoubleRegister(REG_BLUE);
break;
case VEML_WHITE:
return readDoubleRegister(REG_WHITE);
break;
default:
Serial.println("Color is not supported by the sensor");
return 0;
}
uint16_t ColorDetection::getColorValue(color color) {
switch (color) {
case VEML_RED:
return readDoubleRegister(REG_RED);
break;
case VEML_GREEN:
return readDoubleRegister(REG_GREEN);
break;
case VEML_BLUE:
return readDoubleRegister(REG_BLUE);
break;
case VEML_WHITE:
return readDoubleRegister(REG_WHITE);
break;
default:
Serial.println("Color is not supported by the sensor");
return 0;
}
};
uint16_t ColorDetection::readDoubleRegister(uint8_t regAddr){
uint16_t result = 0;
Wire.beginTransmission(VEML_ADDR);
Wire.write(regAddr);
if(Wire.endTransmission() != 0){
Serial.printf("Reading Register %d failed",regAddr);
}
Wire.requestFrom(VEML_ADDR,2);
uint8_t offset = 0;
while(Wire.available()){
result = result << 8;
result = result | (Wire.read()<<offset);
offset = offset + 8;
}
return result;
uint16_t ColorDetection::readDoubleRegister(uint8_t regAddr) {
uint16_t result = 0;
Wire.beginTransmission(VEML_ADDR);
Wire.write(regAddr);
if (Wire.endTransmission() != 0) {
Serial.printf("Reading Register %d failed", regAddr);
}
Wire.requestFrom(VEML_ADDR, 2);
uint8_t offset = 0;
while (Wire.available()) {
result = result << 8;
result = result | (Wire.read() << offset);
offset = offset + 8;
}
return result;
};
void ColorDetection::writeDoubleRegister(uint8_t regAddr, uint16_t data){
//erst low dann high
Wire.beginTransmission(VEML_ADDR);
Wire.write(regAddr);
Wire.write((uint8_t)(data&0x00FF));
Wire.write((uint8_t)((data>>8)&0x00FF));
if(Wire.endTransmission() != 0){
Serial.printf("Reading Register %d failed",regAddr);
}
};
void ColorDetection::writeDoubleRegister(uint8_t regAddr, uint16_t data) {
// erst low dann high
Wire.beginTransmission(VEML_ADDR);
Wire.write(regAddr);
Wire.write((uint8_t)(data & 0x00FF));
Wire.write((uint8_t)((data >> 8) & 0x00FF));
if (Wire.endTransmission() != 0) {
Serial.printf("Reading Register %d failed", regAddr);
}
};
float ColorDetection::modelCurrentConsumption() {
return PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW;
};
float ColorDetection::modelChargeConsumption(uint16_t durationMs) {
return (PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW *
durationMs) / 10e6f;
}

31
src/colorDetection/ColorDetection.h
View File

@ -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,7 +65,28 @@ public:
void begin(void);
void configure(VEML_CONFIG config);
uint16_t getColorValue(color color);
protected:
/**
* @brief Current consumtion of the sensor
* @note May not be accurate, as it is not known if the consumption is
* constant, or only if sensor is active. Could not be measured.
*
* @return
*/
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);
};

123
src/communication/Communication.cpp
View File

@ -7,81 +7,94 @@ uint32_t Communication::groupNumber = 0;
// User-defined callback function pointer
void (*Communication::userCallback)(String &msg) = nullptr;
void Communication::sendMessage(String msg)
{
String data = String(groupNumber) + "#" + msg;
mesh.sendBroadcast(data);
void Communication::sendMessage(String msg) {
String data = String(groupNumber) + "#" + msg;
mesh.sendBroadcast(data);
}
// Needed for painless library
void Communication::receivedCallback(uint32_t from, String &msg)
{
int separatorIndex = msg.indexOf('#');
if (separatorIndex != -1) {
String groupNumberStr = msg.substring(0, separatorIndex);
uint32_t num = groupNumberStr.toInt();
String restOfMsg = msg.substring(separatorIndex + 1);
void Communication::receivedCallback(uint32_t from, String &msg) {
int separatorIndex = msg.indexOf('#');
if (separatorIndex != -1) {
String groupNumberStr = msg.substring(0, separatorIndex);
uint32_t num = groupNumberStr.toInt();
String restOfMsg = msg.substring(separatorIndex + 1);
Serial.printf("startHere: Received from %u groupNumber=%u msg=%s\n", from, num, restOfMsg.c_str());
Serial.printf("startHere: Received from %u groupNumber=%u msg=%s\n", from,
num, restOfMsg.c_str());
if (groupNumber != num) return;
if (groupNumber != num)
return;
// Execute user-defined callback if it is set
if (userCallback) {
userCallback(restOfMsg);
}
// Execute user-defined callback if it is set
if (userCallback) {
userCallback(restOfMsg);
}
}
}
void newConnectionCallback(uint32_t nodeId)
{
Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId);
void newConnectionCallback(uint32_t nodeId) {
Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId);
}
void changedConnectionCallback()
{
Serial.printf("Changed connections\n");
void changedConnectionCallback() { Serial.printf("Changed connections\n"); }
void nodeTimeAdjustedCallback(int32_t offset) {
Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(), offset);
}
void nodeTimeAdjustedCallback(int32_t offset)
{
Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(), offset);
}
void vTaskUpdate(void *pvParameters)
{
for (;;)
{
mesh.update();
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))
{
userCallback = callbackFunc;
void Communication::onReceive(void (*callbackFunc)(String &msg)) {
userCallback = callbackFunc;
}
void Communication::begin(void)
{
Serial.begin(115200);
void Communication::begin(void) {
Serial.begin(115200);
// mesh.setDebugMsgTypes( ERROR | MESH_STATUS | CONNECTION | SYNC | COMMUNICATION | GENERAL | MSG_TYPES | REMOTE ); // all types on
mesh.setDebugMsgTypes(ERROR | STARTUP); // set before init() so that you can see startup messages
// mesh.setDebugMsgTypes( ERROR | MESH_STATUS | CONNECTION | SYNC |
// COMMUNICATION | GENERAL | MSG_TYPES | REMOTE ); // all types on
mesh.setDebugMsgTypes(
ERROR |
STARTUP); // set before init() so that you can see startup messages
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);
mesh.onChangedConnections(&changedConnectionCallback);
mesh.onNodeTimeAdjusted(&nodeTimeAdjustedCallback);
mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
mesh.onReceive(&receivedCallback);
mesh.onNewConnection(&newConnectionCallback);
mesh.onChangedConnections(&changedConnectionCallback);
mesh.onNodeTimeAdjusted(&nodeTimeAdjustedCallback);
static uint8_t ucParameterToPass;
TaskHandle_t xHandle = NULL;
static uint8_t ucParameterToPass;
TaskHandle_t xHandle = NULL;
xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass, tskIDLE_PRIORITY, &xHandle);
configASSERT(xHandle);
};
xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass,
tskIDLE_PRIORITY, &xHandle);
configASSERT(xHandle);
};

22
src/communication/Communication.h
View File

@ -1,29 +1,33 @@
#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:
public:
/**
* @brief initialize the Mesh Compnent, must be called before the other methods are used.
*
*/
* @brief initialize the Mesh Compnent, must be called before the other methods are used.
*
*/
static void begin(void);
void setGroupNumber(uint32_t number);
void sendMessage(String msg);
void onReceive(void (*callbackFunc)(String &msg));
private:
private:
static void (*userCallback)(String &msg);
static void receivedCallback(uint32_t from, String &msg);
static uint32_t groupNumber;

26
src/display/Display.cpp
View File

@ -4,7 +4,7 @@
* @brief Adds the ability to print to the display of the robot.
* @version 0.1
* @date 2024-06-05
*
*
* @copyright Copyright (c) 2024
*/
@ -14,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);
@ -23,7 +31,7 @@ void Display::begin(void){
sendDisplayCMD(stopCompleteOn);
/*which pixels are bright: normal = 1s are bright, inverese= 0s are bright*/
sendDisplayCMD( setNormalMode);
sendDisplayCMD( setOscFreq);
sendDisplayCMD(0x80);
@ -68,7 +76,7 @@ void Display::updateLine(uint charAmount)
if(charAmount+this->charsOnCurrLine>16)
{
this->currLine = (this->currLine+((charAmount+this->charsOnCurrLine)/16))%8;
this->charsOnCurrLine = (charAmount+this->charsOnCurrLine)%17; //there can be 0-16 chars on one line, so the 17th char is on next line
this->charsOnCurrLine = (charAmount+this->charsOnCurrLine)%17; //there can be 0-16 chars on one line, so the 17th char is on next line
}
else
{
@ -79,7 +87,7 @@ void Display::updateLine(uint charAmount)
void Display::print(char *value){
char *nextchar;
/* write data to the buffer */
while(value && *value != '\0') //check if pointer is still valid and string is not terminated
while(value && *value != '\0') //check if pointer is still valid and string is not terminated
{
//check if next character is a linebreak
if(*value=='\n')
@ -108,7 +116,7 @@ void Display::print(char *value){
}
Wire.endTransmission();
}
value++;
value++;
}
};
@ -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;
};

23
src/display/Display.h
View File

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

78
src/infraredLight/InfraredLED.cpp
View File

@ -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;
@ -9,10 +13,10 @@ InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel)
};
void InfraredLED::begin(void){
//we want to change frequency instead of
//we want to change frequency instead of
pwmTimer = ledc_timer_config_t{
.speed_mode = pwmSpeedMode,
.duty_resolution = LEDC_TIMER_10_BIT,
.duty_resolution = DUTY_RESOLUTION,
.timer_num = this->timer,
.freq_hz = 800,
.clk_cfg = LEDC_AUTO_CLK
@ -42,16 +46,80 @@ 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);
};
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;
}

35
src/infraredLight/InfraredLight.h
View File

@ -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,7 +48,32 @@ class InfraredLED{
* @param frequency
*/
void sendFrequency(uint16_t frequency);
protected:
/**
* @brief Estimate the current consumption of setting the specified led to the
* passed duty cycle
* @param duty the duty cycle of the led
* @return consumed current in milliamperes
*/
float modelCurrentConsumption(uint32_t duty);
/**
* @brief Estimate the energy consumption of turning the infrared led on
* @param durationMs time the led will be on
* @return consumed energy in coloumbs
*/
float modelChargeConsumptionOn(uint16_t durationMs);
/**
* @brief Estimate the energy consumption of sending a frequency on the
* infrared led
* @param durationMs time the led will be on
* @param frequency the frequency the led will be flashing
* @return consumed energy in coloumbs
*/
float modelChargeConsumptionSendFrequency(uint16_t durationMs);
protected:
uint8_t ledPin;
ledc_timer_t timer;
ledc_channel_t channel;

45
src/lightDetection/LightDetection.cpp
View File

@ -27,7 +27,7 @@ photoTransistors LightDetection::getBrightest(ptType type){
photoTransistors maxSensor;
uint16_t maxReading = 0;
uint16_t currentReading = 0;
if (type == IR){
maxSensor = IR_FRONT;
for(const auto pt : allIRPTs){
@ -47,15 +47,15 @@ photoTransistors LightDetection::getBrightest(ptType type){
}
}
}
return maxSensor;
};
uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measurments, uint32_t timeBetween){
TickType_t xLastWakeTime = xTaskGetTickCount();
TickType_t frequency = timeBetween / portTICK_PERIOD_MS;
uint64_t cumulatedResult = 0;
uint64_t cumulatedResult = 0;
for(int i = 0; i < measurments; i++){
cumulatedResult += LightDetection::getValue(sensor);
xTaskDelayUntil(&xLastWakeTime,frequency);
@ -64,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;
}

24
src/lightDetection/LightDetection.h
View File

@ -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,7 +78,24 @@ public:
* @return the average of all taken meaurments
*/
static uint32_t getAverageValue(photoTransistors sensor, uint32_t measurments, uint32_t timeBetween);
protected:
/**
* @brief Get current consumption of the selected PTs
*
* @return float the current consumption of the PTs
*/
static float modelCurrentConsumption(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;
static const uint8_t IR_PT_RIGHT_ADC = 5;

35
src/motion/Motion.cpp
View File

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

41
src/motion/Motion.h
View File

@ -17,16 +17,23 @@
#include <freertos/task.h>
#include "driver/ledc.h"
#include "motionDetection/MotionDetection.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
@ -39,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
@ -48,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;
uint16_t duty;
// The phase of the pwm signal, expressed as a number betweeen 0 and
// the maximum value representable by the pwm timer resolution.
int phase;
};
class Motion{
protected:
static inline uint16_t RIGHT_MOTOR_DUTY = DEFAULT_BASE_VALUE;
static inline uint16_t LEFT_MOTOR_DUTY = DEFAULT_BASE_VALUE;
static inline int LEFT_MOTOR_PHASE = 0;
static inline int RIGHT_MOTOR_PHASE = PHASE_180_DEG;
static const int MOTOR_RIGHT_PIN = 11;
static const int MOTOR_LEFT_PIN = 12;
static void moveTask(void * args);
@ -75,8 +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;

79
src/motion/Motor.cpp
View File

@ -1,10 +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->phase = phase;
};
void Motor::begin(void){
@ -16,33 +21,61 @@ 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){
int difference = duty-this->getSpeed();
if (difference > 0){
for(int i = 0;i<difference;i+=difference/20){
this->duty += difference/20;
ledc_set_duty(LEDC_MODE,this->channel,duty);
ledc_update_duty(LEDC_MODE,this->channel);
delayMicroseconds(5);
}
} else {
for(int i = 0;i>difference;i-=abs(difference/20)){
this->duty -= abs(difference/20);
ledc_set_duty(LEDC_MODE,this->channel,duty);
ledc_update_duty(LEDC_MODE,this->channel);
delayMicroseconds(5);
}
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 {
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) {
for (int i = 0; i < difference; i += difference / 20) {
this->duty += difference / 20;
ledc_set_duty(LEDC_MODE, this->channel, duty);
ledc_update_duty(LEDC_MODE, this->channel);
delayMicroseconds(5);
}
} else {
for (int i = 0; i > difference; i -= abs(difference / 20)) {
this->duty -= abs(difference / 20);
ledc_set_duty(LEDC_MODE, this->channel, duty);
ledc_update_duty(LEDC_MODE, this->channel);
delayMicroseconds(5);
}
}
return true;
};
uint16_t Motor::getSpeed(void){
return this->duty;
};
uint16_t Motor::getSpeed(void) { return this->duty; };
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;
}

66
src/motionDetection/IMU_CMDs.h
View File

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

61
src/motionDetection/MotionDetection.cpp
View File

@ -6,12 +6,21 @@ 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);
// set Accel and Gyroscop to Low Noise
this->writeRegister(PWR_MGMT0,0x1F);
//busy Wait for startup
//busy Wait for startup
delayMicroseconds(250);
//set accelconfig
this->writeRegister(0x21,0x05);
@ -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;
@ -44,12 +55,12 @@ IMUResult MotionDetection::getRotation(){
};
float MotionDetection::getTemperature(){
int16_t raw_temperatur = readRegister(REG_TEMP_HIGH)<<8;
raw_temperatur |= readRegister(REG_TEMP_LOW);
raw_temperatur |= readRegister(REG_TEMP_LOW);
return raw_temperatur/128 +25;
};
int8_t MotionDetection::getWhoAmI(){
return readRegister(WHO_AM_I);
return readRegister(WHO_AM_I);
};
bool MotionDetection::isShaken(uint32_t threshold ,uint8_t axis){
@ -113,7 +124,7 @@ Orientation MotionDetection::getTilt(){
}
//yAngle = -1*yAngle-90;
}
return Orientation{xAngle,yAngle};
@ -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;
}
@ -166,7 +177,7 @@ uint8_t MotionDetection::readRegister(uint8_t reg){
result = handler->transfer(cmdRead(reg));
result = handler->transfer(0x00);
digitalWrite(34,HIGH);
handler->endTransaction();
handler->endTransaction();
return result;
};
@ -198,7 +209,7 @@ void MotionDetection::writeToRegisterBank(registerBank bank, uint8_t reg, uint8_
}
uint8_t result = this->readRegister(PWR_MGMT0);
Serial.print("MADDR_W: ");
Serial.println(readRegister(MADDR_W));
Serial.println(readRegister(MADDR_W));
//set Idle Bit
this->writeRegister(PWR_MGMT0,result|0x10);
switch(bank){
@ -245,33 +256,33 @@ void MotionDetection::initFIFO(){
uint MotionDetection::getDataFromFIFO(FIFO_Package* buffer){
int16_t fifocount = 0;
int8_t fifohigh = this->readRegister(FIFO_COUNTH);
int8_t fifolow = this->readRegister(FIFO_COUNTL);
int8_t fifolow = this->readRegister(FIFO_COUNTL);
fifocount = (fifohigh<<8)|fifolow;
//fifocount |= this->readRegister(FIFO_COUNTL);
//fifocount = (this->readRegister(FIFO_COUNTH)<<8);
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));
handler->transfer(buf,16*fifocount);
digitalWrite(34,HIGH);
handler->endTransaction();
writeRegister(0x02,0x04);
delayMicroseconds(10);
for(int i = 0; i<fifocount;i++){
buffer[i].header = buf[0x00+16*i];
buffer[i].accel.x = (buf[0x02+16*i]<<8)|buf[0x01+16*i];
buffer[i].accel.y = (buf[0x04+16*i]<<8)|buf[0x03+16*i];
buffer[i].accel.z = (buf[0x06+16*i]<<8)|buf[0x05+16*i];
buffer[i].gyro.x = (buf[0x08+16*i]<<8)|buf[0x07+16*i];
buffer[i].gyro.y = (buf[0x0A+16*i]<<8)|buf[0x09+16*i];
buffer[i].gyro.z = (buf[0x0C+16*i]<<8)|buf[0x0B+16*i];
buffer[i].temperature = buf[0x0D+16*i];
buffer[i].timestamp = (buf[0x0F+16*i]<<8)|buf[0x0E +16*i];
buffer[i].accel.x = (buf[0x02+16*i]<<8)|buf[0x01+16*i];
buffer[i].accel.y = (buf[0x04+16*i]<<8)|buf[0x03+16*i];
buffer[i].accel.z = (buf[0x06+16*i]<<8)|buf[0x05+16*i];
buffer[i].gyro.x = (buf[0x08+16*i]<<8)|buf[0x07+16*i];
buffer[i].gyro.y = (buf[0x0A+16*i]<<8)|buf[0x09+16*i];
buffer[i].gyro.z = (buf[0x0C+16*i]<<8)|buf[0x0B+16*i];
buffer[i].temperature = buf[0x0D+16*i];
buffer[i].timestamp = (buf[0x0F+16*i]<<8)|buf[0x0E +16*i];
}
return fifocount;
};
@ -284,4 +295,12 @@ void MotionDetection::writeRegister(uint8_t reg, uint8_t value){
digitalWrite(34,HIGH);
delayMicroseconds(10);
handler->endTransaction();
};
};
float MotionDetection::modelCurrentConsumption(){
return PowerParameters::CurrentConsumptions::CURRENT_IMU;
}
float MotionDetection::modelChargeConsumption(uint16_t durationMs) {
return (this->modelCurrentConsumption() * durationMs) / 10e6f;
}

24
src/motionDetection/MotionDetection.h
View File

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

313
src/multiColorLight/MultiColorLight.cpp
View File

@ -1,107 +1,248 @@
#include "MultiColorLight.h"
MultiColorLight::MultiColorLight():rgbLeds(ledAmount,ledPin){
MultiColorLight::MultiColorLight()
: rgbLeds(ledAmount, ledPin) {
};
};
void MultiColorLight::begin(void){
rgbLeds.begin();
this->turnOffLed();
};
void MultiColorLight::setLed(uint8_t index , uint32_t color){
if (index > ledAmount-1){
//TODO: logging
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.setPixelColor(index, normalizeColor(color));
rgbLeds.show();
}
rgbLeds.begin();
this->turnOffLed();
};
void MultiColorLight::setLed(leds leds, uint32_t color){
switch (leds){
case TOP_LEFT:
MultiColorLight::setLed(1,color);break;
case TOP_RIGHT:
MultiColorLight::setLed(0,color);break;
case BOTTOM:
MultiColorLight::setLed(2,color);break;
case TOP:
for (int index = 0; index<2; index++){
MultiColorLight::setLed(index,color);
}break;
case ALL:
for (int index = 0; index<ledAmount; index++){
MultiColorLight::setLed(index,color);
}break;
default:
//TODO logging
break;
void MultiColorLight::setLed(uint8_t index, uint32_t color) {
if (index > ledAmount - 1) {
// TODO: logging
}
uint32_t normalizedColor = normalizeColor(color);
float totalConsumption = modelCurrentConsumption(normalizedColor);
switch (index) {
case 0:
if (!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, uint8_t red, uint8_t green, uint8_t blue){
MultiColorLight::setLed(leds, MultiColorLight::color(red,green,blue));
void MultiColorLight::setLed(leds leds, uint32_t color) {
switch (leds) {
case TOP_LEFT:
MultiColorLight::setLed(1, color);
break;
case TOP_RIGHT:
MultiColorLight::setLed(0, color);
break;
case BOTTOM:
MultiColorLight::setLed(2, color);
break;
case TOP:
for (int index = 0; index < 2; index++) {
MultiColorLight::setLed(index, color);
}
break;
case ALL:
for (int index = 0; index < ledAmount; index++) {
MultiColorLight::setLed(index, color);
}
break;
default:
// TODO logging
break;
}
};
void MultiColorLight::setTopLeds(uint32_t color){
MultiColorLight::setLed(TOP,color);
};
void MultiColorLight::setTopLeds(uint8_t red, uint8_t green, uint8_t blue){
MultiColorLight::setTopLeds(MultiColorLight::color(red,green,blue));
};
void MultiColorLight::blink(uint16_t amount,uint32_t color, leds leds, uint32_t interval){
for(uint16_t index = 0; index < amount;index++){
MultiColorLight::setLed(leds, color);
vTaskDelay(interval);
MultiColorLight::turnOffLed(leds);
vTaskDelay(interval);
}
void MultiColorLight::setLed(leds leds, uint8_t red, uint8_t green,
uint8_t blue) {
MultiColorLight::setLed(leds, MultiColorLight::color(red, green, blue));
};
void MultiColorLight::turnOffLed(leds leds){
switch (leds){
case TOP_LEFT:
MultiColorLight::setLed(1,0);break;
case TOP_RIGHT:
MultiColorLight::setLed(0,0);break;
case BOTTOM:
MultiColorLight::setLed(2,0);break;
case TOP:
for (int index = 0; index<2; index++){
MultiColorLight::setLed(index,0);
}break;
case ALL:
for (int index = 0; index<3; index++){
MultiColorLight::setLed(index,0);
}break;
default:
//TODO logging
break;
}
void MultiColorLight::setTopLeds(uint32_t color) {
MultiColorLight::setLed(TOP, color);
};
uint32_t MultiColorLight::color(uint8_t r, uint8_t g, uint8_t b){
return rgbLeds.Color(r,g,b);
void MultiColorLight::setTopLeds(uint8_t red, uint8_t green, uint8_t blue) {
MultiColorLight::setTopLeds(MultiColorLight::color(red, green, blue));
};
//PRIVATE
uint32_t MultiColorLight::normalizeColor(uint32_t color,uint8_t maxBrightness){
uint8_t red = (color&0x00FF0000)>>16;
uint8_t green = (color&0x0000FF00)>>8;
uint8_t blue = (color&0x000000FF);
if (red > maxBrightness){
red = maxBrightness;
void MultiColorLight::blink(uint16_t amount, uint32_t color, leds leds,
uint32_t interval) {
for (uint16_t index = 0; index < amount; index++) {
MultiColorLight::setLed(leds, color);
vTaskDelay(interval);
MultiColorLight::turnOffLed(leds);
vTaskDelay(interval);
}
};
void MultiColorLight::turnOffLed(leds leds) {
switch (leds) {
case TOP_LEFT:
MultiColorLight::setLed(1, 0);
break;
case TOP_RIGHT:
MultiColorLight::setLed(0, 0);
break;
case BOTTOM:
MultiColorLight::setLed(2, 0);
break;
case TOP:
for (int index = 0; index < 2; index++) {
MultiColorLight::setLed(index, 0);
}
if(green > maxBrightness-70){
green = maxBrightness-70;
break;
case ALL:
for (int index = 0; index < 3; index++) {
MultiColorLight::setLed(index, 0);
}
if(blue > maxBrightness-50){
blue = maxBrightness-50;
break;
default:
// TODO logging
break;
}
};
uint32_t MultiColorLight::color(uint8_t r, uint8_t g, uint8_t b) {
return rgbLeds.Color(r, g, b);
};
// PRIVATE
uint32_t MultiColorLight::normalizeColor(uint32_t color,
uint8_t maxBrightness) {
uint8_t red = (color & 0x00FF0000) >> 16;
uint8_t green = (color & 0x0000FF00) >> 8;
uint8_t blue = (color & 0x000000FF);
if (red > maxBrightness) {
red = maxBrightness;
}
if (green > maxBrightness - 70) {
green = maxBrightness - 70;
}
if (blue > maxBrightness - 50) {
blue = maxBrightness - 50;
}
return MultiColorLight::color(red, green, blue);
};
float MultiColorLight::modelCurrentConsumption(uint32_t color) {
uint32_t normalizedColor = normalizeColor(color);
uint16_t colorComponentRed = (normalizedColor & 0x00FF0000) >> 16;
uint16_t colorComponentGreen = (normalizedColor & 0x0000FF00) >> 8;
uint16_t colorComponentBlue = (normalizedColor & 0x000000FF);
float redChannelConsumption =
(colorComponentRed / 255.0) *
PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
float greenChannelConsumption =
(colorComponentGreen / 255.0) *
PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
float blueChannelConsumption =
(colorComponentBlue / 255.0) *
PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_CHAN_T_ON;
return redChannelConsumption + greenChannelConsumption +
blueChannelConsumption +
PowerParameters::CurrentConsumptions::CURRENT_LED_RGB_BASE;
};
float MultiColorLight::modelCurrentConsumption(uint8_t red, uint8_t green,
uint8_t blue) {
return modelCurrentConsumption(MultiColorLight::color(red, green, blue));
};
float MultiColorLight::modelChargeConsumption(uint8_t index, uint32_t color,
uint16_t durationMs) {
if (index > ledAmount - 1) {
// TODO: logging
}
uint32_t normalizedColor = normalizeColor(color);
float ledConsumption = modelCurrentConsumption(normalizedColor);
return (ledConsumption * durationMs) / 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);
}
return MultiColorLight::color(red,green,blue);
}
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);
};

300
src/multiColorLight/MultiColorLight.h
View File

@ -1,139 +1,209 @@
/**
* @file MultiColorLight.h
* @author Saskia Duebener, Hans Haupt
* @brief This component controls the ability to show multicolored light, using the RGB-LEDs
* @brief This component controls the ability to show multicolored light, using
* the RGB-LEDs
* @version 0.2
* @date 2023-11-25
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef MultiColorLight_h
#define MultiColorLight_h
#include <Adafruit_NeoPixel.h>
#include "../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
*
* @brief Describes combinations of leds on the Dezibot.
* With the Robot in Front of you, when the robot drives away from you, the left
* LED is TOP_LEFT
*
*/
enum leds{
TOP_LEFT,
TOP_RIGHT,
BOTTOM,
TOP,
ALL
};
enum leds { TOP_LEFT, TOP_RIGHT, BOTTOM, TOP, ALL };
class MultiColorLight{
#define TAG "MultiColorLight"
class MultiColorLight {
protected:
static const uint16_t ledAmount = 3;
static const int16_t ledPin = 48;
static const uint8_t maxBrightness = 150;
Adafruit_NeoPixel rgbLeds;
static const uint16_t ledAmount = 3;
static const int16_t ledPin = 48;
static const uint8_t defaultMaxBrightness = 150;
Adafruit_NeoPixel rgbLeds;
static constexpr int maximumExecutionDelayMs = 10;
public:
MultiColorLight();
/**
* @brief initialize the multicolor component
*
*/
void begin(void);
MultiColorLight();
/**
* @brief initialize the multicolor component
*
*/
void begin(void);
/**
* @brief Set the specified led to the passed color
* @param index ranging from 0-2, 0: Right, 1: Left, 2: Bottom
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can range between 0 to 100
*/
void setLed(uint8_t index , uint32_t color);
/**
* @brief Set the specified led to the passed color
* @param index ranging from 0-2, 0: Right, 1: Left, 2: Bottom
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can range
* between 0 to 100
*/
void setLed(uint8_t index, uint32_t color);
/**
* @brief Set the specified leds to the passed color value
*
* @param leds which leds should be updated
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can range between 0 to 100
*/
void setLed(leds leds, uint32_t color);
/**
* @brief Set the specified leds to the passed color value
*
* @param leds which leds should be updated
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can range
* between 0 to 100
*/
void setLed(leds leds, uint32_t color);
/**
* @brief Set the specified leds to the passed color value
*
* @param leds which leds should be updated
* @param red brightness of red, is normalized in the function
* @param green brightness of green, is normalized in the function
* @param blue brightness of blue, is normalized in the function
*/
void setLed(leds leds, uint8_t red, uint8_t green, uint8_t blue);
/**
* @brief sets the two leds on the top of the robot to the specified color
*
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can range between 0 to 100
*/
void setTopLeds(uint32_t color);
/**
* @brief Set the specified leds to the passed color value
*
* @param leds which leds should be updated
* @param red brightness of red, is normalized in the function
* @param green brightness of green, is normalized in the function
* @param blue brightness of blue, is normalized in the function
*/
void setLed(leds leds, uint8_t red, uint8_t green, uint8_t blue);
/**
* @brief sets the two leds on the top of the robot to the specified color
*
* @param red brightness of red, is normalized in the function
* @param green brightness of green, is normalized in the function
* @param blue brightness of blue, is normalized in the function
*/
void setTopLeds(uint8_t red, uint8_t green, uint8_t blue);
/**
* @brief Let LEDs blink, returns after all blinks were executed
*
* @param amount how often should the leds blink
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
* Each color can range between 0 to 100
* Defaults to blue
* @param leds which LEDs should blink, default is TOP
* @param interval how many miliseconds the led is on, defaults to 1s
*/
void blink(uint16_t amount,uint32_t color = 0x00006400,leds leds=TOP, uint32_t interval=1000);
/**
* @brief calculates the current consumption of an LED with the given color
*
* @param color A 32-bit unsigned integer representing the color in the
* format 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
* @return float the current consumption in mA
*/
float modelCurrentConsumption(uint32_t color);
/**
* @brief turn off the given leds
*
* @param leds which leds should be turned off, defaults to ALL
*/
void turnOffLed(leds leds=ALL);
/**
* @brief wrapper to calulate the used colorformat from a rgb-value
*
* @param r red (0-100)
* @param g green (0-100)
* @param b blue (0-100)
* @return A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
*/
uint32_t color(uint8_t r, uint8_t g, uint8_t b);
/**
* @brief calculates the current consumption of an LED with the given color
* @note color is not normalized in this function
*
* @param red brightness of red
* @param green brightness of green
* @param blue brightness of blue
* @return float the current consumption in mA
*/
float modelCurrentConsumption(uint8_t red, uint8_t green, uint8_t blue);
/**
* @brief Estimate the energy consumption of setting the specified led to the
* passed color
* @param index ranging from 0-2, 0: Right, 1: Left, 2: Bottom
* @param color A 32-bit unsigned integer representing the color in the
* format 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can
* range between 0 to 100
* @return consumed energy in coloumbs
*/
float modelChargeConsumption(uint8_t index, uint32_t color,
uint16_t durationMs);
/**
* @brief Estimate the energy consumption of setting the specified leds to the
* passed color value
*
* @param leds which leds should be considered
* @param color A 32-bit unsigned integer representing the color in the
* format 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can
* range between 0 to 100
* @return consumed energy in coloumbs
*/
float modelChargeConsumption(leds leds, uint32_t color,
uint16_t durationMs);
/**
* @brief Estimate the energy consumption of setting the specified leds to the
* passed color value
*
* @param leds which leds should be considered
* @param red brightness of red, is normalized in the function
* @param green brightness of green, is normalized in the function
* @param blue brightness of blue, is normalized in the function
* @return consumed energy in coloumbs
*/
float modelChargeConsumption(leds leds, uint8_t red, uint8_t green,
uint8_t blue, uint16_t durationMs);
/**
* @brief sets the two leds on the top of the robot to the specified color
*
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Each color can range
* between 0 to 100
*/
void setTopLeds(uint32_t color);
/**
* @brief sets the two leds on the top of the robot to the specified color
*
* @param red brightness of red, is normalized in the function
* @param green brightness of green, is normalized in the function
* @param blue brightness of blue, is normalized in the function
*/
void setTopLeds(uint8_t red, uint8_t green, uint8_t blue);
/**
* @brief Let LEDs blink, returns after all blinks were executed
*
* @param amount how often should the leds blink
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
* Each color can range between 0 to 100
* Defaults to blue
* @param leds which LEDs should blink, default is TOP
* @param interval how many miliseconds the led is on, defaults to 1s
*/
void blink(uint16_t amount, uint32_t color = 0x00006400, leds leds = TOP,
uint32_t interval = 1000);
/**
* @brief turn off the given leds
*
* @param leds which leds should be turned off, defaults to ALL
*/
void turnOffLed(leds leds = ALL);
/**
* @brief wrapper to calulate the used colorformat from a rgb-value
*
* @param r red (0-100)
* @param g green (0-100)
* @param b blue (0-100)
* @return A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
*/
uint32_t color(uint8_t r, uint8_t g, uint8_t b);
private:
/**
* @brief normalizes every component of color to not exeed the maxBrightness
*
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
* @param maxBrigthness maximal level of brightness that is allowed for each color
* @return uint32_t A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Where each component can be
* between 0 - maxBrightness
*/
uint32_t normalizeColor(uint32_t color, uint8_t maxBrigthness=maxBrightness);
/**
* @brief normalizes every component of color to not exeed the maxBrightness
*
* @param color A 32-bit unsigned integer representing the color in the format
* 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component.
* @param maxBrigthness maximal level of brightness that is allowed for each
* color
* @return uint32_t A 32-bit unsigned integer representing the color in the
* format 0x00RRGGBB, where RR is the red component, GG is the green
* component, and BB is the blue component. Where each component
* can be between 0 - maxBrightness
*/
uint32_t normalizeColor(uint32_t color,
uint8_t maxBrightness = defaultMaxBrightness);
};
#endif //MultiColorLight_h
#endif // MultiColorLight_h

381
src/power/PowerManager.cpp Normal file
View File

@ -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() {}

198
src/power/PowerManager.h Normal file
View File

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

123
src/power/PowerParameters.h Normal file
View File

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

333
src/power/PowerScheduler.cpp Normal file
View File

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

173
src/power/PowerScheduler.h Normal file
View File

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