From 60314ec101750e587ba39bb5ba92d811c8814b44 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Sun, 12 May 2024 09:56:42 +0200 Subject: [PATCH] change motion from analogWrite to ledc, add fluent interface for motors --- src/motion/Motion.cpp | 49 +++++++++++++++++++++++++------------------ src/motion/Motion.h | 34 ++++++++++++++++++++++++++---- src/motion/Motor.cpp | 33 +++++++++++++++++++++++++++++ 3 files changed, 92 insertions(+), 24 deletions(-) create mode 100644 src/motion/Motor.cpp diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index e26df10..ceff927 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -2,7 +2,7 @@ * @file Motion.cpp * @author Jonathan Schulze, Nick Hübenthal, Hans Haupt * @brief Implementation of the Motion class. - * @version 0.1 + * @version 0.2 * @date 2023-12-13 * * @copyright Copyright (c) 2023 @@ -17,14 +17,23 @@ TaskHandle_t xAntiClockwiseTaskHandle = NULL; // Initialize the movement component. void Motion::begin(void) { - + ledc_timer_config_t motor_timer = { + .speed_mode = LEDC_MODE, + .duty_resolution = DUTY_RES, + .timer_num = TIMER, + .freq_hz = FREQUENCY, + .clk_cfg = LEDC_AUTO_CLK + }; + ledc_timer_config(&motor_timer); + Motion::left.begin(); + Motion::right.begin(); }; void Motion::moveTask(void * args) { - analogWrite(MOTOR_LEFT_PIN, LEFT_MOTOR_DUTY); - analogWrite(MOTOR_RIGHT_PIN, RIGHT_MOTOR_DUTY); + Motion::left.setSpeed(LEFT_MOTOR_DUTY); + Motion::right.setSpeed(RIGHT_MOTOR_DUTY); vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); - analogWrite(MOTOR_LEFT_PIN, 0); - analogWrite(MOTOR_RIGHT_PIN, 0); + Motion::left.setSpeed(0); + Motion::right.setSpeed(0); vTaskDelete(xMoveTaskHandle); }; @@ -33,16 +42,16 @@ void Motion::move(uint32_t moveForMs) { if (moveForMs > 0){ xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle); } else{ - analogWrite(MOTOR_LEFT_PIN, LEFT_MOTOR_DUTY); - analogWrite(MOTOR_RIGHT_PIN, RIGHT_MOTOR_DUTY); + Motion::left.setSpeed(4096); + Motion::right.setSpeed(4096); } }; void Motion::leftMotorTask(void * args) { - analogWrite(MOTOR_LEFT_PIN, LEFT_MOTOR_DUTY); - analogWrite(MOTOR_RIGHT_PIN, 0); + Motion::left.setSpeed(LEFT_MOTOR_DUTY); + Motion::right.setSpeed(0); vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); - analogWrite(MOTOR_LEFT_PIN, 0); + Motion::left.setSpeed(0); vTaskDelete(xClockwiseTaskHandle); }; @@ -51,16 +60,16 @@ void Motion::rotateClockwise(uint32_t rotateForMs) { if (rotateForMs > 0){ xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle); } else { - analogWrite(MOTOR_LEFT_PIN,LEFT_MOTOR_DUTY); - analogWrite(MOTOR_RIGHT_PIN,0); + Motion::left.setSpeed(LEFT_MOTOR_DUTY); + Motion::right.setSpeed(0); } }; void Motion::rightMotorTask(void * args) { - analogWrite(MOTOR_RIGHT_PIN, RIGHT_MOTOR_DUTY); - analogWrite(MOTOR_LEFT_PIN,0); + Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + Motion::left.setSpeed(0); vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); - analogWrite(MOTOR_RIGHT_PIN, 0); + Motion::right.setSpeed(0); vTaskDelete(xAntiClockwiseTaskHandle); }; @@ -69,13 +78,13 @@ void Motion::rotateAntiClockwise(uint32_t rotateForMs) { if(rotateForMs > 0){ xTaskCreate(rightMotorTask, "RightMotor", 4096, (void*)rotateForMs, 10, &xAntiClockwiseTaskHandle); } else { - analogWrite(MOTOR_RIGHT_PIN,RIGHT_MOTOR_DUTY); - analogWrite(MOTOR_LEFT_PIN,0); + Motion::right.setSpeed(RIGHT_MOTOR_DUTY); + Motion::left.setSpeed(0); } }; void Motion::stop(void){ - analogWrite(MOTOR_LEFT_PIN,0); - analogWrite(MOTOR_RIGHT_PIN,0); + Motion::left.setSpeed(0); + Motion::right.setSpeed(0); } diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 6fa3f66..13ae523 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -1,8 +1,8 @@ /** * @file Motion.h - * @author Jonathan Schulze, Nick Hübenthal + * @author Jonathan Schulze, Nick Hübenthal, Hans Haupt * @brief This component controls the ability to rotate and change position. - * @version 0.1 + * @version 0.2 * @date 2023-12-13 * * @copyright Copyright (c) 2023 @@ -15,11 +15,31 @@ #include #include #include +#include "driver/ledc.h" +#define LEDC_MODE LEDC_LOW_SPEED_MODE +#define TIMER LEDC_TIMER_2 +#define CHANNEL_LEFT LEDC_CHANNEL_3 +#define CHANNEL_RIGHT LEDC_CHANNEL_4 +#define DUTY_RES LEDC_TIMER_13_BIT // Set duty resolution to 13 bits +#define FREQUENCY (5000) // Frequency in Hertz. Set frequency at 5 kHz +class Motor{ + public: + Motor(uint8_t pin, ledc_timer_t timer, ledc_channel_t channel); + void begin(void); + void setSpeed(uint16_t duty); + uint16_t getSpeed(void); + protected: + uint8_t pin; + ledc_timer_t timer; + ledc_channel_t channel; + + uint16_t duty; +}; class Motion{ protected: - static const uint8_t RIGHT_MOTOR_DUTY = 128; - static const uint8_t LEFT_MOTOR_DUTY = 128; + static const uint16_t RIGHT_MOTOR_DUTY = 4096; + static const uint16_t LEFT_MOTOR_DUTY = 4096; static const int MOTOR_RIGHT_PIN = 11; static const int MOTOR_LEFT_PIN = 12; static void moveTask(void * args); @@ -27,6 +47,10 @@ protected: static void rightMotorTask(void * args); public: + //Shared Timer to sync movement + static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT); + static inline Motor right = Motor(MOTOR_RIGHT_PIN,TIMER,CHANNEL_RIGHT); + /** * @brief Initialize the movement component. * @@ -61,4 +85,6 @@ public: static void stop(void); }; + + #endif //Motion_h \ No newline at end of file diff --git a/src/motion/Motor.cpp b/src/motion/Motor.cpp new file mode 100644 index 0000000..4dfc3a4 --- /dev/null +++ b/src/motion/Motor.cpp @@ -0,0 +1,33 @@ +#include "Motion.h" + +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::setSpeed(uint16_t duty){ + this->duty = duty; + ledc_set_duty(LEDC_MODE,this->channel,duty); + ledc_update_duty(LEDC_MODE,this->channel); +}; + +uint16_t Motor::getSpeed(void){ + return this->duty; +};