phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-07-04 09:41:43 +02:00
Code Issues Packages Projects Releases Wiki Activity

added basic motioncorrection for the move function

Browse Source
This commit is contained in:
hhaupt
2024-06-14 02:06:37 +02:00
parent 2701446915
commit b1bb29aa1c
2 changed files with 68 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
src/motion/Motion.cpp
View File

@ -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) {
// moveCMD cmd = {moveForMs,imuInst};
void Motion::move(uint32_t moveForMs, uint baseValue) {
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);

36
src/motion/Motion.h
View File

@ -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);
};