From 0d6977e1484c033ecef27ab365946e101a4187b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Phillip=20K=C3=BChne?= Date: Thu, 13 Feb 2025 22:57:55 +0100 Subject: [PATCH] Improve power denial handling --- src/motion/Motion.cpp | 26 +++++++--- src/motion/Motion.h | 4 +- src/motion/Motor.cpp | 117 +++++++++++++++++++++--------------------- 3 files changed, 81 insertions(+), 66 deletions(-) diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 2448f04..0db820c 100644 --- a/src/motion/Motion.cpp +++ b/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); - Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + if (!Motion::left.setSpeed(LEFT_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); - 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 +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){ diff --git a/src/motion/Motion.h b/src/motion/Motion.h index fcc57b4..2c7c0bd 100644 --- a/src/motion/Motion.h +++ b/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 diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp index 74275f7..54b5bf6 100644 --- a/src/motion/Motor.cpp +++ b/src/motion/Motor.cpp @@ -1,72 +1,71 @@ #include "Motion.h" #include "power/Power.h" -#define MOTOR_LEFT_PIN 12 +#define MOTOR_LEFT_PIN 12 #define MOTOR_RIGHT_PIN 11 -Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){ - this->pin = pin; - this->channel = channel; - this->timer = timer; - this->duty = 0; +Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel) { + this->pin = pin; + this->channel = channel; + this->timer = timer; + this->duty = 0; }; -void Motor::begin(void){ - pinMode(this->pin,OUTPUT); - ledc_channel_config_t channelConfig = { - .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 - }; - ledc_channel_config(&channelConfig); - Serial.println("Motor begin done"); +void Motor::begin(void) { + pinMode(this->pin, OUTPUT); + ledc_channel_config_t channelConfig = {.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}; + ledc_channel_config(&channelConfig); + Serial.println("Motor begin done"); }; -void Motor::setSpeed(uint16_t duty){ - const float dutyFactor = duty / static_cast(1 << DUTY_RES); - const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor; - if (this->pin == MOTOR_LEFT_PIN) { - if (!Power::waitForCurrentAllowance( - PowerParameters::PowerConsumers::MOTOR_LEFT, current, - MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) { - ESP_LOGW(TAG, - "Power to set LEFT MOTOR to speed %d not granted in time. " - "Skipping.", - duty); - } - } else { - if (!Power::waitForCurrentAllowance( - PowerParameters::PowerConsumers::MOTOR_RIGHT, current, - MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) { - ESP_LOGW(TAG, - "Power to set RIGHT MOTOR to speed %d not granted in time. " - "Skipping.", - duty); - } +bool Motor::setSpeed(uint16_t duty) { + const float dutyFactor = duty / static_cast(1 << DUTY_RES); + const float current = + PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor; + if (this->pin == MOTOR_LEFT_PIN) { + if (!Power::waitForCurrentAllowance( + PowerParameters::PowerConsumers::MOTOR_LEFT, current, + MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) { + ESP_LOGW(TAG, + "Power to set LEFT MOTOR to speed %d not granted in time. " + "Skipping.", + duty); + return false; } - 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); - } + } else { + if (!Power::waitForCurrentAllowance( + PowerParameters::PowerConsumers::MOTOR_RIGHT, current, + MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) { + ESP_LOGW(TAG, + "Power to set RIGHT MOTOR to speed %d not granted in time. " + "Skipping.", + duty); + 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; };