From c8f7e953635699af58d0753cbc9bab884189dfd1 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Sun, 28 Apr 2024 22:38:35 +0200 Subject: [PATCH] introduced possibility to run move commands without timelimit --- example/example.ino | 18 ---------- example/example/example.ino | 19 ++++++++++ example/{ => motion}/motion.ino | 0 example/plain.cpp | 5 --- src/motion/Motion.cpp | 62 +++++++++++++++++++++------------ src/motion/Motion.h | 29 +++++++++------ 6 files changed, 78 insertions(+), 55 deletions(-) delete mode 100644 example/example.ino create mode 100644 example/example/example.ino rename example/{ => motion}/motion.ino (100%) delete mode 100644 example/plain.cpp diff --git a/example/example.ino b/example/example.ino deleted file mode 100644 index f055251..0000000 --- a/example/example.ino +++ /dev/null @@ -1,18 +0,0 @@ -#include - -Dezibot dezibot = Dezibot(); -const uint8_t MYFOO = 10; -void setup() { - dezibot.begin(); - -} - -void loop() { -dezibot.multiColorLight.setLed(TOP_LEFT,0x000000FF); -dezibot.multiColorLight.setLed(TOP_RIGHT,dezibot.multiColorLight.color(0,100,0)); -dezibot.multiColorLight.blink(10,0x00FF0000,BOTTOM,500); - -delay(1000); -dezibot.multiColorLight.turnOff(ALL); -delay(1000); -} diff --git a/example/example/example.ino b/example/example/example.ino new file mode 100644 index 0000000..e376753 --- /dev/null +++ b/example/example/example.ino @@ -0,0 +1,19 @@ +#include + +Dezibot dezibot = Dezibot(); +const uint8_t MYFOO = 10; +void setup() { + dezibot.begin(); + +} + +void loop() { +dezibot.motion.move(); +delay(1000); +dezibot.motion.rotateAntiClockwise(); +delay(1000); +dezibot.motion.rotateClockwise(); +delay(1000); +dezibot.motion.stop(); +delay(1000); +} diff --git a/example/motion.ino b/example/motion/motion.ino similarity index 100% rename from example/motion.ino rename to example/motion/motion.ino diff --git a/example/plain.cpp b/example/plain.cpp deleted file mode 100644 index c8fad0c..0000000 --- a/example/plain.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "Dezibot.h" - -int main(){ - return 0; -} \ No newline at end of file diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 5244d24..e26df10 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -1,6 +1,6 @@ /** * @file Motion.cpp - * @author Jonathan Schulze, Nick Hübenthal + * @author Jonathan Schulze, Nick Hübenthal, Hans Haupt * @brief Implementation of the Motion class. * @version 0.1 * @date 2023-12-13 @@ -15,49 +15,67 @@ TaskHandle_t xMoveTaskHandle = NULL; TaskHandle_t xClockwiseTaskHandle = NULL; TaskHandle_t xAntiClockwiseTaskHandle = NULL; -// Constructor -Motion::Motion() { - -} - // Initialize the movement component. void Motion::begin(void) { -} -void moveTask(void * args) { - analogWrite(MOTOR_LEFT_PIN, 128); - analogWrite(MOTOR_RIGHT_PIN, 128); +}; +void Motion::moveTask(void * args) { + analogWrite(MOTOR_LEFT_PIN, LEFT_MOTOR_DUTY); + analogWrite(MOTOR_RIGHT_PIN, RIGHT_MOTOR_DUTY); vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); analogWrite(MOTOR_LEFT_PIN, 0); analogWrite(MOTOR_RIGHT_PIN, 0); vTaskDelete(xMoveTaskHandle); -} +}; // Move forward for a certain amount of time. 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); + } +}; -void leftMotorTask(void * args) { - analogWrite(MOTOR_LEFT_PIN, 128); +void Motion::leftMotorTask(void * args) { + analogWrite(MOTOR_LEFT_PIN, LEFT_MOTOR_DUTY); + analogWrite(MOTOR_RIGHT_PIN, 0); vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); analogWrite(MOTOR_LEFT_PIN, 0); vTaskDelete(xClockwiseTaskHandle); -} +}; // Rotate clockwise for a certain amount of time. void Motion::rotateClockwise(uint32_t rotateForMs) { - xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle); -} + if (rotateForMs > 0){ + xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle); + } else { + analogWrite(MOTOR_LEFT_PIN,LEFT_MOTOR_DUTY); + analogWrite(MOTOR_RIGHT_PIN,0); + } +}; -void rightMotorTask(void * args) { - analogWrite(MOTOR_RIGHT_PIN, 128); +void Motion::rightMotorTask(void * args) { + analogWrite(MOTOR_RIGHT_PIN, RIGHT_MOTOR_DUTY); + analogWrite(MOTOR_LEFT_PIN,0); vTaskDelay((uint32_t) args / portTICK_PERIOD_MS); analogWrite(MOTOR_RIGHT_PIN, 0); vTaskDelete(xAntiClockwiseTaskHandle); -} +}; // Rotate anticlockwise for a certain amount of time. -void Motion::rotateAnticlockwise(uint32_t rotateForMs) { - xTaskCreate(rightMotorTask, "RightMotor", 4096, (void*)rotateForMs, 10, &xAntiClockwiseTaskHandle); +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); + } +}; + +void Motion::stop(void){ + analogWrite(MOTOR_LEFT_PIN,0); + analogWrite(MOTOR_RIGHT_PIN,0); } + diff --git a/src/motion/Motion.h b/src/motion/Motion.h index eb1eccc..6fa3f66 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -16,17 +16,17 @@ #include #include -const int MOTOR_RIGHT_PIN = 11; -const int MOTOR_LEFT_PIN = 12; - - class Motion{ protected: + static const uint8_t RIGHT_MOTOR_DUTY = 128; + static const uint8_t LEFT_MOTOR_DUTY = 128; + static const int MOTOR_RIGHT_PIN = 11; + static const int MOTOR_LEFT_PIN = 12; + static void moveTask(void * args); + static void leftMotorTask(void * args); + static void rightMotorTask(void * args); public: - - Motion(); - /** * @brief Initialize the movement component. * @@ -35,21 +35,30 @@ public: /** * @brief Move forward for a certain amount of time. + * Call with moveForMs 0 will start movement, that must be stopped explicit by call to stop(). * @param moveForMs Representing the duration of forward moving in milliseconds. */ - void move(uint32_t moveForMs); + static void move(uint32_t moveForMs=0); /** * @brief Rotate clockwise for a certain amount of time. + * Call with moveForMs 0 will start movement, that must be stopped explicit by call to stop(). * @param rotateForMs Representing the duration of rotating clockwise in milliseconds. */ - void rotateClockwise(uint32_t rotateForMs); + static void rotateClockwise(uint32_t rotateForMs=0); /** * @brief Rotate anticlockwise for a certain amount of time. + * Call with moveForMs 0 will start movement, that must be stopped explicit by call to stop(). * @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds. */ - void rotateAnticlockwise(uint32_t rotateForMs); + static void rotateAntiClockwise(uint32_t rotateForMs=0); + + /** + * @brief stops any current movement, no matter if timebased or endless + * + */ + static void stop(void); }; #endif //Motion_h \ No newline at end of file