From a26acf4a927942c53215e2a83fbf5cba86295ac1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Phillip=20K=C3=BChne?= <phillip.kuehne@stud.htwk-leipzig.de>
Date: Wed, 12 Feb 2025 21:50:29 +0100
Subject: [PATCH] Integrate Motors into Power Management

---
 src/motion/Motion.h  |  5 ++++-
 src/motion/Motor.cpp | 10 ++++++++++
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/motion/Motion.h b/src/motion/Motion.h
index daf70c0..bb26a72 100644
--- a/src/motion/Motion.h
+++ b/src/motion/Motion.h
@@ -17,7 +17,7 @@
 #include <freertos/task.h>
 #include "driver/ledc.h"
 #include "motionDetection/MotionDetection.h"
-#include "power/Power.h"
+#include "../power/Power.h"
 #define LEDC_MODE          LEDC_LOW_SPEED_MODE
 #define TIMER              LEDC_TIMER_2
 #define CHANNEL_LEFT       LEDC_CHANNEL_3 
@@ -25,6 +25,9 @@
 #define DUTY_RES           LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
 #define FREQUENCY          (5000) // Frequency in Hertz. Set frequency at 5 kHz
 #define DEFAULT_BASE_VALUE  3900
+
+#define MOTOR_MAX_EXECUTION_DELAY_MS 100
+
 class Motor{
     public:
         Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel);
diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp
index d6c68e3..a5c4e2b 100644
--- a/src/motion/Motor.cpp
+++ b/src/motion/Motor.cpp
@@ -1,6 +1,9 @@
 #include "Motion.h"
 #include "power/Power.h"
 
+#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;
@@ -24,6 +27,13 @@ void Motor::begin(void){
 };
 
 void 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;
+    if (this->pin == MOTOR_LEFT_PIN){
+        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_LEFT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+    } else {
+        Power::waitForCurrentAllowance(PowerParameters::PowerConsumers::MOTOR_RIGHT, current, MOTOR_MAX_EXECUTION_DELAY_MS, NULL);
+    }
     int difference = duty-this->getSpeed();
     if (difference > 0){
         for(int i = 0;i<difference;i+=difference/20){