diff --git a/src/motion/Motion.cpp b/src/motion/Motion.cpp index 4b4f513..2448f04 100644 --- a/src/motion/Motion.cpp +++ b/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;icorrectionThreshold){ + 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 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); diff --git a/src/motion/Motion.h b/src/motion/Motion.h index 30dffff..d0a6eb2 100644 --- a/src/motion/Motion.h +++ b/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 const uint16_t LEFT_MOTOR_DUTY = 4096; + static inline uint16_t RIGHT_MOTOR_DUTY = 3900; + 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); + };