diff --git a/.gitignore b/.gitignore index 4612205..f9463f4 100644 --- a/.gitignore +++ b/.gitignore @@ -45,4 +45,4 @@ docs/* compile_commands.json # Build directories -build/ \ No newline at end of file +build/ diff --git a/example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino b/example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino new file mode 100644 index 0000000..42fbb08 --- /dev/null +++ b/example/advanced/Power_Management_Tests/Battery_discharge_logging/Battery_discharge_logging.ino @@ -0,0 +1,29 @@ +#include "Dezibot.h" +// Output CSV-Data about the power state on secondary UART mapped to bottom +// header + +// Using alternate Serial pins to not be powered by the USB port +#define RXD_HEADER 16 +#define TXD_HEADER 17 + +Dezibot dezibot = Dezibot(); +void setup() { + // put your setup code here, to run once: + dezibot.begin(); + Serial1.begin(9600, SERIAL_8N1, RXD_HEADER, TXD_HEADER); + Serial1.printf("Timestamp (ms),Current (mA),charge (%%),charge (C),voltage (V),isUSBPowered,isBatteryPowered,isBatteryCharging,isBatteryDischarging,isBatteryFullyCharged\r\n"); +} + +void loop() { + Serial1.printf("%d,%f,%f,%f,%f,%d,%d,%d,%d,%d\r\n", + millis(), + dezibot.power.getCurrentCurrent(), + dezibot.power.getBatteryChargePercent(), + dezibot.power.getBatteryChargeCoulombs(), + dezibot.power.getBatteryVoltage(), + dezibot.power.isUSBPowered(), dezibot.power.isBatteryPowered(), + dezibot.power.isBatteryCharging(), + dezibot.power.isBatteryDischarging(), + dezibot.power.isBatteryFullyCharged()); + sleep(1); +} \ No newline at end of file diff --git a/example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino b/example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino new file mode 100644 index 0000000..cc48749 --- /dev/null +++ b/example/advanced/Power_Management_Tests/Calculated_voltage_test/Calculated_voltage_test.ino @@ -0,0 +1,56 @@ +#include "Dezibot.h" + +Dezibot dezibot = Dezibot(); +void setup() { + // put your setup code here, to run once: + dezibot.begin(); +} + +float i_3v3; +float i_bat; +float bat_chargePercent; +float bat_coloumbs; +float bat_voltage; +bool isUSBPowered; +bool isCharging; +bool isFullyCharged; + +char i3v3_line[26]; +char ibat_line[26]; +char bat_chargePercent_line[26]; +char bat_coloumbs_line[26]; +char bat_voltage_line[26]; +char isUSBPowered_line[26]; +char isCharging_line[26]; +char isFullyCharged_line[26]; + +void loop() { + i_3v3 = dezibot.power.getCurrentCurrent(); + i_bat = dezibot.power.getBatteryCurrent(); + bat_chargePercent = dezibot.power.getBatteryChargePercent(); + bat_coloumbs = dezibot.power.getBatteryChargeCoulombs(); + bat_voltage = dezibot.power.getBatteryVoltage(); + isUSBPowered = dezibot.power.isUSBPowered(); + isCharging = dezibot.power.isBatteryCharging(); + isFullyCharged = dezibot.power.isBatteryFullyCharged(); + + sprintf(i3v3_line, "i_3v3: %.1f mA", i_3v3); + sprintf(ibat_line, "i_bat: %.1f mA", i_bat); + sprintf(bat_chargePercent_line, "CHG: %.2f %%", bat_chargePercent); + sprintf(bat_coloumbs_line, "CHG: %.2f C", bat_coloumbs); + sprintf(bat_voltage_line, "U: %.2f3V", bat_voltage); + sprintf(isUSBPowered_line, "USBPower: %d", isUSBPowered); + sprintf(isCharging_line, "Charging: %d", isCharging); + sprintf(isFullyCharged_line, "Full: %d", isFullyCharged); + + dezibot.display.clear(); + dezibot.display.println(i3v3_line); + dezibot.display.println(ibat_line); + dezibot.display.println(bat_chargePercent_line); + dezibot.display.println(bat_coloumbs_line); + dezibot.display.println(bat_voltage_line); + dezibot.display.println(isUSBPowered_line); + dezibot.display.println(isCharging_line); + dezibot.display.println(isFullyCharged_line); + delay(1000); +} \ No newline at end of file diff --git a/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino b/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino new file mode 100644 index 0000000..eac8906 --- /dev/null +++ b/example/advanced/Power_Management_Tests/Estimation_Test/Estimation_Test.ino @@ -0,0 +1,49 @@ +#include "Dezibot.h" + +Dezibot dezibot = Dezibot(); +long randomDurationMs = 0; + +void setup() { + // put your setup code here, to run once: + dezibot.begin(); + Serial.begin(115200); + // init RNG + int seed = analogRead(GPIO_NUM_42); + randomSeed(seed); + Serial.printf("Initialized PRNG with seed %d", seed); +} + +void loop() { + randomDurationMs = random(0, 100); + uint32_t color = random(0, 0xFFFFFF); + Serial.printf("Estimated current and charge consumption of turning an RGB " + "LED on at color 0x%06X for %d ms: %f mA, %f C\n", + color, randomDurationMs, + dezibot.multiColorLight.modelCurrentConsumption(color), + dezibot.multiColorLight.modelChargeConsumption( + TOP_LEFT, color, randomDurationMs)); + randomDurationMs = random(0, 100); + uint32_t randomDuty = random(0, 1 << LEDC_TIMER_10_BIT); + Serial.printf( + "Estimated current and charge consumption of turning front IR LED on at %d duty for " + "%d ms: %f mA, %f C\n", + randomDuty, + randomDurationMs, + dezibot.infraredLight.front.modelCurrentConsumption(randomDuty), + dezibot.infraredLight.front.modelChargeConsumptionOn(randomDurationMs)); + Serial.printf( + "Estimated current and charge consumption of turning bottom IR LED on at %d duty for " + "%d ms: %f mA, %f C\n", + randomDuty, + randomDurationMs, + dezibot.infraredLight.bottom.modelCurrentConsumption(randomDuty), + dezibot.infraredLight.bottom.modelChargeConsumptionOn(randomDurationMs)); + randomDurationMs = random(0, 100); + Serial.printf( + "Estimated current and charge consumption of running motor at default " + "duty cycle for " + "%d ms: %f mA, %f C\n", + randomDurationMs, dezibot.motion.left.modelCurrentConsumption(3900), + dezibot.motion.left.modelChargeConsumption(3900, randomDurationMs)); + delay(10000); +} \ No newline at end of file diff --git a/example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino b/example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino new file mode 100644 index 0000000..3c18529 --- /dev/null +++ b/example/advanced/Power_Management_Tests/Power-regulated_ColorCycle/Power-regulated_ColorCycle.ino @@ -0,0 +1,44 @@ +#include "Dezibot.h" + +/* + * This serves as an example of how to use the power management system deals with gradual changes. + * The ColorCycle example is a good one, because it contains gradual overstepping of the power budget, + * with the effect being easily visible. + */ + +Dezibot dezibot = Dezibot(); +void setup() { + // put your setup code here, to run once: + dezibot.begin(); + Serial.begin(115200); +} + +void loop() { + // put your main code here, to run repeatedly: + for (int d = 0; d < 255; d++) { + dezibot.multiColorLight.setLed( + ALL, dezibot.multiColorLight.color(d, 0, 255 - d)); + delay(2); + Serial.printf("current: %f, battery: %f\r\n", + dezibot.power.getCurrentCurrent(), + dezibot.power.getBatteryChargePercent()); + } + + for (int d = 0; d < 255; d++) { + dezibot.multiColorLight.setLed( + ALL, dezibot.multiColorLight.color(255 - d, d, 0)); + delay(2); + Serial.printf("current: %f, battery: %f\r\n", + dezibot.power.getCurrentCurrent(), + dezibot.power.getBatteryChargePercent()); + } + + for (int d = 0; d < 255; d++) { + dezibot.multiColorLight.setLed( + ALL, dezibot.multiColorLight.color(0, 255 - d, d)); + delay(2); + Serial.printf("current: %f, battery: %f\r\n", + dezibot.power.getCurrentCurrent(), + dezibot.power.getBatteryChargePercent()); + } +} diff --git a/example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino b/example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino new file mode 100644 index 0000000..2bc2725 --- /dev/null +++ b/example/advanced/Power_Management_Tests/Sudden_Consumption_Jumps/Sudden_Consumption_Jumps.ino @@ -0,0 +1,50 @@ +#include "Dezibot.h" + +/* + * This serves as an example of how to use the power management system deals with huge requests + * for power on an already heavily loaded system. + */ + +Dezibot dezibot; + +void setup() { + dezibot.begin(); + Serial.begin(115200); + // Wait for Serial to init + while (!Serial) { + ; + ; + } + Serial.println("Starting Power Management Test"); + Serial.println("Status on end of setup:"); + + dezibot.power.dumpPowerStatistics(); + dezibot.power.dumpConsumerStatistics(); +} + +void dumpInfoAndWait() { + delay(100); + dezibot.power.dumpPowerStatistics(); + dezibot.power.dumpConsumerStatistics(); + delay(900); +} + +void loop() { + dezibot.multiColorLight.setLed(ALL, 0x00FFFFFF); + Serial.println("Turned on all RGB LEDs"); + dumpInfoAndWait(); + + dezibot.motion.move(); + Serial.println("Turned on all motors"); + dumpInfoAndWait(); + + dezibot.infraredLight.bottom.turnOn(); + Serial.println("Turned on bottom IR LEDs"); + dumpInfoAndWait(); + + dezibot.multiColorLight.turnOffLed(ALL); + dezibot.motion.stop(); + dezibot.infraredLight.bottom.turnOff(); + Serial.println("Turned off all LEDs and motors"); + dumpInfoAndWait(); +} \ No newline at end of file diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino new file mode 100644 index 0000000..54bff7d --- /dev/null +++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption/Varied_Power_Consumption.ino @@ -0,0 +1,116 @@ +#include "Dezibot.h" + +/* + * Test case of varying power consumption, with logging of modeled state on + * secondary UART. + */ + +Dezibot dezibot; + +// Using alternate Serial pins to not be powered by the USB port +#define RXD_HEADER 16 +#define TXD_HEADER 17 + +// Task for independently toggling the bottom IR LED +void powerChange(void *pvParameters) { + while (true) { + dezibot.infraredLight.bottom.turnOn(); + delay(1000); + dezibot.infraredLight.bottom.turnOff(); + delay(1000); + } +} + +// Tast for independently toggling the front IR LED +void powerChange2(void *pvParameters) { + while (true) { + dezibot.infraredLight.front.turnOn(); + delay(1000); + dezibot.infraredLight.front.turnOff(); + delay(1000); + } +} + +// Task for periodic logging of power state +void outputCsvLine(void *pvParameters) { + while (true) { + Serial1.printf( + "%d,%f,%f,%f,%f,%f,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%" + "f,%f,%f,%f\r\n", + millis(), dezibot.power.getCurrentCurrent(), + dezibot.power.getMax3V3Current(), + dezibot.power.getBatteryChargePercent(), + dezibot.power.getBatteryChargeCoulombs(), + dezibot.power.getBatteryVoltage(), dezibot.power.isUSBPowered(), + dezibot.power.isBatteryPowered(), dezibot.power.isBatteryCharging(), + dezibot.power.isBatteryDischarging(), + dezibot.power.isBatteryFullyCharged(), + dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::ESP), + dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::WIFI), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_BOTTOM), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::RGBW_SENSOR), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::LED_IR_FRONT), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::LED_IR_BOTTOM), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::PT_IR), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::PT_DL), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::LED_UV), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::DISPLAY_OLED), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::MOTOR_LEFT), + dezibot.power.getConsumerCurrent( + PowerParameters::PowerConsumers::MOTOR_RIGHT), + dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::IMU)); + delay(100); + } +} + +void setup() { + dezibot.begin(); + Serial1.begin(115200, SERIAL_8N1, RXD_HEADER, TXD_HEADER); + // Output CSV-Header for Timestamp and modelled current of all components + Serial1.printf( + "Timestamp (ms),Current (mA),calculated max current (mA),charge " + "(%%),charge (C),voltage " + "(V),isUSBPowered,isBatteryPowered,isBatteryCharging," + "isBatteryDischarging,isBatteryFullyCharged,ESP (mA),WIFI " + "(mA),LED_RGB_TOP_LEFT (mA)," + "LED_RGB_TOP_RIGHT (mA),LED_RGB_BOTTOM (mA),RGBW_SENSOR " + "(mA),LED_IR_FRONT (mA),LED_IR_BOTTOM (mA),PT_IR (mA),PT_DL (mA)," + "LED_UV (mA),DISPLAY_OLED (mA),MOTOR_LEFT (mA),MOTOR_RIGHT " + "(mA),IMU (mA)\r\n"); + // Start logging task + xTaskCreate(outputCsvLine, "outputCsvLine", 4096, NULL, tskIDLE_PRIORITY, + NULL); + + // Start power consumption task + xTaskCreate(powerChange, "powerChange", 4096, NULL, tskIDLE_PRIORITY, NULL); + + // Start second power consumption task + xTaskCreate(powerChange2, "powerChange2", 4096, NULL, tskIDLE_PRIORITY, NULL); +} + +void loop() { + while (true) { + dezibot.multiColorLight.turnOffLed(); + for (leds led : {TOP_LEFT, TOP_RIGHT, BOTTOM, TOP, ALL}) { + dezibot.multiColorLight.setLed(led, 255, 0, 0); + delay(1000); + dezibot.multiColorLight.setLed(led, 0, 255, 0); + delay(1000); + dezibot.multiColorLight.setLed(led, 0, 0, 255); + delay(1000); + } + } +} \ No newline at end of file diff --git a/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino b/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino new file mode 100644 index 0000000..7e680fe --- /dev/null +++ b/example/advanced/Power_Management_Tests/Varied_Power_Consumption_no_logging/Varied_Power_Consumption_no_logging.ino @@ -0,0 +1,57 @@ +#include "Dezibot.h" + +/* + * Test case of varying power consumption, with logging of modeled state on + * secondary UART. + */ + +Dezibot dezibot; + +// Using alternate Serial pins to not be powered by the USB port +#define RXD_HEADER 16 +#define TXD_HEADER 17 + +// Task for 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); + } + } +} \ No newline at end of file diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index 5add5aa..2576f9f 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -5,18 +5,18 @@ #include "Dezibot.h" #include +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(); }; - - diff --git a/src/Dezibot.h b/src/Dezibot.h index 8dcacf5..93353cd 100644 --- a/src/Dezibot.h +++ b/src/Dezibot.h @@ -19,6 +19,7 @@ #include "infraredLight/InfraredLight.h" #include "communication/Communication.h" #include "display/Display.h" +#include "power/PowerManager.h" class Dezibot { @@ -33,6 +34,7 @@ public: InfraredLight infraredLight; Communication communication; Display display; + PowerManager power; void begin(void); }; diff --git a/src/colorDetection/ColorDetection.cpp b/src/colorDetection/ColorDetection.cpp index b41bb16..06b3bcf 100644 --- a/src/colorDetection/ColorDetection.cpp +++ b/src/colorDetection/ColorDetection.cpp @@ -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()<>8)&0x00FF)); - if(Wire.endTransmission() != 0){ - Serial.printf("Reading Register %d failed",regAddr); - } -}; \ No newline at end of file +void ColorDetection::writeDoubleRegister(uint8_t regAddr, uint16_t data) { + // erst low dann high + Wire.beginTransmission(VEML_ADDR); + Wire.write(regAddr); + Wire.write((uint8_t)(data & 0x00FF)); + Wire.write((uint8_t)((data >> 8) & 0x00FF)); + if (Wire.endTransmission() != 0) { + Serial.printf("Reading Register %d failed", regAddr); + } +}; + + +float ColorDetection::modelCurrentConsumption() { + return PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW; +}; + +float ColorDetection::modelChargeConsumption(uint16_t durationMs) { + return (PowerParameters::CurrentConsumptions::CURRENT_SENSOR_RGBW * + durationMs) / 10e6f; +} diff --git a/src/colorDetection/ColorDetection.h b/src/colorDetection/ColorDetection.h index 6fd284b..4f2a7ce 100644 --- a/src/colorDetection/ColorDetection.h +++ b/src/colorDetection/ColorDetection.h @@ -11,9 +11,10 @@ #ifndef ColorDetection_h #define ColorDetection_h -#include -#include +#include "../power/PowerManager.h" #include +#include +#include //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); }; diff --git a/src/communication/Communication.cpp b/src/communication/Communication.cpp index 6e443f9..1669ecf 100644 --- a/src/communication/Communication.cpp +++ b/src/communication/Communication.cpp @@ -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); -}; \ No newline at end of file + xTaskCreate(vTaskUpdate, "vTaskMeshUpdate", 4096, &ucParameterToPass, + tskIDLE_PRIORITY, &xHandle); + configASSERT(xHandle); +}; diff --git a/src/communication/Communication.h b/src/communication/Communication.h index e6644f7..090d99f 100644 --- a/src/communication/Communication.h +++ b/src/communication/Communication.h @@ -1,29 +1,33 @@ #ifndef Communication_h #define Communication_h -#include +#include "../power/PowerManager.h" #include #include +#include #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; diff --git a/src/display/Display.cpp b/src/display/Display.cpp index 9e687d5..6fe4a6c 100644 --- a/src/display/Display.cpp +++ b/src/display/Display.cpp @@ -4,7 +4,7 @@ * @brief Adds the ability to print to the display of the robot. * @version 0.1 * @date 2024-06-05 - * + * * @copyright Copyright (c) 2024 */ @@ -14,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; +}; diff --git a/src/display/Display.h b/src/display/Display.h index 15774a4..8c9bc47 100644 --- a/src/display/Display.h +++ b/src/display/Display.h @@ -11,9 +11,15 @@ #ifndef Display_h #define Display_h -#include -#include +#include "../power/PowerManager.h" #include "DisplayCMDs.h" +#include +#include + +// 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); }; diff --git a/src/infraredLight/InfraredLED.cpp b/src/infraredLight/InfraredLED.cpp index 7298603..386821d 100644 --- a/src/infraredLight/InfraredLED.cpp +++ b/src/infraredLight/InfraredLED.cpp @@ -1,6 +1,10 @@ #include "InfraredLight.h" #define pwmSpeedMode LEDC_LOW_SPEED_MODE +#define IR_FRONT_PIN 14 +#define IR_BOTTOM_PIN 13 +#define DUTY_RESOLUTION LEDC_TIMER_10_BIT +#define DUTY_CYCLE_FREQUENCY 512 InfraredLED::InfraredLED(uint8_t pin,ledc_timer_t timer, ledc_channel_t channel){ this->ledPin = pin; @@ -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); -}; \ No newline at end of file +}; + +float InfraredLED::modelCurrentConsumption(uint32_t duty){ + // Float to force float division without casting + constexpr float max_value = 1 << DUTY_RESOLUTION; + const float duty_factor = duty / max_value; + if (this->ledPin == IR_BOTTOM_PIN) { + return duty_factor * PowerParameters::CurrentConsumptions::CURRENT_LED_IR_BOTTOM; + } else if (this->ledPin == IR_FRONT_PIN) { + return duty_factor * PowerParameters::CurrentConsumptions::CURRENT_LED_IR_FRONT; + } + return NAN; +}; + +float InfraredLED::modelChargeConsumptionOn(uint16_t durationMs) { + 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; +} diff --git a/src/infraredLight/InfraredLight.h b/src/infraredLight/InfraredLight.h index 024e10a..9ef00c5 100644 --- a/src/infraredLight/InfraredLight.h +++ b/src/infraredLight/InfraredLight.h @@ -10,10 +10,14 @@ */ #ifndef InfraredLight_h #define InfraredLight_h -#include -#include +#include "../power/PowerManager.h" #include "driver/ledc.h" +#include +#include +#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; diff --git a/src/lightDetection/LightDetection.cpp b/src/lightDetection/LightDetection.cpp index 8934941..46c52da 100644 --- a/src/lightDetection/LightDetection.cpp +++ b/src/lightDetection/LightDetection.cpp @@ -27,7 +27,7 @@ photoTransistors LightDetection::getBrightest(ptType type){ photoTransistors maxSensor; uint16_t maxReading = 0; uint16_t currentReading = 0; - + if (type == IR){ maxSensor = IR_FRONT; for(const auto pt : allIRPTs){ @@ -47,15 +47,15 @@ photoTransistors LightDetection::getBrightest(ptType type){ } } } - + return maxSensor; }; uint32_t LightDetection::getAverageValue(photoTransistors sensor, uint32_t measurments, uint32_t timeBetween){ - + TickType_t xLastWakeTime = xTaskGetTickCount(); TickType_t frequency = timeBetween / portTICK_PERIOD_MS; - uint64_t cumulatedResult = 0; + uint64_t cumulatedResult = 0; for(int i = 0; i < measurments; i++){ cumulatedResult += LightDetection::getValue(sensor); xTaskDelayUntil(&xLastWakeTime,frequency); @@ -64,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; -}; \ No newline at end of file +}; + +float LightDetection::modelCurrentConsumption(photoTransistors sensor){ + if(sensor == DL_FRONT || sensor == DL_BOTTOM){ + return PowerParameters::CurrentConsumptions::CURRENT_PT * 2; + } else { + return PowerParameters::CurrentConsumptions::CURRENT_PT * 4; + } +}; + +float LightDetection::modelChargeConsumptionOn(photoTransistors sensor, + uint16_t durationMs) { + return (LightDetection::modelCurrentConsumption(sensor) * durationMs) / 10e6f; +} diff --git a/src/lightDetection/LightDetection.h b/src/lightDetection/LightDetection.h index 32e25f3..7597e78 100644 --- a/src/lightDetection/LightDetection.h +++ b/src/lightDetection/LightDetection.h @@ -11,10 +11,13 @@ #ifndef LightDetection_h #define LightDetection_h -#include +#include "../power/PowerManager.h" #include +#include +#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; diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 2448f04..7f316ca 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -31,8 +31,13 @@ void Motion::begin(void) { void Motion::moveTask(void * args) { uint32_t runtime = (uint32_t)args; - Motion::left.setSpeed(LEFT_MOTOR_DUTY); - Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + if (!Motion::left.setSpeed(LEFT_MOTOR_DUTY) || !Motion::right.setSpeed(RIGHT_MOTOR_DUTY)) { + if(Serial){ + Serial.println("Failed to start motors due to power constraints"); + } + Motion::stop(); // Use to clean up + return; + } Motion::xLastWakeTime = xTaskGetTickCount(); while(1){ if(runtime>40||runtime==0){ @@ -72,8 +77,11 @@ void Motion::moveTask(void * args) { RIGHT_MOTOR_DUTY+=changerate; } - Motion::left.setSpeed(LEFT_MOTOR_DUTY); - Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + bool leftSpeedchangeSuccess = Motion::left.setSpeed(LEFT_MOTOR_DUTY); + bool rightSpeedchangeSuccess = Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + if (!leftSpeedchangeSuccess || !rightSpeedchangeSuccess) { + ESP_LOGW(TAG, "Failed to change motor speed due to power constraints"); + } } else { vTaskDelayUntil(&xLastWakeTime,runtime); Motion::left.setSpeed(0); @@ -116,7 +124,13 @@ void Motion::leftMotorTask(void * args) { xAntiClockwiseTaskHandle = NULL; } Motion::right.setSpeed(0); - Motion::left.setSpeed(LEFT_MOTOR_DUTY); + if(!Motion::left.setSpeed(LEFT_MOTOR_DUTY)){ + if(Serial){ + Serial.println("Can not rotate Clockwise due to power constraints"); + } + Motion::stop(); + return; + } while(1){ if((runtime>40)||(runtime==0)){ vTaskDelayUntil(&xLastWakeTime,40); @@ -155,7 +169,14 @@ void Motion::rightMotorTask(void * args) { vTaskDelete(xClockwiseTaskHandle); xClockwiseTaskHandle = NULL; } - Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + if(!Motion::left.setSpeed(RIGHT_MOTOR_DUTY)){ + if(Serial){ + Serial.println( + "Can not rotate Antilcockwise due to power constraints"); + } + Motion::stop(); + return; + } Motion::left.setSpeed(0); while(1){ if(runtime>40||runtime==0){ @@ -197,6 +218,8 @@ void Motion::stop(void){ vTaskDelete(xClockwiseTaskHandle); xClockwiseTaskHandle = NULL; } + PowerManager::releaseCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT); + PowerManager::releaseCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT); Motion::left.setSpeed(0); Motion::right.setSpeed(0); } diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 2a7ed8a..eb56d21 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -17,16 +17,23 @@ #include #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; diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp index 0c26a58..78fc1f2 100644 --- a/src/motion/Motor.cpp +++ b/src/motion/Motor.cpp @@ -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;iduty += 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(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; +} diff --git a/src/motionDetection/IMU_CMDs.h b/src/motionDetection/IMU_CMDs.h index ab420c7..25ac657 100644 --- a/src/motionDetection/IMU_CMDs.h +++ b/src/motionDetection/IMU_CMDs.h @@ -6,8 +6,70 @@ #define ADDR_MASK 0x7F //Registers -#define MCLK_RDY 0x00 - +//User bank 0 +#define MCLK_RDY 0x00 +#define DEVICE_CONFIG 0x01 +#define SIGNAL_PATH_RESET 0x02 +#define DRIVE_CONFIG1 0x03 +#define DRIVE_CONFIG2 0x04 +#define DRIVE_CONFIG3 0x05 +#define INT_CONFIG 0x06 +#define TEMP_DATA1 0x09 +#define TEMP_DATA0 0x0A +#define ACCEL_DATA_X1 0x0B +#define ACCEL_DATA_X0 0x0C +#define ACCEL_DATA_Y1 0x0D +#define ACCEL_DATA_Y0 0x0E +#define ACCEL_DATA_Z1 0x0F +#define ACCEL_DATA_Z0 0x10 +#define GYRO_DATA_X1 0x11 +#define GYRO_DATA_X0 0x12 +#define GYRO_DATA_Y1 0x13 +#define GYRO_DATA_Y0 0x14 +#define GYRO_DATA_Z1 0x15 +#define GYRO_DATA_Z0 0x16 +#define TMST_FSYNCH 0x17 +#define TMST_FSYNCL 0x18 +#define APEX_DATA4 0x1D +#define APEX_DATA5 0x1E +#define PWR_MGMT0 0x1F +#define GYRO_CONFIG0 0x20 +#define ACCEL_CONFIG0 0x21 +#define TEMP_CONFIG0 0x22 +#define GYRO_CONFIG1 0x23 +#define ACCEL_CONFIG1 0x24 +#define APEX_CONFIG0 0x25 +#define APEX_CONFIG1 0x26 +#define WOM_CONFIG 0x27 +#define FIFO_CONFIG1 0x28 +#define FIFO_CONFIG2 0x29 +#define FIFO_CONFIG3 0x2A +#define INT_SOURCE0 0x2B +#define INT_SOURCE1 0x2C +#define INT_SOURCE3 0x2D +#define INT_SOURCE4 0x2E +#define FIFO_LOST_PKT0 0x2F +#define FIFO_LOST_PKT1 0x30 +#define APEX_DATA0 0x31 +#define APEX_DATA1 0x32 +#define APEX_DATA2 0x33 +#define APEX_DATA3 0x34 +#define INTF_CONFIG0 0x35 +#define INTF_CONFIG1 0x36 +#define INT_STATUS_DRDY 0x39 +#define INT_STATUS 0x3A +#define INT_STATUS2 0x3B +#define INT_STATUS3 0x3C +#define FIFO_COUNTH 0x3D +#define FIFO_COUNTL 0x3E +#define FIFO_DATA 0x3F +#define WHO_AM_I 0x75 +#define BLK_SEL_W 0x79 +#define MADDR_W 0x7A +#define M_W 0x7B +#define BLK_SEL_R 0x7C +#define MADDR_R 0x7D +#define M_R 0x7E #define REG_TEMP_LOW 0x0A #define REG_TEMP_HIGH 0X09 diff --git a/src/motionDetection/MotionDetection.cpp b/src/motionDetection/MotionDetection.cpp index 6386e2f..19cdd7c 100644 --- a/src/motionDetection/MotionDetection.cpp +++ b/src/motionDetection/MotionDetection.cpp @@ -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; iendTransaction(); -}; \ No newline at end of file +}; + +float MotionDetection::modelCurrentConsumption(){ + return PowerParameters::CurrentConsumptions::CURRENT_IMU; +} + +float MotionDetection::modelChargeConsumption(uint16_t durationMs) { + return (this->modelCurrentConsumption() * durationMs) / 10e6f; +} diff --git a/src/motionDetection/MotionDetection.h b/src/motionDetection/MotionDetection.h index 8fecbb6..8bdd63b 100644 --- a/src/motionDetection/MotionDetection.h +++ b/src/motionDetection/MotionDetection.h @@ -10,9 +10,13 @@ */ #ifndef MotionDetection_h #define MotionDetection_h -#include -#include +#include "../power/PowerManager.h" #include "IMU_CMDs.h" +#include +#include + +#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 \ No newline at end of file diff --git a/src/multiColorLight/MultiColorLight.cpp b/src/multiColorLight/MultiColorLight.cpp index aa22a4a..90e0ef8 100644 --- a/src/multiColorLight/MultiColorLight.cpp +++ b/src/multiColorLight/MultiColorLight.cpp @@ -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 - 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); -} \ No newline at end of file + 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); +}; diff --git a/src/multiColorLight/MultiColorLight.h b/src/multiColorLight/MultiColorLight.h index 44498cf..9731145 100644 --- a/src/multiColorLight/MultiColorLight.h +++ b/src/multiColorLight/MultiColorLight.h @@ -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 +#include "../power/PowerManager.h" #include "ColorConstants.h" +#include + +#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 \ No newline at end of file +#endif // MultiColorLight_h \ No newline at end of file diff --git a/src/power/PowerManager.cpp b/src/power/PowerManager.cpp new file mode 100644 index 0000000..1f012b6 --- /dev/null +++ b/src/power/PowerManager.cpp @@ -0,0 +1,381 @@ +/** + * @file PowerManager.h + * @author Phillip Kühne + * @brief This component provides utilities for keeping track of power usage + * consumption. + * @version 0.1 + * @date 2024-11-23 + */ + +#include "PowerManager.h" + +SemaphoreHandle_t PowerManager::powerMutex = NULL; + +void vTaskUpdatePowerState(void *pvParameters) { + for (;;) { + ESP_LOGV(TAG, "Updating Power State..."); + TickType_t executionStart = xTaskGetTickCount(); + PowerManager::updatePowerStateHandler(); + vTaskDelayUntil( + &executionStart, + pdMS_TO_TICKS(PowerParameters::POWER_STATE_UPDATE_INTERVAL_MS)); + } +} + +void PowerManager::begin() { + // Check if another instance of us already initialized the power scheduler, + // if not, we will do it. + ESP_LOGI(TAG, "Initializing Power Management"); + + // Create mutex if it doesn't exist + if (powerMutex == NULL) { + powerMutex = xSemaphoreCreateMutex(); + if (powerMutex == NULL) { + ESP_LOGE(TAG, "Failed to create power mutex"); + Serial.println("Failed to create power mutex"); + } + } + + // Initialize IO for battery voltage measurement + pinMode(PowerParameters::PinConfig::BAT_ADC_EN, OUTPUT); + pinMode(PowerParameters::PinConfig::BAT_ADC, INPUT); + + if (powerScheduler == nullptr) { + ESP_LOGI(TAG, "Creating Power Scheduler"); + powerScheduler = &PowerScheduler::getPowerScheduler( + PowerParameters::Battery::CELL_CURRENT_1C_MA, + PowerParameters::Battery::CELL_CURRENT_2C_MA); + PowerManager::initPowerState(); + PowerManager::recalculateCurrentBudgets(); + TaskHandle_t xHandle = NULL; + xTaskCreate(vTaskUpdatePowerState, "vTaskPowerStateUpdate", 4096, NULL, + tskIDLE_PRIORITY, &xHandle); + configASSERT(xHandle); + + if (!(powerScheduler->tryAccquireCurrentAllowance( + PowerParameters::PowerConsumers::ESP, + PowerParameters::CurrentConsumptions::CURRENT_ESP_AVG))) { + ESP_LOGE(TAG, "Could not get power for ESP"); + Serial.println("Could not get power for ESP"); + } + } +} + +float PowerManager::getFreeLimitCurrentBudget() { + return powerScheduler->getFreeLimitCurrentBudget(); +} + +float PowerManager::getFreeMaximumCurrentBudget() { + return powerScheduler->getFreeMaximumCurrentBudget(); +} + +bool PowerManager::tryAccquireCurrentAllowance( + PowerParameters::PowerConsumers consumer, uint16_t neededCurrent, + uint16_t requestedDurationMs) { + return powerScheduler->tryAccquireCurrentAllowance(consumer, neededCurrent, + requestedDurationMs); +} + +void PowerManager::releaseCurrent(PowerParameters::PowerConsumers consumer) { + powerScheduler->releaseCurrent(consumer); +} + +bool PowerManager::waitForCurrentAllowance(PowerParameters::PowerConsumers consumer, + uint16_t neededCurrent, + uint16_t maxSlackTimeMs, + uint16_t requestedDurationMs) { + return powerScheduler->waitForCurrentAllowance( + consumer, neededCurrent, maxSlackTimeMs, requestedDurationMs); +} + +void PowerManager::beginPermanentDeepSleep(void) { + return powerScheduler->beginPermanentDeepSleep(); +} + +float PowerManager::getCurrentCurrent(void) { + return powerScheduler->getCurrentCurrent(); +} + +float PowerManager::getBatteryCurrent(void) { + const float i_3v3 = getCurrentCurrent(); + const float u_3v3 = 3.3; + const float u_bat = getBatteryVoltage(); + const float eta = PowerParameters::BUCK_BOOST_EFFICIENCY; + return (u_3v3 * i_3v3) / (u_bat * eta); +} + +void PowerManager::recalculateCurrentBudgets(void) { + return powerScheduler->recalculateCurrentBudgets(); +} + +float PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers consumer) { + return powerScheduler->getConsumerCurrent(consumer); +} + +float PowerManager::getBatteryVoltage() { + // Get the battery voltage from the ADC and convert it to a voltage + // using the voltage divider. + // Enable voltage divider + digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, HIGH); + // Allow voltage to stabilize + delayMicroseconds(10); + // Returns value between 0 and 4095 mapping to between 0 and 3.3V + uint16_t batteryAdcValue = analogRead(PowerParameters::PinConfig::BAT_ADC); + // Disable voltage divider + digitalWrite(PowerParameters::PinConfig::BAT_ADC_EN, LOW); + // Convert ADC value to voltage + float batteryVoltage = + (batteryAdcValue / 4095.0 * 3.3) * + PowerParameters::Battery::BAT_ADC::VOLTAGE_DIVIDER_FACTOR; + ESP_LOGD(TAG, "Battery ADC value: %d, Calculated voltage: %.2f", + batteryAdcValue, batteryVoltage); + return batteryVoltage; +} + +float PowerManager::getBatteryChargePercent() { return percentRemaining; } + +float PowerManager::getBatteryChargeCoulombs() { return coloumbsRemaining; } + +float PowerManager::getBatteryVoltageChargePercent() { + // Directly get the battery voltage, correct the curve with an offset and + // calculate the charge state based on the discharge curve. + float batteryVoltage = getBatteryVoltage() + PowerManager::fullVoltageOffset; + float chargeState = 0; + // Clamp edge cases + if (batteryVoltage >= + PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0]) { + return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[0]; + } + if (batteryVoltage <= + PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES + [PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1]) { + return PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES + [PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS - 1]; + } + float p1_x, p1_y, p2_x, p2_y; + for (int i = 0; i < PowerParameters::Battery::DISCHARGE_CURVE::NUM_POINTS; + i++) { + if (batteryVoltage >= + PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i]) { + p1_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i]; + p1_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i]; + p2_y = PowerParameters::Battery::DISCHARGE_CURVE::CHARGE_STATES[i + 1]; + p2_x = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[i + 1]; + chargeState = + ((p2_y - p1_y) / (p2_x - p1_x)) * (batteryVoltage - p1_x) + p1_y; + return chargeState; + } + } +} + +void PowerManager::updatePowerStateHandler() { + + // Update supply and charge state flags + PowerManager::busPowered = !digitalRead(PowerParameters::PinConfig::VUSB_SENS); + PowerManager::chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT); + + // If the battery is charging and fully charged + if (PowerManager::busPowered && !PowerManager::chargingState) { + // Calibrate voltage offset on full Battery + PowerManager::fullVoltageOffset = PowerParameters::Battery::DISCHARGE_CURVE::VOLTAGES[0] - getBatteryVoltage(); + PowerManager::coloumbsRemaining = PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB; + } + + ESP_LOGD(TAG, "Bus Powered: %d, Charging: %d", busPowered, chargingState); + + float currentCurrent = powerScheduler->getCurrentCurrent(); + int referenceCurrentMa = + PowerParameters::Battery::DISCHARGE_CURVE::REFERENCE_CURRENT_A * 1000; + + float coloumbsConsumedSinceLastUpdate; + + // Calculate remaining battery charge in Coulombs based on current and time + if (!PowerManager::busPowered) { + coloumbsConsumedSinceLastUpdate = + (currentCurrent / 1000) * + ((pdTICKS_TO_MS(xTaskGetTickCount() - lastPowerStateUpdate)) / 1000.0); + + // Update coloumbs remaining + coloumbsRemaining -= coloumbsConsumedSinceLastUpdate; + } + float chargeState; + + // If current flow is close enough to reference, get battery charge state via + // voltage curve + if (!PowerManager::busPowered) { + if ((currentCurrent > (referenceCurrentMa * 0.6)) && + (currentCurrent < (referenceCurrentMa * 1.4))) { + // Get battery charge state from voltage curve + chargeState = getBatteryVoltageChargePercent(); + ESP_LOGD(TAG, "Charge state from voltage: %f", chargeState); + } else { + // Estimate battery charge state from charge consumption + float oldChargeState = lastSOC[latestSoCIndex]; + chargeState = oldChargeState - + ((coloumbsConsumedSinceLastUpdate / + PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB) * + 100); + ESP_LOGD(TAG, "Charge state from current: %f", chargeState); + } + } else { + // If we are charging, we can't estimate the charge state based on current + // consumption + chargeState = getBatteryVoltageChargePercent(); + ESP_LOGD(TAG, "Charge state from voltage (USB powered): %f", chargeState); + } + + // Bound value between 0 and 100 + chargeState = chargeState > 100 ? 100 : chargeState; + chargeState = chargeState < 0 ? 0 : chargeState; + + addSoCSample(chargeState); + + for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) { + ESP_LOGD(TAG, "SoC[%d]: %f", i, lastSOC[i]); + } + + // Update percentage remaining based on charge state average + float sampleSum = 0; + for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) { + sampleSum += lastSOC[i]; + } + PowerManager::percentRemaining = + sampleSum / PowerParameters::Battery::AVERAGING_SAMPLES; + + // Update last update time + PowerManager::lastPowerStateUpdate = xTaskGetTickCount(); + + // Update the available current (changes based on battery state of charge) + PowerManager::powerScheduler->recalculateCurrentBudgets(); + ESP_LOGD(TAG, "Current: %f mA, Charge: %f Coulombs, %f %%", currentCurrent, + coloumbsRemaining, percentRemaining); + + return; +} + +float PowerManager::getMax3V3Current() { + // Conversion from Thesis + float u_bat = getBatteryVoltage(); + float i_bat = PowerParameters::Battery::CELL_CURRENT_1C_MA; + float eta = PowerParameters::BUCK_BOOST_EFFICIENCY; + constexpr float u_3v3 = 3.3; + return (u_bat * i_bat * eta) / u_3v3; +} + +void PowerManager::addSoCSample(float soc) { + PowerMutex lock(powerMutex); + if (!lock.isLocked()) { + ESP_LOGE(TAG, "Could not take power to add SoC sample"); + return; + } + latestSoCIndex = + (latestSoCIndex + 1) % PowerParameters::Battery::AVERAGING_SAMPLES; + lastSOC[latestSoCIndex] = soc; +} + +void PowerManager::initPowerState(void) { + // Initialize the power state + lastPowerStateUpdate = xTaskGetTickCount(); + // TODO: Get initial battery charge state based on voltage, set coloumbs based + // on that + const float initialChargePercentages = getBatteryVoltageChargePercent(); + for (int i = 0; i < PowerParameters::Battery::AVERAGING_SAMPLES; i++) { + lastSOC[i] = initialChargePercentages; + } + coloumbsRemaining = initialChargePercentages / 100 * + PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB; + constexpr float fullColoumbs = + PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB; + percentRemaining = initialChargePercentages; + // Set up flags and pins for them + pinMode(PowerParameters::PinConfig::VUSB_SENS, INPUT_PULLUP); + pinMode(PowerParameters::PinConfig::BAT_CHG_STAT, INPUT_PULLUP); + busPowered = digitalRead(PowerParameters::PinConfig::VUSB_SENS); + chargingState = digitalRead(PowerParameters::PinConfig::BAT_CHG_STAT); +} + +void PowerManager::dumpPowerStatistics() { + Serial.printf("======== Dezibot Power Statistics ========\r\n"); + Serial.printf("Current: %f mA\r\n", PowerManager::getCurrentCurrent()); + Serial.printf("Battery Voltage: %f V\r\n", PowerManager::getBatteryVoltage()); + Serial.printf("Battery Charge: %f %%\r\n", PowerManager::getBatteryChargePercent()); + Serial.printf("Battery Charge: %f Coulombs\r\n", + PowerManager::getBatteryChargeCoulombs()); + Serial.printf("Max 3.3V Current in this state (1C, 2C): %f mA, %f mA \r\n", + PowerManager::getMax3V3Current(), PowerManager::getMax3V3Current() * 2); + Serial.printf("=========================================\r\n"); +} + +void PowerManager::dumpConsumerStatistics() { + Serial.printf("======== Dezibot Consumer Statistics ========\r\n"); + Serial.printf("ESP: %f mA\r\n", PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::ESP)); + Serial.printf("WIFI: %f mA\r\n", PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::WIFI)); + Serial.printf("LED_RGB_TOP_LEFT: %f mA\r\n", + PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT)); + Serial.printf("LED_RGB_TOP_RIGHT: %f mA\r\n", + PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT)); + Serial.printf("LED_RGB_BOTTOM: %f mA\r\n", + PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_RGB_BOTTOM)); + Serial.printf( + "RGBW_SENSOR: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::RGBW_SENSOR)); + Serial.printf("LED_IR_BOTTOM: %f mA\r\n", + PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::LED_IR_BOTTOM)); + Serial.printf( + "LED_IR_FRONT: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::LED_IR_FRONT)); + Serial.printf( + "PT_IR: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::PT_IR)); + Serial.printf( + "PT_DL: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::PT_DL)); + Serial.printf( + "LED_UV: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::LED_UV)); + Serial.printf( + "DISPLAY_OLED: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::DISPLAY_OLED)); + Serial.printf( + "MOTOR_LEFT: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_LEFT)); + Serial.printf( + "MOTOR_RIGHT: %f mA\r\n", + PowerManager::getConsumerCurrent(PowerParameters::PowerConsumers::MOTOR_RIGHT)); + Serial.printf("IMU: %f mA\r\n", PowerManager::getConsumerCurrent( + PowerParameters::PowerConsumers::IMU)); + Serial.printf("=============================================\r\n"); +} + +bool PowerManager::isUSBPowered() { return busPowered; } + +bool PowerManager::isBatteryPowered() { return !busPowered; } + +bool PowerManager::isBatteryCharging() { return chargingState && busPowered; } + +bool PowerManager::isBatteryDischarging() { return !chargingState && !busPowered; } + +bool PowerManager::isBatteryFullyCharged() { return !chargingState && busPowered; } +int PowerManager::latestSoCIndex = 0; +float PowerManager::lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES] = {0}; +TickType_t PowerManager::lastPowerStateUpdate = 0; +float PowerManager::coloumbsRemaining = + PowerParameters::Battery::CELL_CHARGE_FULL_COLOUMB; +float PowerManager::percentRemaining = 100.0; +PowerScheduler *PowerManager::powerScheduler = nullptr; + +bool PowerManager::busPowered = false; + +bool PowerManager::chargingState = false; + +float PowerManager::fullVoltageOffset = 0; + +PowerManager::PowerManager() {} + +PowerManager::~PowerManager() {} diff --git a/src/power/PowerManager.h b/src/power/PowerManager.h new file mode 100644 index 0000000..362e7b7 --- /dev/null +++ b/src/power/PowerManager.h @@ -0,0 +1,198 @@ +/** + * @file PowerManager.h + * @author Phillip Kühne + * @brief This component provides utilities for keeping track of power usage + * consumption. + * @version 0.1 + * @date 2024-11-23 + */ + +#include "PowerScheduler.h" +#include "esp_adc/adc_cali.h" +#include "esp_adc/adc_cali_scheme.h" +#include "driver/gpio.h" +#include "driver/adc.h" +#include "esp_adc/adc_oneshot.h" +#include "freertos/semphr.h" +#ifndef PowerManager_h +#define PowerManager_h + +#define TAG "Power" + +enum TaskResumptionReason { POWER_AVAILABLE, TIMEOUT }; + +class PowerManager { + +private: + static SemaphoreHandle_t powerMutex; + static constexpr uint16_t MUTEX_TIMEOUT_MS = 1; + + // RAII for mutex + class PowerMutex { + private: + SemaphoreHandle_t &mutex; + bool locked; + + public: + PowerMutex(SemaphoreHandle_t &mutex) : mutex(mutex), locked(false) { + locked = + (xSemaphoreTake(mutex, pdMS_TO_TICKS(MUTEX_TIMEOUT_MS)) == pdTRUE); + if (!locked) { + ESP_LOGW(TAG, "Could not take power mutex"); + } + } + + ~PowerMutex() { + if (locked) { + xSemaphoreGive(mutex); + } + } + bool isLocked() const { return locked; } + }; + +protected: + /// @brief PowerScheduler instance to manage power consumption + static PowerScheduler *powerScheduler; + + /* + * Power State + */ + + /// @brief last time of power state update + static TickType_t lastPowerStateUpdate; + + /// @brief remaining Charge in coulombs + static float coloumbsRemaining; + + /// @brief remaining Charge in percent + static float percentRemaining; + + static bool busPowered; + + static bool chargingState; + + static float fullVoltageOffset; + + /// @brief Circular array of last calculated values for current state of + /// charge + static float lastSOC[PowerParameters::Battery::AVERAGING_SAMPLES]; + static int latestSoCIndex; + + /// @brief Add calculated value to circular array, pushing out oldest value + static void addSoCSample(float soc); + + /// @brief initialize the power state + static void initPowerState(void); + +public: + static void begin(void); + PowerManager(); + ~PowerManager(); + /// @brief Get the current free current budget (to C1 discharge) + /// @return the amount of power that is currently available (in mA) + static float getFreeLimitCurrentBudget(void); + /// @brief Get the current hard maximum free current (to C2 discharge) + /// @return the maximum amount of power that can be allocated (in mA) + static float getFreeMaximumCurrentBudget(void); + + /// @brief Request an allowance of a certain number of milliamperes from the + /// power scheduler without waiting for it (meaning it will not be scheduled + /// for future allocation). Only one can be active per consumer. + /// @param neededCurrent the amount of current we want to be accounted for + /// (in mA) + /// @return whether the current could be successfully allocated + static bool + tryAccquireCurrentAllowance(PowerParameters::PowerConsumers consumer, + uint16_t neededCurrent, + uint16_t requestedDurationMs = 0); + /// @brief "Return" the current currently allocated to a consumer + /// @param consumer the active consumer to release the current for + static void releaseCurrent(PowerParameters::PowerConsumers consumer); + + /// @brief Wait for a certain amount of current to be available. This will + /// "reseve a spot in the queue". Only one can be active per consumer. + /// @param neededCurrent the amount of power we want to be accounted for (in + /// mW) + /// @param TicksToWait the amount of time to wait for the power to become + /// available + /// @return whether the power could be successfully allocatedy + static bool waitForCurrentAllowance(PowerParameters::PowerConsumers consumer, + uint16_t neededCurrent, + uint16_t maxSlackTimeMs, + uint16_t requestedDurationMs); + /// @brief Put the ESP32 into deep sleep mode, without a method to wake up + /// again. Basically this is a shutdown. + static void beginPermanentDeepSleep(void); + + /// @brief Get currently granted current + /// @return the amount of current that is currently allocated (in mA) + static float getCurrentCurrent(void); + + /// @brief get the current theoretically flowing at the battery + /// @return the amount of current that is currently flowing (in mA) + static float getBatteryCurrent(void); + + /// @brief Responsible for recalculating the current budgets + /// @note these change based on the current state of charge + static void recalculateCurrentBudgets(void); + + // @brief Get current consumption of a consumer + static float getConsumerCurrent(PowerParameters::PowerConsumers consumer); + + /// @brief Get battery voltage measurement. + /// @return Battery Terminal Voltage in Volts + static float getBatteryVoltage(); + + /// @brief Get estimated battery charge state as percentage + /// @return Battery charge state in percent + static float getBatteryChargePercent(); + + /// @brief Get estimated battery charge state as percentage based on + // voltage directly + /// @return Battery charge state in percent + static float getBatteryVoltageChargePercent(); + + /// @brief Get estimated battery charge state as coulombs + /// @return Battery charge state in coulombs + static float getBatteryChargeCoulombs(); + + /// @brief get available current (after voltage conversion and efficiency + /// losses, referencing 1C discharge) + /// @return available current in milliamps + static float getMax3V3Current(); + + /// @brief update Power State + /// @note needs to be public for task creation + static void updatePowerStateHandler(); + + /// @brief dump power statistics to serial + static void dumpPowerStatistics(); + + /// @brief dump consumer statistics to serial + static void dumpConsumerStatistics(); + + /// @brief get wether power is supplied via USB + /// @return true if power is supplied via USB + static bool isUSBPowered(); + + /// @brief get wether power is supplied via battery + /// @return true if power is supplied via battery + static bool isBatteryPowered(); + + /// @brief get wether the battery is currently charging + /// @return true if the battery is charging + static bool isBatteryCharging(); + + /// @brief get wether the battery is currently discharging + /// @return true if the battery is discharging + static bool isBatteryDischarging(); + + /// @brief get wether the battery is currently fully charged + /// @return true if the battery is fully charged + static bool isBatteryFullyCharged(); + +}; + +extern PowerManager power; + +#endif // Power diff --git a/src/power/PowerParameters.h b/src/power/PowerParameters.h new file mode 100644 index 0000000..24ea9f0 --- /dev/null +++ b/src/power/PowerParameters.h @@ -0,0 +1,123 @@ +/** + * @file PowerParameters.h + * @author Phillip Kühne + * @brief + * @version 0.1 + * @date 2024-11-28 + * + * @copyright (c) 2024 + * + */ + +#ifndef PowerParameters_h +#define PowerParameters_h + +#include +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 \ No newline at end of file diff --git a/src/power/PowerScheduler.cpp b/src/power/PowerScheduler.cpp new file mode 100644 index 0000000..e2c6aa3 --- /dev/null +++ b/src/power/PowerScheduler.cpp @@ -0,0 +1,333 @@ +/** + * @file PowerScheduler.cpp + * @author Phillip Kühne + * @brief The actual power scheduler class, which keeps track of the power + * budget and allocates power to different components. + * @version 0.1 + * @date 2024-12-21 + * + * @copyright (c) 2024 + * + */ + +#include "PowerScheduler.h" +#include "PowerManager.h" + +bool PowerScheduler::tryAccquireCurrentAllowance( + PowerParameters::PowerConsumers consumer, float neededCurrent, + uint16_t requestedDurationMs) { + float existingConsumption = getConsumerCurrent(consumer); + const bool currentLimitNotExceeded = + this->freeLimitCurrentBudget + existingConsumption > 0; + const bool neededCurrentAvailableBelowMaximum = + this->freeMaximumCurrentBudget + existingConsumption >= neededCurrent; + const bool currentIsInsignificant = + neededCurrent < PowerParameters::CURRENT_INSIGNIFICANT; + if (currentIsInsignificant || + (currentLimitNotExceeded && neededCurrentAvailableBelowMaximum)) { + if (existingConsumption > 0) { + releaseCurrent(consumer); + } + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + ESP_LOGE( + TAG, + "Failed to Acquire PowerScheduler Mutex during Current Allocation"); + return false; + } + this->currentAllowances.push_back(PowerScheduler::CurrentAllowance{ + .consumer = consumer, + .maxSlackTimeMs = 0, + .requestedDurationMs = requestedDurationMs, + .taskHandle = xTaskGetCurrentTaskHandle(), + .neededCurrent = neededCurrent, + .requestedAt = xTaskGetTickCount(), + .grantedAt = xTaskGetTickCount(), + .granted = true}); + this->recalculateCurrentBudgets(); + lock.unlock(); + return true; + } else { + ESP_LOGI(TAG, + "Task %p denied %f mA of power;" + "currently allocated: %f mA;" + "total available (2C): %f mA", + xTaskGetCurrentTaskHandle(), neededCurrent, getCurrentCurrent(), + maximumCurrent); + return false; + } +} + +void PowerScheduler::releaseCurrent(PowerParameters::PowerConsumers consumer) { + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return; + } + for (auto it = currentAllowances.begin(); it != currentAllowances.end(); + ++it) { + if (it->consumer == consumer) { + currentAllowances.erase(it); + break; + } + } + lock.unlock(); + recalculateCurrentBudgets(); + // Check if there are tasks waiting for power + checkWaitingTasks(); +} + +bool PowerScheduler::waitForCurrentAllowance( + PowerParameters::PowerConsumers consumer, float neededCurrent, + uint16_t maxSlackTimeMs, uint16_t requestedDurationMs) { + if (tryAccquireCurrentAllowance(consumer, neededCurrent, + requestedDurationMs)) { + return true; + } else { + + // Suspend the task while waiting for power to be available + TaskHandle_t currentTask = xTaskGetCurrentTaskHandle(); + TickType_t initialTickCount = xTaskGetTickCount(); + PowerScheduler::CurrentAllowance newAllowance = { + .consumer = consumer, + .maxSlackTimeMs = maxSlackTimeMs, + .requestedDurationMs = requestedDurationMs, + .taskHandle = currentTask, + .neededCurrent = neededCurrent, + .requestedAt = initialTickCount, + .granted = false}; + PowerSchedulerMutex lock(powerSchedulerMutex); + if (lock.isLocked() == false) { + return false; + } + this->currentAllowances.push_back(newAllowance); + lock.unlock(); + uint32_t notificationValue; + BaseType_t notificationStatus = xTaskNotifyWait( + 0, 0, ¬ificationValue, 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(consumer), + xTaskGetTickCount() - initialTickCount); + return true; + } else { + // Still not enough power available for us. Wait the remaining ticks. + xTaskNotifyWait(0, 0, ¬ificationValue, + 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 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(); + this->powerSchedulerMutex = xSemaphoreCreateMutex(); +} + +PowerScheduler::~PowerScheduler() { + if (powerSchedulerMutex != NULL) { + vSemaphoreDelete(powerSchedulerMutex); + } +} diff --git a/src/power/PowerScheduler.h b/src/power/PowerScheduler.h new file mode 100644 index 0000000..c0e93aa --- /dev/null +++ b/src/power/PowerScheduler.h @@ -0,0 +1,173 @@ +/** + * @file PowerScheduler.hpp + * @author Phillip Kühne + * @brief The actual power scheduler class, which keeps track of the power + * budget and allocates power to different components. + * @version 0.1 + * @date 2024-12-21 + * + * @copyright (c) 2024 + * + */ + +#include "PowerParameters.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include +#include + +#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 currentAllowances; +}; + +static PowerScheduler *powerSchedulerInstance; + +#endif // PowerScheduler_h \ No newline at end of file