updated movement Tasks to handle multiple calling properbly and prepare calibration feature

src/Dezibot.cpp

@@ -19,6 +19,5 @@ Dezibot::Dezibot():multiColorLight(){
 void Dezibot::begin(void) {
     motion.begin();
     multiColorLight.begin();
-    motionDetection.begin();
     Wire.begin(1,2);
 };

src/Dezibot.h

@@ -28,7 +28,6 @@ public:
     ColorDetection colorDetection;
     MultiColorLight multiColorLight;
     Motion motion ;
-    MotionDetection motionDetection;
     void begin(void);
 
 };

src/motion/Motion.cpp

@@ -11,9 +11,6 @@
 
 #include "Motion.h"
 
-TaskHandle_t xMoveTaskHandle = NULL;
-TaskHandle_t xClockwiseTaskHandle = NULL;
-TaskHandle_t xAntiClockwiseTaskHandle = NULL;
 
 // Initialize the movement component.
 
@@ -29,36 +26,83 @@ void Motion::begin(void) {
     ledc_timer_config(&motor_timer);
     Motion::left.begin();
     Motion::right.begin();
+    detection.begin();
 };
 void Motion::moveTask(void * args) {
+    uint32_t runtime = (uint32_t)args;
+
     Motion::left.setSpeed(LEFT_MOTOR_DUTY);
     Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
-    //moveCMD cmd = (moveCMD)args;
+    Motion::xLastWakeTime = xTaskGetTickCount();
-    //vTaskDelay( cmd.duration/ portTICK_PERIOD_MS);
+    while(1){
-    Motion::left.setSpeed(0);
+        if(runtime>40||runtime==0){
-    Motion::right.setSpeed(0);
+            vTaskDelayUntil(&xLastWakeTime,40);
-    vTaskDelete(xMoveTaskHandle);
+            runtime -= 40;
+            //calc new parameters
+            //set new parameters
+        } else {
+            vTaskDelayUntil(&xLastWakeTime,runtime);
+            Motion::left.setSpeed(0);
+            Motion::right.setSpeed(0);
+            vTaskDelete(xMoveTaskHandle);
+        }
+    }
 };
 
 // Move forward for a certain amount of time.
 void Motion::move(uint32_t moveForMs) {
        // moveCMD cmd = {moveForMs,imuInst};
-      //  xTaskCreate(moveTask, "Move", 4096, (void*)cmd, 10, &xMoveTaskHandle);
+      
-        Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+       if(xMoveTaskHandle){
-        Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
+            vTaskDelete(xMoveTaskHandle);
+            xMoveTaskHandle = NULL;
+       }
+       if(xClockwiseTaskHandle){
+
+            vTaskDelete(xClockwiseTaskHandle);
+            xClockwiseTaskHandle = NULL;
+       }
+       if(xAntiClockwiseTaskHandle){
+            vTaskDelete(xAntiClockwiseTaskHandle);
+            xAntiClockwiseTaskHandle = NULL;
+
+       }
+        xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle);
+        
 };
 
 void Motion::leftMotorTask(void * args) {
-    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+     uint32_t runtime = (uint32_t)args;
+     if(xMoveTaskHandle){
+        vTaskDelete(xMoveTaskHandle);
+        xMoveTaskHandle = NULL;
+    }
+    if(xAntiClockwiseTaskHandle){
+        vTaskDelete(xAntiClockwiseTaskHandle);
+        xAntiClockwiseTaskHandle = NULL;
+    }
     Motion::right.setSpeed(0);
-    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
+    Motion::left.setSpeed(LEFT_MOTOR_DUTY);
-    Motion::left.setSpeed(0);
+    while(1){
-    vTaskDelete(xClockwiseTaskHandle);
+        if((runtime>40)||(runtime==0)){
+            vTaskDelayUntil(&xLastWakeTime,40);
+            runtime -=40;
+        } else {
+            vTaskDelayUntil(&xLastWakeTime,runtime);
+            Motion::left.setSpeed(0);
+            vTaskDelete(xClockwiseTaskHandle);
+        }
+        vTaskDelayUntil(&xLastWakeTime,40);
+    }
 };
 
 // Rotate clockwise for a certain amount of time.
 void Motion::rotateClockwise(uint32_t rotateForMs) {
+
     if (rotateForMs > 0){
+        if(xClockwiseTaskHandle){
+            vTaskDelete(xClockwiseTaskHandle);
+        }
         xTaskCreate(leftMotorTask, "LeftMotor", 4096, (void*)rotateForMs, 10, &xClockwiseTaskHandle);
     } else {
         Motion::left.setSpeed(LEFT_MOTOR_DUTY);
@@ -67,16 +111,35 @@ void Motion::rotateClockwise(uint32_t rotateForMs) {
 };
 
 void Motion::rightMotorTask(void * args) {
+    uint32_t runtime = (uint32_t)args;
+     if(xMoveTaskHandle){
+        vTaskDelete(xMoveTaskHandle);
+        xMoveTaskHandle = NULL;
+    }
+    if(xClockwiseTaskHandle){
+        vTaskDelete(xClockwiseTaskHandle);
+        xClockwiseTaskHandle = NULL;
+    }
     Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
     Motion::left.setSpeed(0);
-    vTaskDelay((uint32_t) args / portTICK_PERIOD_MS);
+    while(1){
-    Motion::right.setSpeed(0);
+        if(runtime>40||runtime==0){
-    vTaskDelete(xAntiClockwiseTaskHandle);
+            vTaskDelayUntil(&xLastWakeTime,40);
+            runtime -= 40;
+        } else {
+            vTaskDelayUntil(&xLastWakeTime,runtime);
+            Motion::right.setSpeed(0);
+            vTaskDelete(xAntiClockwiseTaskHandle);
+        }
+    }
 };
 
 // Rotate anticlockwise for a certain amount of time.
 void Motion::rotateAntiClockwise(uint32_t rotateForMs) {
     if(rotateForMs > 0){
+        if(xAntiClockwiseTaskHandle){
+            vTaskDelete(xAntiClockwiseTaskHandle);
+        }
         xTaskCreate(rightMotorTask, "RightMotor", 4096, (void*)rotateForMs, 10, &xAntiClockwiseTaskHandle);
     } else {
         Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
@@ -85,9 +148,19 @@ void Motion::rotateAntiClockwise(uint32_t rotateForMs) {
 };
 
 void Motion::stop(void){
+    if(xMoveTaskHandle){
+        vTaskDelete(xMoveTaskHandle);
+        xMoveTaskHandle = NULL;
+    }
+    if(xAntiClockwiseTaskHandle){
+        vTaskDelete(xAntiClockwiseTaskHandle);
+        xAntiClockwiseTaskHandle = NULL;
+    }
+    if(xClockwiseTaskHandle){
+        vTaskDelete(xClockwiseTaskHandle);
+        xClockwiseTaskHandle = NULL;
+    }
     Motion::left.setSpeed(0);
     Motion::right.setSpeed(0);
-    //just for testing
- //   Serial.println(imu->getAcceleration().z);
 }
  

src/motion/Motion.h

@@ -52,12 +52,18 @@ protected:
     static void moveTask(void * args);
     static void leftMotorTask(void * args);
     static void rightMotorTask(void * args);
+    static inline TaskHandle_t xMoveTaskHandle = NULL;
+    static inline TaskHandle_t xClockwiseTaskHandle = NULL;
+    static inline TaskHandle_t xAntiClockwiseTaskHandle = NULL;
+    static inline TickType_t xLastWakeTime;
 
 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);
-    friend class Dezibot;
+    
+    //MotionDetection instance, for motion Correction and user (access with dezibot.motion.detection)
+    static inline MotionDetection detection;
 
     /**
      * constructor gets a pointer to the motiondetection class, which enables correction of the motion