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 #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(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