From 60175861db3e8b5af354b90905c9f4c039a4e9cb Mon Sep 17 00:00:00 2001 From: Hans Haupt Date: Tue, 5 Dec 2023 22:04:59 +0000 Subject: [PATCH 01/14] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 02d8428..479be71 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,8 @@ To allow easy usability for users not familier with C++, prevent passing around * folders containing components are named in lowerCamelCase * methods are named in lowerCamelCase * constants are named in ALL_CAPS_SNAKE_CASE +* Values of Enums follow the conventions of constants + ### Bytestream Every class that implements Byte-Based Communication needs to implement the Arduino Streaminterface ### Components From 68883dadb8c3a9de2cb2e1f3218ec2bcb0072cb4 Mon Sep 17 00:00:00 2001 From: n1i9c9k9 Date: Wed, 13 Dec 2023 12:29:44 +0100 Subject: [PATCH 02/14] functions to implement --- src/motion/Motion.h | 43 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 99cb603..16353d8 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -1,7 +1,48 @@ +/** + * @file Motion.h + * @author Jonathan Schulze, Nick Hübenthal + * @brief This component controls the ability to rotate and change position. + * @version 0.1 + * @date 2023-12-13 + * + * @copyright Copyright (c) 2023 + * + */ + #ifndef Motion_h #define Motion_h class Motion{ +protected: + +public: + Motion(); + + /** + * @brief Initialize the movement component. + * + */ + void begin(void); + + /** + * @brief Move forward for a certain amount of time. + * @param moveForMs Representing the duration of forward moving in milliseconds. + */ + void move(u8int moveForMs); + + /** + * @brief Rotate left for a certain amount of time. + * @param rotateForMs Representing the duration of rotating left in milliseconds. + */ + void rotateLeft(u8int rotateForMs); + + /** + * @brief Rotate right for a certain amount of time. + * @param rotateForMs Representing the duration of rotating right in milliseconds. + */ + void rotateRight(u8int rotateForMs); }; -#endif //Motion_h \ No newline at end of file +#endif //Motion_h + + From 0d1854c8d7db2ca6be588117d4639d29290f8669 Mon Sep 17 00:00:00 2001 From: n1i9c9k9 Date: Wed, 13 Dec 2023 17:12:02 +0100 Subject: [PATCH 03/14] suggestion for motion.h --- src/motion/Motion.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 16353d8..7199ef7 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -28,21 +28,19 @@ public: * @brief Move forward for a certain amount of time. * @param moveForMs Representing the duration of forward moving in milliseconds. */ - void move(u8int moveForMs); + void move(uint8_t moveForMs); /** * @brief Rotate left for a certain amount of time. * @param rotateForMs Representing the duration of rotating left in milliseconds. */ - void rotateLeft(u8int rotateForMs); + void rotateLeft(uint8_t rotateForMs); /** * @brief Rotate right for a certain amount of time. * @param rotateForMs Representing the duration of rotating right in milliseconds. */ - void rotateRight(u8int rotateForMs); + void rotateRight(uint8_t rotateForMs); }; -#endif //Motion_h - - +#endif //Motion_h \ No newline at end of file From 32f82557b3f2dadb377afd5a6c4df3393287506d Mon Sep 17 00:00:00 2001 From: n1i9c9k9 Date: Wed, 3 Jan 2024 11:37:58 +0100 Subject: [PATCH 04/14] changed int to 32 bits and method names --- src/motion/Motion.h | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 7199ef7..e0edb66 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -11,6 +11,8 @@ #ifndef Motion_h #define Motion_h +#include + class Motion{ protected: @@ -28,19 +30,19 @@ public: * @brief Move forward for a certain amount of time. * @param moveForMs Representing the duration of forward moving in milliseconds. */ - void move(uint8_t moveForMs); + void move(uint32_t moveForMs); /** - * @brief Rotate left for a certain amount of time. - * @param rotateForMs Representing the duration of rotating left in milliseconds. + * @brief Rotate clockwise for a certain amount of time. + * @param rotateForMs Representing the duration of rotating clockwise in milliseconds. */ - void rotateLeft(uint8_t rotateForMs); - + void rotateClockwise(uint32_t rotateForMs); + /** - * @brief Rotate right for a certain amount of time. - * @param rotateForMs Representing the duration of rotating right in milliseconds. + * @brief Rotate anticlockwise for a certain amount of time. + * @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds. */ - void rotateRight(uint8_t rotateForMs); + void rotateAnticlockwise(uint32_t rotateForMs); }; #endif //Motion_h \ No newline at end of file From 4165afd578f098450bed3b71407c58a50c8b1888 Mon Sep 17 00:00:00 2001 From: 99cardz Date: Mon, 8 Jan 2024 15:37:39 +0100 Subject: [PATCH 05/14] implement move method --- src/motion/Motion.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/motion/Motion.h b/src/motion/Motion.h index e0edb66..7348769 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -13,11 +13,15 @@ #define Motion_h #include +const int MOTOR_RIGHT_PIN = 11; +const int MOTOR_LEFT_PIN = 12; + class Motion{ protected: public: + Motion(); /** From 8fa5de4b12f8927e8e7589b2997bb3c3af2ef52f Mon Sep 17 00:00:00 2001 From: 99cardz Date: Mon, 8 Jan 2024 15:37:52 +0100 Subject: [PATCH 06/14] implement move method --- src/motion/Motion.cpp | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 src/motion/Motion.cpp diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp new file mode 100644 index 0000000..028aed3 --- /dev/null +++ b/src/motion/Motion.cpp @@ -0,0 +1,41 @@ +/** + * @file Motion.cpp + * @author Jonathan Schulze, Nick Hübenthal + * @brief Implementation of the Motion class. + * @version 0.1 + * @date 2023-12-13 + * + * @copyright Copyright (c) 2023 + * + */ + +#include "Motion.h" + +// Constructor +Motion::Motion() { + +} + +// Initialize the movement component. +void Motion::begin(void) { + +} + +// Move forward for a certain amount of time. +void Motion::move(uint32_t moveForMs) { + analogWrite(MOTOR_LEFT_PIN, 128); + analogWrite(MOTOR_RIGHT_PIN, 128); + vTaskDelay(moveForMs); + analogWrite(MOTOR_LEFT_PIN, 0); + analogWrite(MOTOR_RIGHT_PIN, 0); +} + +// Rotate clockwise for a certain amount of time. +void Motion::rotateClockwise(uint32_t rotateForMs) { + +} + +// Rotate anticlockwise for a certain amount of time. +void Motion::rotateAnticlockwise(uint32_t rotateForMs) { + +} From f2253a7cb5e940e4707d35706b26a66e7298cc0f Mon Sep 17 00:00:00 2001 From: 99cardz Date: Mon, 8 Jan 2024 15:44:36 +0100 Subject: [PATCH 07/14] added Arduino and vtask imports --- src/motion/Motion.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 7348769..eb1eccc 100644 --- a/src/motion/Motion.h +++ b/src/motion/Motion.h @@ -12,6 +12,9 @@ #ifndef Motion_h #define Motion_h #include +#include +#include +#include const int MOTOR_RIGHT_PIN = 11; const int MOTOR_LEFT_PIN = 12; From e42058bb82f6d111c284cd129f2253fccd052e14 Mon Sep 17 00:00:00 2001 From: 99cardz Date: Mon, 8 Jan 2024 16:26:56 +0100 Subject: [PATCH 08/14] experimenting with xTaskCreate --- src/Dezibot.cpp | 54 +------------------------------------------ src/motion/Motion.cpp | 12 ++++++---- 2 files changed, 9 insertions(+), 57 deletions(-) diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp index 3285444..38eb327 100644 --- a/src/Dezibot.cpp +++ b/src/Dezibot.cpp @@ -26,57 +26,5 @@ Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); #define GPIO_LED 48 void Dezibot::begin(void) { - Adafruit_NeoPixel ledStrip = Adafruit_NeoPixel(3, GPIO_LED, NEO_GRB + NEO_KHZ800); - Wire.begin(1, 2); - if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { - Serial.println("SSD1306 allocation failed"); - for (;;); // Don't proceed, loop forever - } - - display.display(); - vTaskDelay(2000); - display.clearDisplay(); - vTaskDelay(2000); - - // Draw a single pixel in white - display.drawPixel(10, 10, SSD1306_WHITE); - - // Show the display buffer on the screen. You MUST call display() after - // drawing commands to make them visible on screen! - display.display(); - vTaskDelay(2000); - - Serial.begin(9600); - Serial.println("start"); - - - vTaskDelay(1000); - - while (1) { - /* Blink off (output low) */ - ledStrip.setPixelColor(1, ledStrip.Color(100, 100, 100)); - ledStrip.show(); // Aktualisiere die Farbe des Pixels - vTaskDelay(1000); - /* Blink on (output high) */ - ledStrip.setPixelColor(1, ledStrip.Color(0, 0, 0)); - ledStrip.show(); // Aktualisiere die Farbe des Pixels - vTaskDelay(1000); - - struct timeval tv_now; - gettimeofday(&tv_now, NULL); - int64_t time_us = (int64_t) tv_now.tv_sec * 1000000L + (int64_t) tv_now.tv_usec; - - - Serial.println(time_us); - - display.clearDisplay(); - - display.setTextSize(2); // Draw 2X-scale text - display.setTextColor(SSD1306_WHITE); - display.setCursor(10, 0); - display.println(F("scroll")); - display.display(); // Show initial text - vTaskDelay(1000); - - } + motion.begin(); } \ No newline at end of file diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 028aed3..d4c0113 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -20,16 +20,20 @@ Motion::Motion() { void Motion::begin(void) { } - -// Move forward for a certain amount of time. -void Motion::move(uint32_t moveForMs) { +void moveTask(void * moveForMs) { analogWrite(MOTOR_LEFT_PIN, 128); analogWrite(MOTOR_RIGHT_PIN, 128); - vTaskDelay(moveForMs); + vTaskDelay((uint32_t) moveForMs); analogWrite(MOTOR_LEFT_PIN, 0); analogWrite(MOTOR_RIGHT_PIN, 0); } +// Move forward for a certain amount of time. +void Motion::move(uint32_t moveForMs) { + xTaskCreate(moveTask, "Move", configMINIMAL_STACK_SIZE, (void*)moveForMs, 1, NULL); +} + + // Rotate clockwise for a certain amount of time. void Motion::rotateClockwise(uint32_t rotateForMs) { From 3d4b62438ea8f2f7128dfe1f03ed602f557a0dfe Mon Sep 17 00:00:00 2001 From: 99cardz Date: Wed, 10 Jan 2024 12:48:28 +0100 Subject: [PATCH 09/14] added arduino-cli commands to upload and compile --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index b2fb3b1..c0d6b18 100644 --- a/README.md +++ b/README.md @@ -136,6 +136,10 @@ For instance, in `src/Dezibot.h`, to include `src/motion/Motion.h`, you should w * USB Mode: "Hardware CDC and JTAG" * Programmer: "Esptool" +Using `arduino-cli` to compile and upload: +`arduino-cli upload /Users/jo/Documents/Arduino/theSketch -p /dev/cu.usbmodem101 -b esp32:esp32:nora_w10` +`arduino-cli compile /Users/jo/Documents/Arduino/theSketch -p /dev/cu.usbmodem101 -b esp32:esp32:nora_w10` + #### Display It is important to specify the SDA and SCL ports by using `Wire.begin(SDA, SCL)`. From 5a731e5fa418d3c7304bf2da657c93c2d5d735bd Mon Sep 17 00:00:00 2001 From: 99cardz Date: Wed, 10 Jan 2024 13:57:03 +0100 Subject: [PATCH 10/14] vTask Testing --- src/motion/Motion.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index d4c0113..049a75d 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -31,6 +31,11 @@ void moveTask(void * moveForMs) { // Move forward for a certain amount of time. void Motion::move(uint32_t moveForMs) { xTaskCreate(moveTask, "Move", configMINIMAL_STACK_SIZE, (void*)moveForMs, 1, NULL); + // analogWrite(MOTOR_LEFT_PIN, 128); + // analogWrite(MOTOR_RIGHT_PIN, 128); + // vTaskDelay(moveForMs); + // analogWrite(MOTOR_LEFT_PIN, 0); + // analogWrite(MOTOR_RIGHT_PIN, 0); } From fb3b4211d5a6ddfd3a40bd1ec49686b909ee7a8f Mon Sep 17 00:00:00 2001 From: 99cardz Date: Wed, 10 Jan 2024 15:56:26 +0100 Subject: [PATCH 11/14] tasks working now --- src/motion/Motion.cpp | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 049a75d..5244d24 100644 --- a/src/motion/Motion.cpp +++ b/src/motion/Motion.cpp @@ -11,6 +11,10 @@ #include "Motion.h" +TaskHandle_t xMoveTaskHandle = NULL; +TaskHandle_t xClockwiseTaskHandle = NULL; +TaskHandle_t xAntiClockwiseTaskHandle = NULL; + // Constructor Motion::Motion() { @@ -20,31 +24,40 @@ Motion::Motion() { void Motion::begin(void) { } -void moveTask(void * moveForMs) { +void moveTask(void * args) { analogWrite(MOTOR_LEFT_PIN, 128); analogWrite(MOTOR_RIGHT_PIN, 128); - vTaskDelay((uint32_t) moveForMs); + 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) { - xTaskCreate(moveTask, "Move", configMINIMAL_STACK_SIZE, (void*)moveForMs, 1, NULL); - // analogWrite(MOTOR_LEFT_PIN, 128); - // analogWrite(MOTOR_RIGHT_PIN, 128); - // vTaskDelay(moveForMs); - // analogWrite(MOTOR_LEFT_PIN, 0); - // analogWrite(MOTOR_RIGHT_PIN, 0); + xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle); } +void leftMotorTask(void * args) { + analogWrite(MOTOR_LEFT_PIN, 128); + 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); +} +void rightMotorTask(void * args) { + analogWrite(MOTOR_RIGHT_PIN, 128); + 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); } From b8474815c0b4a1a54622c5f8ea862672c822e778 Mon Sep 17 00:00:00 2001 From: 99cardz Date: Tue, 6 Feb 2024 13:33:56 +0100 Subject: [PATCH 12/14] add example sketch --- example/motion.ino | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 example/motion.ino diff --git a/example/motion.ino b/example/motion.ino new file mode 100644 index 0000000..2bdfd47 --- /dev/null +++ b/example/motion.ino @@ -0,0 +1,18 @@ +#include +Dezibot dezibot = Dezibot(); +void setup() { + dezibot.begin(); + Serial.begin(9600); +} +void loop() { + Serial.println("bla"); + dezibot.motion.move(1000); + dezibot.multiColorLight.setLed(BOTTOM, RED); + delay(2000); dezibot.motion.rotateAnticlockwise(1000); + dezibot.multiColorLight.setLed(BOTTOM, GREEN); + delay(2000); dezibot.motion.rotateClockwise(1000); + dezibot.multiColorLight.setLed(BOTTOM, BLUE); + delay(2000); + dezibot.multiColorLight.turnOffLed(); + delay(2000); +} \ No newline at end of file From 45977c95b9018e6741e751bbf6af7f306f6b2c32 Mon Sep 17 00:00:00 2001 From: 99cardz Date: Tue, 6 Feb 2024 13:34:23 +0100 Subject: [PATCH 13/14] formatting --- example/motion.ino | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/example/motion.ino b/example/motion.ino index 2bdfd47..e1f6d37 100644 --- a/example/motion.ino +++ b/example/motion.ino @@ -8,9 +8,11 @@ void loop() { Serial.println("bla"); dezibot.motion.move(1000); dezibot.multiColorLight.setLed(BOTTOM, RED); - delay(2000); dezibot.motion.rotateAnticlockwise(1000); + delay(2000); + dezibot.motion.rotateAnticlockwise(1000); dezibot.multiColorLight.setLed(BOTTOM, GREEN); - delay(2000); dezibot.motion.rotateClockwise(1000); + delay(2000); + dezibot.motion.rotateClockwise(1000); dezibot.multiColorLight.setLed(BOTTOM, BLUE); delay(2000); dezibot.multiColorLight.turnOffLed(); From c8f7e953635699af58d0753cbc9bab884189dfd1 Mon Sep 17 00:00:00 2001 From: hhaupt Date: Sun, 28 Apr 2024 22:38:35 +0200 Subject: [PATCH 14/14] 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