Improve power denial handling

src/motion/Motion.cpp

@@ -31,8 +31,11 @@ void Motion::begin(void) {
 void Motion::moveTask(void * args) {
     uint32_t runtime = (uint32_t)args;
 
-    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+    if (!Motion::left.setSpeed(LEFT_MOTOR_DUTY) || !Motion::right.setSpeed(RIGHT_MOTOR_DUTY)) {
-    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+        Serial.println("ailed 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 +75,11 @@ void Motion::moveTask(void * args) {
                 RIGHT_MOTOR_DUTY+=changerate;
             } 
 
-            Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+            bool leftSpeedchangeSuccess = Motion::left.setSpeed(LEFT_MOTOR_DUTY);
-            Motion::right.setSpeed(RIGHT_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 +122,11 @@ void Motion::leftMotorTask(void * args) {
         xAntiClockwiseTaskHandle = NULL;
     }
     Motion::right.setSpeed(0);
-    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+    if(!Motion::left.setSpeed(LEFT_MOTOR_DUTY)){
+        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 +165,11 @@ void Motion::rightMotorTask(void * args) {
         vTaskDelete(xClockwiseTaskHandle);
         xClockwiseTaskHandle = NULL;
     }
-    Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+    if(!Motion::left.setSpeed(RIGHT_MOTOR_DUTY)){
+        Serial.println("Can not rotate Antilockwise due to power constraints");
+        Motion::stop();
+        return;
+    }
     Motion::left.setSpeed(0);
     while(1){
         if(runtime>40||runtime==0){

src/motion/Motion.h

@@ -45,8 +45,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

src/motion/Motor.cpp

@@ -13,22 +13,21 @@ Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){
 
 void Motor::begin(void) {
   pinMode(this->pin, OUTPUT);
-    ledc_channel_config_t channelConfig = {
+  ledc_channel_config_t channelConfig = {.gpio_num = this->pin,
-        .gpio_num       = this->pin,
                                          .speed_mode = LEDC_MODE,
                                          .channel = this->channel,
                                          .intr_type = LEDC_INTR_DISABLE,
                                          .timer_sel = this->timer,
                                          .duty = 0, // Set duty to 0%
-        .hpoint         = 0
+                                         .hpoint = 0};
-    };
   ledc_channel_config(&channelConfig);
   Serial.println("Motor begin done");
 };
 
-void Motor::setSpeed(uint16_t duty){
+bool Motor::setSpeed(uint16_t duty) {
   const float dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
-    const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
+  const float current =
+      PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
   if (this->pin == MOTOR_LEFT_PIN) {
     if (!Power::waitForCurrentAllowance(
             PowerParameters::PowerConsumers::MOTOR_LEFT, current,
@@ -37,6 +36,7 @@ void Motor::setSpeed(uint16_t duty){
                "Power to set LEFT MOTOR to speed %d not granted in time. "
                "Skipping.",
                duty);
+      return false;
     }
   } else {
     if (!Power::waitForCurrentAllowance(
@@ -46,6 +46,7 @@ void Motor::setSpeed(uint16_t duty){
                "Power to set RIGHT MOTOR to speed %d not granted in time. "
                "Skipping.",
                duty);
+      return false;
     }
   }
   int difference = duty - this->getSpeed();
@@ -64,9 +65,7 @@ void Motor::setSpeed(uint16_t duty){
       delayMicroseconds(5);
     }
   }
-
+  return true;
 };
 
-uint16_t Motor::getSpeed(void){
+uint16_t Motor::getSpeed(void) { return this->duty; };
-    return this->duty;
-};