added basic motioncorrection for the move function

src/motion/Motion.cpp

@@ -40,6 +40,40 @@ void Motion::moveTask(void * args) {
             runtime -= 40;
             //calc new parameters
             //set new parameters
+            int fifocount = detection.getDataFromFIFO(buffer);
+            int rightCounter = 0;
+            int leftCounter = 0;
+            int changerate = 0;
+            for(int i = 0;i<fifocount;i++){
+                if(buffer[i].gyro.z>correctionThreshold){
+                    rightCounter++;
+                } else if(buffer[i].gyro.z<-correctionThreshold){
+                    leftCounter++;
+                }
+            }
+            int difference = abs(leftCounter-rightCounter);
+            if (difference>25){
+                changerate = 200;
+            } else if(difference>20){
+                changerate = 100;
+            } else if(difference >15){
+                changerate = 50;
+            } else if(difference > 10){
+                changerate = 20;
+            } else{
+                changerate = 5; 
+            }
+
+            if(leftCounter>rightCounter){ //rotates anticlock
+                LEFT_MOTOR_DUTY+=changerate;
+                RIGHT_MOTOR_DUTY-=changerate;
+            } else if(leftCounter<rightCounter){
+                LEFT_MOTOR_DUTY-=changerate;
+                RIGHT_MOTOR_DUTY+=changerate;
+            } 
+
+            Motion::left.setSpeed(LEFT_MOTOR_DUTY);
+            Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
         } else {
             vTaskDelayUntil(&xLastWakeTime,runtime);
             Motion::left.setSpeed(0);
@@ -50,9 +84,7 @@ void Motion::moveTask(void * args) {
 };
 
 // Move forward for a certain amount of time.
-void Motion::move(uint32_t moveForMs) {
+void Motion::move(uint32_t moveForMs, uint baseValue) {    
-       // moveCMD cmd = {moveForMs,imuInst};
-      
        if(xMoveTaskHandle){
             vTaskDelete(xMoveTaskHandle);
             xMoveTaskHandle = NULL;
@@ -67,6 +99,8 @@ void Motion::move(uint32_t moveForMs) {
             xAntiClockwiseTaskHandle = NULL;
 
        }
+       LEFT_MOTOR_DUTY = baseValue;
+       RIGHT_MOTOR_DUTY = baseValue;
         xTaskCreate(moveTask, "Move", 4096, (void*)moveForMs, 10, &xMoveTaskHandle);
         
 };
@@ -97,8 +131,9 @@ void Motion::leftMotorTask(void * args) {
 };
 
 // Rotate clockwise for a certain amount of time.
-void Motion::rotateClockwise(uint32_t rotateForMs) {
+void Motion::rotateClockwise(uint32_t rotateForMs,uint baseValue) {
-
+    LEFT_MOTOR_DUTY = baseValue;
+    RIGHT_MOTOR_DUTY = baseValue;
     if (rotateForMs > 0){
         if(xClockwiseTaskHandle){
             vTaskDelete(xClockwiseTaskHandle);
@@ -135,7 +170,9 @@ void Motion::rightMotorTask(void * args) {
 };
 
 // Rotate anticlockwise for a certain amount of time.
-void Motion::rotateAntiClockwise(uint32_t rotateForMs) {
+void Motion::rotateAntiClockwise(uint32_t rotateForMs,uint baseValue) {
+    LEFT_MOTOR_DUTY = baseValue;
+    RIGHT_MOTOR_DUTY = baseValue;
     if(rotateForMs > 0){
         if(xAntiClockwiseTaskHandle){
             vTaskDelete(xAntiClockwiseTaskHandle);

src/motion/Motion.h

@@ -45,8 +45,8 @@ class Motor{
 class Motion{
 protected:
     static MotionDetection* imuInst;
-    static const uint16_t RIGHT_MOTOR_DUTY = 4096;
+    static inline uint16_t RIGHT_MOTOR_DUTY = 3900;
-    static const uint16_t LEFT_MOTOR_DUTY = 4096;
+    static inline uint16_t LEFT_MOTOR_DUTY = 3900;
     static const int MOTOR_RIGHT_PIN = 11; 
     static const int MOTOR_LEFT_PIN = 12;
     static void moveTask(void * args);
@@ -57,6 +57,9 @@ protected:
     static inline TaskHandle_t xAntiClockwiseTaskHandle = NULL;
     static inline TickType_t xLastWakeTime;
 
+    static inline FIFO_Package* buffer = new FIFO_Package[64];
+    static inline int correctionThreshold = 150; 
+
 public:
     //Shared Timer to sync movement
     static inline Motor left = Motor(MOTOR_LEFT_PIN,TIMER,CHANNEL_LEFT);
@@ -65,10 +68,6 @@ public:
     //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
-    */
-
     /**
      * @brief Initialize the movement component.
      * 
@@ -78,23 +77,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().
+     * The function applys a basic algorithm to improve the straigthness of the movement. 
+     * Lifting the robot from the desk may corrupt the results and is not recommended.
+     *  
      * @param moveForMs Representing the duration of forward moving in milliseconds.
+     * @param baseValue The value that is used to start with the calibrated movement. Defaults to 3900. 
+     * If the Dezibot is not moving forward at all increasing the value may help. If the robot is just jumping up and down but not forward, try a lower value. 
     */
-    static void move(uint32_t moveForMs=0);
+    static void move(uint32_t moveForMs=0,uint baseValue=3900);
 
     /**
      * @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.
+     * @param rotateForMs Representing the duration of rotating clockwise in milliseconds, or 0 to rotate until another movecmd is issued. Default is 0
+     * @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value)
     */
-    static void rotateClockwise(uint32_t rotateForMs=0);
+    static void rotateClockwise(uint32_t rotateForMs=0,uint baseValue=3900);
     
     /**
      * @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.
+     * @param rotateForMs Representing the duration of rotating anticlockwise in milliseconds or 0 to let the robot turn until another movecommand is issued. Default is 0.
+     * @param baseValue The value that is used to start with the calibrated movement (not released yet, currently just the used value).
     */
-    static void rotateAntiClockwise(uint32_t rotateForMs=0);
+    static void rotateAntiClockwise(uint32_t rotateForMs=0,uint baseValue=3900);
 
     /**
      * @brief stops any current movement, no matter if timebased or endless
@@ -102,6 +108,14 @@ public:
      */
     static void stop(void);
 
+    /**
+     * @brief Does the same as the move function, but this function does not apply any kind of algorithm to improve the result.
+     * 
+     * @param moveForMs how many ms should the robot move, or 0 to let the robot move until another move command is mentioned, default is 0
+     * @param baseValue the duty value that is used for the movement, default is 0
+     */
+    static void moveWithoutCorrection(uint32_t moveForMs=0, uint baseValue = 3900);
+
 };