jonas
/
ADIS_Projects
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Advanced Distributed Systems module at HSLU
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
212 Commits
1 Branch
0 Tags
4.6 MiB
 
 
Tag: Branch: Tree: 3b900f1d8a
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '3b900f1d8a'
${ noResults }
ADIS_Projects/ADIS_Sumo/Sumo/LineFollow.c

509 lines
17 KiB
Raw Blame History

/**
* \file
* \brief Implements line following of the robot
* \author Erich Styger, erich.styger@hslu.ch
* \license SPDX-License-Identifier: BSD-3-Clause
* This is the implementation of the line following.
*/
#include "platform.h"
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
#include "LineFollow.h"
#include "McuRTOS.h"
#include "McuShell.h"
#include "Shell.h"
#include "Motor.h"
#include "Reflectance.h"
#include "McuWait.h"
#include "McuUtility.h"
#include "Pid.h"
#include "Turn.h"
#include "Maze.h"
#include "Shell.h"
#if PL_CONFIG_USE_BUZZER
#include "Buzzer.h"
#endif
#if PL_CONFIG_USE_DRIVE
#include "Drive.h"
#endif
#define LINE_FOLLOW_FW (1) /* test setting to do forward line following */
#define LINE_DO_DEBUG_WAIT (0) /* if set to one, it will add some debug wait to stop the engines */
#if LINE_DO_DEBUG_WAIT
#define LINE_DO_DEBUG_WAIT_MS (500) /* waiting time in milliseconds */
#endif
#if LINE_DO_DEBUG_WAIT /* debug mode */
static void DebugStopAndWait(void) {
static volatile bool waitHere = FALSE;
TURN_Turn(TURN_STOP, NULL);
vTaskDelay(LINE_DO_DEBUG_WAIT_MS);
waitHere = TRUE;
}
#endif
#define PL_TURN_ON_FINISH (0) /* temporary only */
typedef enum {
LINE_SPEED_LOW,
LINE_SPEED_MEDIUM,
LINE_SPEED_HIGH
} LINE_SpeedMode;
static LINE_SpeedMode lineFollowSpeed = LINE_SPEED_LOW; /* set value in LineTask()! */
typedef enum {
STATE_IDLE, /* idle, not doing anything */
STATE_FOLLOW_SEGMENT, /* line following segment, going forward */
#if PL_CONFIG_APP_LINE_MAZE
STATE_TURN, /* reached an intersection, turning around */
STATE_FINISHED, /* reached finish area */
#endif
STATE_STOP /* stop the engines */
} StateType;
/* task notification bits */
#define LF_START_FOLLOWING (1<<0) /* start line following */
#define LF_STOP_FOLLOWING (1<<1) /* stop line following */
static TaskHandle_t LFTaskHandle;
static volatile StateType LF_currState = STATE_IDLE;
#if PL_CONFIG_APP_LINE_MAZE
static uint8_t LF_solvedIdx = 0; /* index to iterate through the solution, zero is the solution start index */
#endif
void LF_StartFollowing(void) {
(void)xTaskNotify(LFTaskHandle, LF_START_FOLLOWING, eSetBits);
}
void LF_StopFollowing(void) {
(void)xTaskNotify(LFTaskHandle, LF_STOP_FOLLOWING, eSetBits);
}
void LF_StartStopFollowing(void) {
if (LF_IsFollowing()) {
(void)xTaskNotify(LFTaskHandle, LF_STOP_FOLLOWING, eSetBits);
} else {
(void)xTaskNotify(LFTaskHandle, LF_START_FOLLOWING, eSetBits);
}
}
/*!
* \brief follows a line segment.
* \return Returns TRUE if still on line segment
*/
bool LF_FollowSegment(REF_LineKindMode mode, bool forward) {
uint16_t currLine;
bool onLine;
REF_LineKind currLineKind;
REF_GetLineValue(&onLine);
currLineKind = REF_GetLineKind(mode);
if (currLineKind!=REF_LINE_STRAIGHT) {
SHELL_SendString((unsigned char*)"LF_FollowSegment: ");
SHELL_SendString(REF_LineKindStr(currLineKind));
SHELL_SendString((unsigned char*)"\r\n");
}
#if PL_CONFIG_APP_LINE_FOLLOWING
if (currLineKind==REF_LINE_STRAIGHT || currLineKind!=REF_LINE_NONE) { /* as long we have something: follow line */
#elif PL_CONFIG_APP_LINE_MAZE
if (currLineKind==REF_LINE_STRAIGHT) { /* as long we have a line: follow line */
#endif
currLine = REF_GetLineValue(&onLine);
PID_Line(currLine, REF_MIDDLE_LINE_VALUE, REF_LineWidth(), forward); /* continue move along the line */
return TRUE;
} else { /* stop motors */
SHELL_SendString((unsigned char*)"LF_FollowSegment stop: ");
SHELL_SendString(REF_LineKindStr(currLineKind));
SHELL_SendString((unsigned char*)"\r\n");
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
vTaskDelay(pdMS_TO_TICKS(20)); /* give time for PWM to change */
return FALSE; /* intersection/change of direction or not on line any more */
}
}
/*!
* \brief Move from outside onto a line/segment to follow it.
* \return Returns TRUE if still on line segment
*/
bool LF_MoveOnSegment(bool turningLeft) {
uint16_t currLine, targetLine;
int tmp;
bool onLine;
REF_LineKind currLineKind;
uint16_t lineWidth;
currLine = REF_GetLineValue(&onLine);
currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_ALL);
lineWidth = REF_LineWidth();
if (lineWidth>2000) {
if (turningLeft) {
tmp = REF_MIDDLE_LINE_VALUE+lineWidth;
} else {
tmp = REF_MIDDLE_LINE_VALUE-lineWidth;
}
if (tmp<0) {
tmp = 0;
}
if (tmp>REF_MAX_LINE_VALUE) {
tmp = REF_MAX_LINE_VALUE;
}
targetLine = tmp;
} else {
targetLine = REF_MIDDLE_LINE_VALUE;
}
if (currLineKind==REF_LINE_STRAIGHT || currLineKind==REF_LINE_LEFT || currLineKind==REF_LINE_RIGHT || currLineKind==REF_LINE_FULL) {
PID_Line(currLine, targetLine, REF_LineWidth(), TRUE); /* move along the line */
return TRUE;
} else {
return FALSE; /* intersection/change of direction or not on line any more */
}
}
bool LF_FollowSegmentLinePos(REF_LineKindMode mode, uint16_t setLinePos) {
uint16_t currLine;
bool onLine;
REF_LineKind currLineKind;
currLine = REF_GetLineValue(&onLine);
currLineKind = REF_GetLineKind(mode);
if (currLineKind==REF_LINE_STRAIGHT) {
PID_Line(currLine, setLinePos, REF_LineWidth(), TRUE); /* move along the line */
return TRUE;
} else {
return FALSE; /* intersection/change of direction or not on line any more */
}
}
static void StateMachine(void) {
for(;;) {
switch (LF_currState) {
case STATE_IDLE:
break;
case STATE_FOLLOW_SEGMENT:
#if PL_CONFIG_APP_LINE_MAZE
if (!LF_FollowSegment(REF_LINE_KIND_MODE_MAZE, LINE_FOLLOW_FW)) {
TURN_Turn(TURN_STOP, NULL);
#if LINE_DO_DEBUG_WAIT /* debug mode */
DebugStopAndWait();
#endif
LF_currState = STATE_TURN; /* make turn */
continue; /* process next step immediately */
}
#else
{
REF_LineKind currLineKind;
if (!LF_FollowSegment(REF_LINE_KIND_MODE_LINE_FOLLOW, LINE_FOLLOW_FW)) {
SHELL_SendString((unsigned char*)"No segment any more!\r\n");
currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_LINE_FOLLOW);
SHELL_SendString((unsigned char*)"Line: ");
SHELL_SendString(REF_LineKindStr(currLineKind));
SHELL_SendString((unsigned char*)"\r\n");
SHELL_SendString((unsigned char*)"stop!\r\n");
LF_currState = STATE_STOP; /* stop if we do not have a line any more */
}
}
#endif
break;
#if PL_CONFIG_APP_LINE_MAZE
case STATE_TURN:
if (MAZE_IsSolved()) {
TURN_Kind turn;
turn = MAZE_GetSolvedTurn(&LF_solvedIdx);
if (turn==TURN_STOP) { /* last turn reached */
TURN_Turn(turn, NULL);
#if PL_CONFIG_USE_DRIVE
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
#endif
LF_currState = STATE_STOP;
SHELL_SendString((unsigned char *)"MAZE: I'm back to the start of maze!\r\n");
#if PL_CONFIG_USE_BUZZER
BUZ_PlayTune(1);
#endif
MAZE_ClearSolution(); /* clear solution */
#if PL_TURN_ON_START
/* turn the robot on start so it is ready to run the maze again */
#if PL_CONFIG_USE_QUADRATURE
TURN_Turn(TURN_LEFT180, NULL);
#else /* not accurate enough without position sensor */
TURN_Turn(TURN_LEFT90, NULL); /* do turn in two steps */
TURN_Turn(TURN_LEFT90, NULL);
#endif
#endif
TURN_Turn(TURN_STOP, NULL);
#if PL_CONFIG_USE_DRIVE
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
#endif
/* now ready to solve maze again */
SHELL_SendString((unsigned char *)"MAZE: ready to start again!\r\n");
} else { /* perform turning */
TURN_Turn(TURN_STEP_LINE_FW_AND_POST_LINE, NULL); /* Step over line */
TURN_Turn(turn, NULL);
#if LINE_DO_DEBUG_WAIT /* debug mode */
DebugStopAndWait();
#endif
#if PL_CONFIG_USE_DRIVE
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode, so we can do line following (line following is PWM) */
#endif
LF_currState = STATE_FOLLOW_SEGMENT;
continue; /* process next state immediately */
}
} else { /* still evaluating maze */
bool deadEndGoBw = FALSE;
bool finished = FALSE;
if (MAZE_EvaluteTurn(&finished, &deadEndGoBw)==ERR_OK) { /* finished turning */
#if LINE_DO_DEBUG_WAIT /* debug mode */
DebugStopAndWait();
#endif
#if PL_CONFIG_USE_DRIVE
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
#endif
if (finished) {
LF_currState = STATE_FINISHED;
MAZE_SetSolved();
LF_solvedIdx = 0; /* set index to start of solution */
#if PL_CONFIG_HAS_BUZZER
BUZ_PlayTune(BUZ_TUNE_MAZE_DESTINATION);
#endif
#if PL_TURN_ON_FINISH
/* turn the robot */
#if PL_CONFIG_USE_QUADRATURE
TURN_Turn(TURN_LEFT180, NULL);
#else /* not accurate enough without position sensor */
TURN_Turn(TURN_LEFT90, NULL); /* do turn in two steps */
TURN_Turn(TURN_LEFT90, NULL);
#endif
#endif
//TURN_Turn(TURN_STOP, NULL);
#if LINE_DO_DEBUG_WAIT /* debug mode */
DebugStopAndWait();
#endif
#if PL_CONFIG_USE_DRIVE
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
#endif
/* now ready to do line following */
} else {
LF_currState = STATE_FOLLOW_SEGMENT;
}
} else { /* error case */
LF_currState = STATE_STOP;
}
}
break;
#endif
#if PL_CONFIG_APP_LINE_MAZE
case STATE_FINISHED:
SHELL_SendString((unsigned char *)"MAZE: Found maze destination!\r\n");
#if PL_CONFIG_USE_BUZZER
BUZ_PlayTune(BUZ_TUNE_MAZE_DESTINATION);
#endif
//SHELL_SendString((unsigned char *)"MAZE: Going back to start...\r\n");
//LF_currState = STATE_FOLLOW_SEGMENT; /* go back to start */
//MAZE_ClearSolution(); /* clear solution */
LF_currState = STATE_STOP;
break;
#endif
case STATE_STOP:
SHELL_SendString((unsigned char *)"Stopped!\r\n");
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
TURN_Turn(TURN_STOP, NULL);
LF_currState = STATE_IDLE;
break;
} /* switch */
break; /* get out of for loop */
} /* for */
}
bool LF_IsFollowing(void) {
return LF_currState!=STATE_IDLE;
}
static const unsigned char *LINE_GetSpeedModeString(LINE_SpeedMode mode) {
switch(mode) {
case LINE_SPEED_LOW: return (const unsigned char*)"LOW";
case LINE_SPEED_MEDIUM: return (const unsigned char*)"MEDIUM";
case LINE_SPEED_HIGH: return (const unsigned char*)"HIGH";
default: break;
}
return (const unsigned char*)"UNKNOWN";
}
static uint8_t LINE_SetSpeed(LINE_SpeedMode speed) {
uint8_t res;
PID_Config *lineFwPid, *posLeftPid, *posRightPid;
res = PID_GetPIDConfig(PID_CONFIG_LINE_FW, &lineFwPid);
if (res!=ERR_OK || lineFwPid==NULL) {
return ERR_FAILED;
}
res = PID_GetPIDConfig(PID_CONFIG_POS_LEFT, &posLeftPid);
if (res!=ERR_OK || posLeftPid==NULL) {
return ERR_FAILED;
}
res = PID_GetPIDConfig(PID_CONFIG_POS_RIGHT, &posRightPid);
if (res!=ERR_OK || posRightPid==NULL) {
return ERR_FAILED;
}
switch(speed) {
case LINE_SPEED_LOW:
lineFwPid->maxSpeedPercent = 15;
lineFwPid->pFactor100 = 4000;
lineFwPid->dFactor100 = 500;
TURN_SetSteps(680, 170, 180);
posLeftPid->maxSpeedPercent = 30;
posLeftPid->pFactor100 = 2000;
posRightPid->pFactor100 = posLeftPid->pFactor100;
posRightPid->maxSpeedPercent = posLeftPid->maxSpeedPercent;
break;
case LINE_SPEED_MEDIUM:
lineFwPid->maxSpeedPercent = 30; /* 15 => 30 */
lineFwPid->pFactor100 = 5500;
lineFwPid->iAntiWindup = 100000;
TURN_SetSteps(700, 190, 100);
posLeftPid->maxSpeedPercent = 80;
posLeftPid->pFactor100 = 2000;
posRightPid->pFactor100 = posLeftPid->pFactor100;
posRightPid->maxSpeedPercent = posLeftPid->maxSpeedPercent;
break;
case LINE_SPEED_HIGH: /* to be verified */
lineFwPid->maxSpeedPercent = 40;
lineFwPid->pFactor100 = 2000;
lineFwPid->iAntiWindup = 30000;
TURN_SetSteps(700, 150, 80);
posLeftPid->maxSpeedPercent = 100;
posLeftPid->pFactor100 = 2000;
posRightPid->pFactor100 = posLeftPid->pFactor100;
posRightPid->maxSpeedPercent = posLeftPid->maxSpeedPercent;
break;
default:
return ERR_FAILED;
} /* switch */
lineFollowSpeed = speed;
SHELL_SendString((unsigned char*)"Changed parameters for speed!\n");
return ERR_OK;
}
static void LineTask (void *pvParameters) {
uint32_t notifcationValue;
BaseType_t notified;
(void)pvParameters; /* not used */
#if PL_SLOWER_SPEED
(void)LINE_SetSpeed(LINE_SPEED_LOW);
#else /* check! */
(void)LINE_SetSpeed(LINE_SPEED_MEDIUM);
#endif
for(;;) {
notified = xTaskNotifyWait(0UL, LF_START_FOLLOWING|LF_STOP_FOLLOWING, &notifcationValue, 1); /* check flags, need to wait for one tick */
if (notified==pdTRUE) { /* received notification */
if (notifcationValue&LF_START_FOLLOWING) {
if (REF_CanUseSensor()) {
PID_Start();
LF_currState = STATE_FOLLOW_SEGMENT;
#if PL_CONFIG_USE_DRIVE
(void)DRV_SetMode(DRV_MODE_NONE); /* disable any drive mode */
#endif
} else {
SHELL_SendString((unsigned char*)"Sensors not ready!\r\n");
#if PL_CONFIG_USE_BUZZER
(void)BUZ_Beep(500, 500);
#endif
}
}
if (notifcationValue&LF_STOP_FOLLOWING) {
LF_currState = STATE_STOP;
}
}
StateMachine();
if (LF_IsFollowing()) {
vTaskDelay(5/portTICK_PERIOD_MS);
} else {
vTaskDelay(100/portTICK_PERIOD_MS);
}
}
}
static void LF_PrintHelp(const McuShell_StdIOType *io) {
McuShell_SendHelpStr((unsigned char*)"line", (unsigned char*)"Group of line following commands\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows line help or status\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" speed <mode>", (unsigned char*)"Line following speed, either low, medium or high\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" start|stop", (unsigned char*)"Starts or stops line following\r\n", io->stdOut);
}
static void LF_PrintStatus(const McuShell_StdIOType *io) {
uint8_t buf[32];
McuShell_SendStatusStr((unsigned char*)"line follow", (unsigned char*)"Line following status\r\n", io->stdOut);
switch (LF_currState) {
case STATE_IDLE:
McuShell_SendStatusStr((unsigned char*)" state", (unsigned char*)"IDLE\r\n", io->stdOut);
break;
case STATE_FOLLOW_SEGMENT:
McuShell_SendStatusStr((unsigned char*)" state", (unsigned char*)"FOLLOW_SEGMENT\r\n", io->stdOut);
break;
case STATE_STOP:
McuShell_SendStatusStr((unsigned char*)" state", (unsigned char*)"STOP\r\n", io->stdOut);
break;
#if PL_CONFIG_APP_LINE_MAZE
case STATE_TURN:
McuShell_SendStatusStr((unsigned char*)" state", (unsigned char*)"TURN\r\n", io->stdOut);
break;
case STATE_FINISHED:
McuShell_SendStatusStr((unsigned char*)" state", (unsigned char*)"FINISHED\r\n", io->stdOut);
break;
#endif
default:
McuShell_SendStatusStr((unsigned char*)" state", (unsigned char*)"UNKNOWN\r\n", io->stdOut);
break;
} /* switch */
McuUtility_strcpy(buf, sizeof(buf), LINE_GetSpeedModeString(lineFollowSpeed));
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
McuShell_SendStatusStr((unsigned char*)" speed", buf, io->stdOut);
}
uint8_t LF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
uint8_t res = ERR_OK;
if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, (char*)"line help")==0) {
LF_PrintHelp(io);
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"line status")==0) {
LF_PrintStatus(io);
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"line start")==0) {
LF_StartFollowing();
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"line stop")==0) {
LF_StopFollowing();
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"line speed low")==0) {
(void)LINE_SetSpeed(LINE_SPEED_LOW);
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"line speed medium")==0) {
(void)LINE_SetSpeed(LINE_SPEED_MEDIUM);
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"line speed high")==0) {
(void)LINE_SetSpeed(LINE_SPEED_HIGH);
*handled = TRUE;
}
return res;
}
void LF_Deinit(void) {
/* nothing needed */
}
void LF_Init(void) {
LF_currState = STATE_IDLE;
if (xTaskCreate(LineTask, "Line", 600/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+2, &LFTaskHandle) != pdPASS) {
for(;;){} /* error */
}
}
#endif /* PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE */