phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-10-31 07:10:00 +01:00
Code Issues Packages Projects Releases Wiki Activity

Improve power denial handling

Browse Source
This commit is contained in:
Phillip Kühne
2025-02-13 22:57:55 +01:00
parent 35c11f42e2
commit 0d6977e148
3 changed files with 81 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/motion/Motion.cpp
View File

@@ -31,8 +31,11 @@ void Motion::begin(void) {
void Motion::moveTask(void * args) { void Motion::moveTask(void * args) {
uint32_t runtime = (uint32_t)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(); Motion::xLastWakeTime = xTaskGetTickCount();
while(1){ while(1){
if(runtime>40||runtime==0){ if(runtime>40||runtime==0){
@@ -72,8 +75,11 @@ void Motion::moveTask(void * args) {
RIGHT_MOTOR_DUTY+=changerate; 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 { } else {
vTaskDelayUntil(&xLastWakeTime,runtime); vTaskDelayUntil(&xLastWakeTime,runtime);
Motion::left.setSpeed(0); Motion::left.setSpeed(0);
@@ -116,7 +122,11 @@ void Motion::leftMotorTask(void * args) {
xAntiClockwiseTaskHandle = NULL; xAntiClockwiseTaskHandle = NULL;
} }
Motion::right.setSpeed(0); 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){ while(1){
if((runtime>40)||(runtime==0)){ if((runtime>40)||(runtime==0)){
vTaskDelayUntil(&xLastWakeTime,40); vTaskDelayUntil(&xLastWakeTime,40);
@@ -155,7 +165,11 @@ void Motion::rightMotorTask(void * args) {
vTaskDelete(xClockwiseTaskHandle); vTaskDelete(xClockwiseTaskHandle);
xClockwiseTaskHandle = NULL; 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); Motion::left.setSpeed(0);
while(1){ while(1){
if(runtime>40||runtime==0){ if(runtime>40||runtime==0){

4
src/motion/Motion.h
View File

@@ -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. * @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 * @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 * @brief returns the currently activ speed

117
src/motion/Motor.cpp
View File

@@ -1,72 +1,71 @@
#include "Motion.h" #include "Motion.h"
#include "power/Power.h" #include "power/Power.h"
#define MOTOR_LEFT_PIN 12 #define MOTOR_LEFT_PIN 12
#define MOTOR_RIGHT_PIN 11 #define MOTOR_RIGHT_PIN 11
Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel){ Motor::Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel) {
this->pin = pin; this->pin = pin;
this->channel = channel; this->channel = channel;
this->timer = timer; this->timer = timer;
this->duty = 0; this->duty = 0;
}; };
void Motor::begin(void){ void Motor::begin(void) {
pinMode(this->pin,OUTPUT); 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,
.speed_mode = LEDC_MODE, .channel = this->channel,
.channel = this->channel, .intr_type = LEDC_INTR_DISABLE,
.intr_type = LEDC_INTR_DISABLE, .timer_sel = this->timer,
.timer_sel = this->timer, .duty = 0, // Set duty to 0%
.duty = 0, // Set duty to 0% .hpoint = 0};
.hpoint = 0 ledc_channel_config(&channelConfig);
}; Serial.println("Motor begin done");
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 dutyFactor = duty / static_cast<float>(1 << DUTY_RES);
const float current = PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor; const float current =
if (this->pin == MOTOR_LEFT_PIN) { PowerParameters::CurrentConsumptions::CURRENT_MOTOR_T_ON * dutyFactor;
if (!Power::waitForCurrentAllowance( if (this->pin == MOTOR_LEFT_PIN) {
PowerParameters::PowerConsumers::MOTOR_LEFT, current, if (!Power::waitForCurrentAllowance(
MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) { PowerParameters::PowerConsumers::MOTOR_LEFT, current,
ESP_LOGW(TAG, MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
"Power to set LEFT MOTOR to speed %d not granted in time. " ESP_LOGW(TAG,
"Skipping.", "Power to set LEFT MOTOR to speed %d not granted in time. "
duty); "Skipping.",
} duty);
} else { return false;
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);
}
} }
int difference = duty-this->getSpeed(); } else {
if (difference > 0){ if (!Power::waitForCurrentAllowance(
for(int i = 0;i<difference;i+=difference/20){ PowerParameters::PowerConsumers::MOTOR_RIGHT, current,
this->duty += difference/20; MOTOR_MAX_EXECUTION_DELAY_MS, NULL)) {
ledc_set_duty(LEDC_MODE,this->channel,duty); ESP_LOGW(TAG,
ledc_update_duty(LEDC_MODE,this->channel); "Power to set RIGHT MOTOR to speed %d not granted in time. "
delayMicroseconds(5); "Skipping.",
} duty);
} else { return false;
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);
}
} }
}
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){ uint16_t Motor::getSpeed(void) { return this->duty; };
return this->duty;
};