phillip/dezibot
1
0
Fork 0
mirror of https://gitlab.dit.htwk-leipzig.de/phillip.kuehne/dezibot.git synced 2025-05-27 23:11:48 +02:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
hhaupt 2024-06-14 01:22:46 +02:00
parent 4f308412e4
commit 2701446915
4 changed files with 100 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/Dezibot.cpp
View File

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

1
src/Dezibot.h
View File

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

113
src/motion/Motion.cpp
View File

@ -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;
//vTaskDelay( cmd.duration/ portTICK_PERIOD_MS);
Motion::left.setSpeed(0);
Motion::right.setSpeed(0);
vTaskDelete(xMoveTaskHandle);
Motion::xLastWakeTime = xTaskGetTickCount();
while(1){
if(runtime>40||runtime==0){
vTaskDelayUntil(&xLastWakeTime,40);
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);
Motion::right.setSpeed(RIGHT_MOTOR_DUTY);
if(xMoveTaskHandle){
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(0);
vTaskDelete(xClockwiseTaskHandle);
Motion::left.setSpeed(LEFT_MOTOR_DUTY);
while(1){
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);
Motion::right.setSpeed(0);
vTaskDelete(xAntiClockwiseTaskHandle);
while(1){
if(runtime>40||runtime==0){
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);
}

8
src/motion/Motion.h
View File

@ -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