jonas
/
ADIS_Projects
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add line following stuff

Browse Source
main
Simon Frei 4 years ago
parent 50df84d31e
commit dc2dde72fb
11 changed files with 2198 additions and 32 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 2
      ADIS_Sumo_Styger/Sumo/Application.c
  2. 509
      ADIS_Sumo_Styger/Sumo/LineFollow.c
  3. 77
      ADIS_Sumo_Styger/Sumo/LineFollow.h
  4. 81
      ADIS_Sumo_Styger/Sumo/LineHistory.c
  5. 24
      ADIS_Sumo_Styger/Sumo/LineHistory.h
  6. 489
      ADIS_Sumo_Styger/Sumo/Maze.c
  7. 93
      ADIS_Sumo_Styger/Sumo/Maze.h
  8. 33
      ADIS_Sumo_Styger/Sumo/Pid.c
  9. 793
      ADIS_Sumo_Styger/Sumo/Reflectance.c
  10. 127
      ADIS_Sumo_Styger/Sumo/Reflectance.h
  11. 2
      ADIS_Sumo_Styger/source/platform.h

2
ADIS_Sumo_Styger/Sumo/Application.c
Unescape View File

@ -212,7 +212,7 @@ static void StateMachine(bool buttonPress) {
#if PL_CONFIG_USE_LINE_SENSOR #if PL_CONFIG_USE_LINE_SENSOR
static uint8_t cnt; static uint8_t cnt;
#endif #endif
// TODO: send appstate via Mqtt
switch (appState) { switch (appState) {
case APP_STATE_STARTUP: case APP_STATE_STARTUP:
#if PL_CONFIG_USE_BUZZER && PL_DO_MINT #if PL_CONFIG_USE_BUZZER && PL_DO_MINT

509
ADIS_Sumo_Styger/Sumo/LineFollow.c
Unescape View File

@ -0,0 +1,509 @@
/**
* \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 */

77
ADIS_Sumo_Styger/Sumo/LineFollow.h
Unescape View File

@ -0,0 +1,77 @@
/**
* \file
* \brief Interface to the line following module
* \author Erich Styger, erich.styger@hslu.ch
* \license SPDX-License-Identifier: BSD-3-Clause
* This is the interface to line following module.
*/
#ifndef LINEFOLLOW_H_
#define LINEFOLLOW_H_
#include "platform.h"
#if PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE
#include "Reflectance.h"
#if PL_CONFIG_USE_SHELL
#include "McuShell.h"
/*!
* \brief Module command line parser
* \param cmd Pointer to command string to be parsed
* \param handled Set to TRUE if command has handled by parser
* \param io Shell standard I/O handler
* \return Error code, ERR_OK if everything was ok
*/
uint8_t LF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io);
#endif
/*!
* \brief Start line following
*/
void LF_StartFollowing(void);
/*!
* \brief Stop line following
*/
void LF_StopFollowing(void);
/*!
* \brief Start/stop line following
*/
void LF_StartStopFollowing(void);
/*!
* \brief Function to determine if line following is active
* \return TRUE if currently line following, FALSE otherwise
*/
bool LF_IsFollowing(void);
/*!
* \brief Move from outside onto a line/segment to follow it.
* \return Returns TRUE if still on line
*/
bool LF_MoveOnSegment(bool turningLeft);
/*!
* \brief follows a line segment.
* \return Returns TRUE if still on line segment
*/
bool LF_FollowSegment(REF_LineKindMode mode, bool forward);
bool LF_FollowSegmentLinePos(REF_LineKindMode mode, uint16_t setLinePos);
/*!
* \brief Module initialization.
*/
void LF_Init(void);
/*!
* \brief Module de-initialization.
*/
void LF_Deinit(void);
#endif /* PL_CONFIG_APP_LINE_FOLLOWING || PL_CONFIG_APP_LINE_MAZE */
#endif /* LINEFOLLOW_H_ */

81
ADIS_Sumo_Styger/Sumo/LineHistory.c
Unescape View File

@ -0,0 +1,81 @@
/*
* Copyright (c) 2021, Erich Styger
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include "platform.h"
#if PL_CONFIG_USE_LINE_SENSOR
#include "LineHistory.h"
#include "Reflectance.h"
#define HISTORY_MIN_SENSOR_VAL REF_MIN_LINE_VAL /* minimum threshold value which is recorded in history */
static uint16_t SensorHistory[REF_NOF_SENSORS]; /* value of history while moving forward */
void HISTORY_SampleSensors(void) {
uint8_t i;
uint16_t val[REF_NOF_SENSORS];
REF_GetSensorValues(&val[0], REF_NOF_SENSORS);
for(i=0; i<REF_NOF_SENSORS; i++) {
if (val[i]>=HISTORY_MIN_SENSOR_VAL) { /* only count line values */
if (val[i]>SensorHistory[i]) {
SensorHistory[i] = val[i];
}
}
}
}
/*!
* \brief Can be called during turning, will use it to sample sensor values.
*/
bool HISTORY_SampleTurnStopFunction(void) {
HISTORY_SampleSensors();
return FALSE; /* do not stop turning */
}
REF_LineKind HISTORY_LineKind(void) {
int i, cnt, cntLeft, cntRight;
cnt = cntLeft = cntRight = 0;
for(i=0;i<REF_NOF_SENSORS;i++) {
if (SensorHistory[i]>=HISTORY_MIN_SENSOR_VAL) { /* count above threshold values */
cnt++;
#if REF_SENSOR1_IS_LEFT
if (i<REF_NOF_SENSORS/2) {
cntLeft++;
} else {
cntRight++;
}
#else
if (i<REF_NOF_SENSORS/2) {
cntRight++;
} else {
cntLeft++;
}
#endif
}
}
if (cnt==0) {
return REF_LINE_NONE;
} else if (cnt==REF_NOF_SENSORS) {
return REF_LINE_FULL;
} else if (SensorHistory[0]<HISTORY_MIN_SENSOR_VAL && SensorHistory[REF_NOF_SENSORS-1]<HISTORY_MIN_SENSOR_VAL) {
return REF_LINE_STRAIGHT; /* there is still white to the left and right */
} else if (SensorHistory[0]>=HISTORY_MIN_SENSOR_VAL) {
return REF_LINE_LEFT;
} else { /* must be cntRight>cntLeft */
return REF_LINE_RIGHT;
}
}
void HISTORY_Clear(void) {
int i;
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorHistory[i] = 0;
}
}
#endif /* PL_CONFIG_USE_LINE_SENSOR */

24
ADIS_Sumo_Styger/Sumo/LineHistory.h
Unescape View File

@ -0,0 +1,24 @@
/*
* Copyright (c) 2021, Erich Styger
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#ifndef SOURCES_INTRO_ROBOLIB_LINEHISTORY_H_
#define SOURCES_INTRO_ROBOLIB_LINEHISTORY_H_
#include "Reflectance.h"
#if PL_CONFIG_USE_LINE_SENSOR
void HISTORY_SampleSensors(void);
/*!
* \brief Can be called during turning, will use it to sample sensor values.
*/
bool HISTORY_SampleTurnStopFunction(void);
REF_LineKind HISTORY_LineKind(void);
void HISTORY_Clear(void);
#endif /* PL_CONFIG_USE_LINE_SENSOR */
#endif /* SOURCES_INTRO_ROBOLIB_LINEHISTORY_H_ */

489
ADIS_Sumo_Styger/Sumo/Maze.c
Unescape View File

@ -0,0 +1,489 @@
/**
* \file
* \brief Implementation of maze solving
* \author Erich Styger, erich.styger@hslu.ch
* \license SPDX-License-Identifier: BSD-3-Clause
* This module is used to solve a line maze.
*/
#include "platform.h"
#if PL_CONFIG_APP_LINE_MAZE
#include "Maze.h"
#include "Turn.h"
#include "McuShell.h"
#include "McuRTOS.h"
#if PL_CONFIG_USE_LEDS
#include "leds.h"
#include "McuLED.h"
#endif
#include "LineFollow.h"
#include "LineHistory.h"
#include "McuEvents.h"
#include "McuUtility.h"
#include "PID.h"
#include "Reflectance.h"
#include "Shell.h"
#if PL_CONFIG_HAS_NVM_CONFIG
#include "NVM_Config.h"
#endif
#if PL_CONFIG_USE_BUZZER
#include "Buzzer.h"
#endif
#define MAZE_DEBUG_PRINT (0) /* careful: this will slow down the PID loop frequency! */
#define MAZE_DO_DEBUG_WAIT (0 /*|| PL_SLOWER_SPEED*/) /* if set to one, it will add some debug wait to stop the engines */
#if MAZE_DO_DEBUG_WAIT
#define MAZE_DO_DEBUG_WAIT_MS (500) /* waiting time in milliseconds */
#endif
#if MAZE_DO_DEBUG_WAIT /* debug mode */
static void DebugStopAndWait(void) {
static volatile bool waitHere = FALSE;
TURN_Turn(TURN_STOP, NULL);
vTaskDelay(MAZE_DO_DEBUG_WAIT_MS);
waitHere = TRUE;
}
#endif
#define MAZE_MAX_PATH 64 /* maximum number of turns in path */
static TURN_Kind path[MAZE_MAX_PATH];
static uint8_t pathLength;
static bool isSolved = FALSE;
static bool useLeftHandOnTheWallRule = TRUE;
bool MAZE_IsLeftHandRule(void) {
return useLeftHandOnTheWallRule;
}
uint8_t MAZE_SetHandRule(bool isLeft) {
const NVMC_RobotData *ptr;
NVMC_RobotData data;
uint8_t res;
res = ERR_OK;
useLeftHandOnTheWallRule = isLeft;
ptr = NVMC_GetRobotData();
if (ptr!=NULL && ptr->MazeLeftHandOnTheWall!=useLeftHandOnTheWallRule) {
data = *NVMC_GetRobotData();
res = NVMC_SaveRobotData(&data);
}
return res;
}
/* used to convert path items while running backwards */
static TURN_Kind MirrorTurn(TURN_Kind turn) {
switch(turn) {
case TURN_LEFT90: return TURN_RIGHT90;
case TURN_RIGHT90: return TURN_LEFT90;
case TURN_LEFT180: return TURN_STRAIGHT;
case TURN_RIGHT180: return TURN_STRAIGHT;
case TURN_STRAIGHT: return TURN_STRAIGHT;
case TURN_STEP_LINE_FW: return TURN_STEP_LINE_BW;
case TURN_STEP_LINE_BW: return TURN_STEP_LINE_FW;
case TURN_STEP_POST_LINE_FW: return TURN_STEP_POST_LINE_BW;
case TURN_STEP_POST_LINE_BW: return TURN_STEP_POST_LINE_FW;
case TURN_STEP_LINE_FW_AND_POST_LINE: return TURN_STEP_LINE_BW_AND_POST_LINE;
case TURN_STEP_LINE_BW_AND_POST_LINE: return TURN_STEP_LINE_FW_AND_POST_LINE;
case TURN_STOP:
case TURN_FINISH:
default:
return TURN_STOP;
}
}
TURN_Kind MAZE_SelectTurnBw(REF_LineKind prev, REF_LineKind curr);
/*!
* \brief Performs a turn while doing backward line following.
* \return Returns TRUE while turn is still in progress.
*/
uint8_t MAZE_EvaluateTurnBw(void) {
REF_LineKind historyLineKind, currLineKind;
TURN_Kind turn;
HISTORY_Clear(); /* clear values */
HISTORY_SampleSensors(); /* store current values */
TURN_Turn(TURN_STEP_LINE_BW, HISTORY_SampleTurnStopFunction); /* make step over line */
historyLineKind = HISTORY_LineKind(); /* new read new values */
currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_MAZE);
#if MAZE_DEBUG_PRINT
SHELL_SendString((unsigned char*)" history: ");
SHELL_SendString((unsigned char*)REF_LineKindStr(historyLineKind));
SHELL_SendString((unsigned char*)" curr: ");
SHELL_SendString((unsigned char*)REF_LineKindStr(currLineKind));
SHELL_SendString((unsigned char*)"\r\n");
#endif
turn = MAZE_SelectTurnBw(historyLineKind, currLineKind);
if (turn==TURN_STOP) { /* should not happen here? */
LF_StopFollowing();
SHELL_SendString((unsigned char*)"stopped\r\n");
return ERR_FAILED; /* error case */
} else { /* turn or do something */
#if MAZE_DEBUG_PRINT
SHELL_SendString((unsigned char*)"bw turning ");
SHELL_SendString((unsigned char*)TURN_TurnKindStr(turn));
SHELL_SendString((unsigned char*)"\r\n");
#endif
TURN_Turn(TURN_STEP_LINE_FW_AND_POST_LINE, NULL); /* step over intersection */ /* ???? not good for going backward over -| */
TURN_Turn(turn, NULL); /* make turn */
MAZE_AddPath(MirrorTurn(turn));
MAZE_SimplifyPath();
return ERR_OK; /* turn finished */
}
}
/*!
* \brief Performs a turn.
* \return Returns TRUE while turn is still in progress.
*/
uint8_t MAZE_EvaluteTurn(bool *finished, bool *deadEndGoBw) {
REF_LineKind historyLineKind, currLineKind;
TURN_Kind turn;
*finished = FALSE; /* defaults */
*deadEndGoBw = FALSE; /* default */
currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_MAZE);
if (currLineKind==REF_LINE_NONE) { /* nothing, must be dead end */
turn = TURN_LEFT180;
} else {
HISTORY_Clear(); /* clear history values */
HISTORY_SampleSensors(); /* store current values */
#if PL_CONFIG_USE_QUADRATURE
TURN_Turn(TURN_STEP_LINE_FW_AND_POST_LINE, HISTORY_SampleTurnStopFunction); /* do the line and beyond in one step */
#else
TURN_Turn(TURN_STEP_LINE_FW, NULL); /* make forward step over line */
#endif
#if MAZE_DO_DEBUG_WAIT /* debug mode */
DebugStopAndWait();
#endif
historyLineKind = HISTORY_LineKind(); /* new read new values */
currLineKind = REF_GetLineKind(REF_LINE_KIND_MODE_MAZE);
turn = MAZE_SelectTurn(historyLineKind, currLineKind);
#if MAZE_DEBUG_PRINT
SHELL_SendString((unsigned char*)"selected turn: ");
SHELL_SendString((unsigned char*)TURN_TurnKindStr(turn));
SHELL_SendString((unsigned char*)"\r\n");
#endif
}
#if MAZE_DO_DEBUG_WAIT /* debug mode */
DebugStopAndWait();
#endif
if (turn==TURN_FINISH) {
*finished = TRUE;
SHELL_SendString((unsigned char*)"MAZE: finish area!\r\n");
#if PL_CONFIG_USE_BUZZER
BUZ_PlayTune(BUZ_TUNE_MAZE_DESTINATION);
#endif
return ERR_OK;
} else if (turn==TURN_STRAIGHT && *deadEndGoBw) {
MAZE_AddPath(TURN_LEFT180); /* would have been a turn around */
MAZE_SimplifyPath();
SHELL_SendString((unsigned char*)"MAZE: going backward\r\n");
return ERR_OK;
} else if (turn==TURN_STRAIGHT) {
MAZE_AddPath(turn);
MAZE_SimplifyPath();
SHELL_SendString((unsigned char*)"MAZE: going straight\r\n");
return ERR_OK;
} else if (turn==TURN_STOP) { /* should not happen here? */
LF_StopFollowing();
SHELL_SendString((unsigned char*)"MAZE: Failure, stopped!!!\r\n");
return ERR_FAILED; /* error case */
} else { /* turn or do something */
#if MAZE_DEBUG_PRINT
SHELL_SendString((unsigned char*)"turning ");
SHELL_SendString((unsigned char*)TURN_TurnKindStr(turn));
SHELL_SendString((unsigned char*)"\r\n");
#endif
#if !PL_CONFIG_USE_QUADRATURE /* if using quadrature, we stepped this piece already above */
if (turn==TURN_LEFT90 || turn==TURN_RIGHT90) {
TURN_Turn(TURN_STEP_POST_LINE_FW, NULL); /* step before doing the turn so we turn on the middle of the intersection */
}
#endif
TURN_Turn(turn, NULL); /* make turn */
MAZE_AddPath(turn);
MAZE_SimplifyPath();
return ERR_OK; /* turn finished */
}
}
static TURN_Kind RevertTurn(TURN_Kind turn) {
if (turn==TURN_LEFT90) {
turn = TURN_RIGHT90;
} else if (turn==TURN_RIGHT90) {
turn = TURN_LEFT90;
}
return turn;
}
/**
* \brief Reverts the path
*/
static void MAZE_RevertPath(void) {
int i, j;
TURN_Kind tmp;
if (pathLength==0) {
return;
}
j = pathLength-1;
i = 0;
while(i<=j) {
tmp = path[i];
path[i] = RevertTurn(path[j]);
path[j] = RevertTurn(tmp);
i++; j--;
}
}
TURN_Kind MAZE_SelectTurn(REF_LineKind prev, REF_LineKind curr) {
if (prev==REF_LINE_NONE && curr==REF_LINE_NONE) { /* dead end */
if (MAZE_IsLeftHandRule()) {
return TURN_RIGHT180; /* make U turn */
} else {
return TURN_LEFT180; /* make U turn */
}
} else if (prev==REF_LINE_FULL && curr==REF_LINE_NONE) { /* 'T' */
if (MAZE_IsLeftHandRule()) {
return TURN_LEFT90;
} else {
return TURN_RIGHT90;
}
} else if (prev==REF_LINE_RIGHT && curr==REF_LINE_NONE) { /* right turn */
return TURN_RIGHT90;
} else if (prev==REF_LINE_LEFT && curr==REF_LINE_NONE) { /* left turn */
return TURN_LEFT90;
} else if (prev==REF_LINE_FULL && curr==REF_LINE_STRAIGHT) { /* '+' intersection */
if (MAZE_IsLeftHandRule()) {
return TURN_LEFT90;
} else {
return TURN_RIGHT90;
}
} else if (prev==REF_LINE_FULL && curr==REF_LINE_FULL) { /* finish area */
return TURN_FINISH;
} else if (prev==REF_LINE_RIGHT && curr==REF_LINE_STRAIGHT) { /* forward and right turn */
if (MAZE_IsLeftHandRule()) {
return TURN_STRAIGHT;
} else {
return TURN_RIGHT90;
}
} else if (prev==REF_LINE_LEFT && curr==REF_LINE_STRAIGHT) { /* forward and left turn */
if (MAZE_IsLeftHandRule()) {
return TURN_LEFT90;
} else {
return TURN_STRAIGHT;
}
}
return TURN_STOP; /* error case */
}
TURN_Kind MAZE_SelectTurnBw(REF_LineKind prev, REF_LineKind curr) {
if (prev==REF_LINE_LEFT && curr==REF_LINE_NONE) { /* was turn to left */
return TURN_LEFT90;
} else if (prev==REF_LINE_RIGHT && curr==REF_LINE_NONE) { /* was turn to right */
return TURN_RIGHT90;
} else if (prev==REF_LINE_RIGHT && curr==REF_LINE_STRAIGHT) { /* |- */
if (MAZE_IsLeftHandRule()) {
return TURN_RIGHT90;
} else {
return TURN_LEFT180;
}
} else if (prev==REF_LINE_LEFT && curr==REF_LINE_STRAIGHT) { /* -| */
if (MAZE_IsLeftHandRule()) {
return TURN_RIGHT180;
} else {
return TURN_LEFT90;
}
} else if (prev==REF_LINE_FULL && curr==REF_LINE_NONE) { /* T upside-down */
if (MAZE_IsLeftHandRule()) {
return TURN_RIGHT90;
} else {
return TURN_LEFT90;
}
} else if (prev==REF_LINE_FULL && curr==REF_LINE_STRAIGHT) { /* '+' intersection */
if (MAZE_IsLeftHandRule()) {
return TURN_RIGHT90;
} else {
return TURN_LEFT90;
}
}
return TURN_STOP; /* error case */
}
void MAZE_SetSolved(void) {
isSolved = TRUE;
MAZE_RevertPath();
MAZE_AddPath(TURN_STOP); /* add an action to stop */
}
bool MAZE_IsSolved(void) {
return isSolved;
}
void MAZE_AddPath(TURN_Kind kind) {
if (pathLength<MAZE_MAX_PATH) {
path[pathLength] = kind;
pathLength++;
} else {
/* error! */
}
}
/*!
* \brief Performs path simplification.
* The idea is that whenever we encounter x-TURN_RIGHT180-x or x-TURN_LEFT180-x, we simplify it by cutting the dead end.
* For example if we have TURN_LEFT90-TURN_RIGHT180-TURN_LEFT90, this can be simplified with TURN_STRAIGHT.
*/
void MAZE_SimplifyPath(void) {
/* implemented for left-hand-on-the-wall rule for now! */
uint16_t totalAngle, i;
if (pathLength<3) { /* we need at least 3 entries for simplification */
return;
}
if (!(path[pathLength-2] == TURN_RIGHT180 || path[pathLength-2] == TURN_LEFT180)) {
/* we simplify only if second-to-last is a dead-end */
return;
}
totalAngle = 0;
for (i=1; i<=3; i++) {
switch(path[pathLength-i]) {
case TURN_LEFT90: totalAngle += 270; break;
case TURN_RIGHT90: totalAngle += 90; break;
case TURN_LEFT180: totalAngle += 180; break;
case TURN_RIGHT180: totalAngle += 180; break;
default:
/* would be an error? */
break;
} /* switch */
}
totalAngle %= 360; /* make sure angle is within 0...360 degrees */
switch(totalAngle) {
case 0: path[pathLength-3] = TURN_STRAIGHT; break;
case 90: path[pathLength-3] = TURN_RIGHT90; break;
case 180: path[pathLength-3] = TURN_LEFT180; break;
case 270: path[pathLength-3] = TURN_LEFT90; break;
}
pathLength -= 2; /* was able to cut the path by 2 entries :-) */
}
static void MAZE_PrintHelp(const McuShell_StdIOType *io) {
McuShell_SendHelpStr((unsigned char*)"maze", (unsigned char*)"Group of maze following commands\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Shows maze help or status\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" clear", (unsigned char*)"Clear the maze solution\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" rule (left|right)", (unsigned char*)"Left or Right-hand-on-the-wall rule\r\n", io->stdOut);
}
static void MAZE_PrintStatus(const McuShell_StdIOType *io) {
int i;
McuShell_SendStatusStr((unsigned char*)"maze", (unsigned char*)"maze solving status\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" rule", MAZE_IsLeftHandRule()?(unsigned char*)"left-hand-on-the-wall\r\n":(unsigned char*)"right-hand-on-the-wall\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" solved", MAZE_IsSolved()?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" path", (unsigned char*)"(", io->stdOut);
McuShell_SendNum8u(pathLength, io->stdOut);
McuShell_SendStr((unsigned char*)") ", io->stdOut);
for(i=0;i<pathLength;i++) {
McuShell_SendStr(TURN_TurnKindStr(path[i]), io->stdOut);
McuShell_SendStr((unsigned char*)" ", io->stdOut);
}
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
}
uint8_t MAZE_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*)"maze help")==0) {
MAZE_PrintHelp(io);
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)McuShell_CMD_STATUS)==0 || McuUtility_strcmp((char*)cmd, (char*)"maze status")==0) {
MAZE_PrintStatus(io);
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"maze clear")==0) {
MAZE_ClearSolution();
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"maze rule left")==0) {
useLeftHandOnTheWallRule = TRUE;
*handled = TRUE;
} else if (McuUtility_strcmp((char*)cmd, (char*)"maze rule right")==0) {
useLeftHandOnTheWallRule = FALSE;
*handled = TRUE;
}
return res;
}
TURN_Kind MAZE_GetSolvedTurn(uint8_t *solvedIdx) {
if (*solvedIdx < pathLength) {
return path[(*solvedIdx)++];
} else {
return TURN_STOP;
}
}
void MAZE_ClearSolution(void) {
isSolved = FALSE;
pathLength = 0;
}
#if 0
typedef enum {
MAZE_STATE_INIT,
MAZE_STATE_IDLE,
MAZE_STATE_FOLLOW_LINE
} MazeStateType;
static MazeStateType mazeState = MAZE_STATE_INIT;
static void MAZE_StateMachine(void) {
switch (mazeState) {
case MAZE_STATE_INIT:
#if PL_CONFIG_USE_LEDS
McuLED_On(LEDS_Left);
McuLED_On(LEDS_Right);
#endif
if (REF_CanUseSensor()) {
mazeState = MAZE_STATE_IDLE;
}
break;
case MAZE_STATE_IDLE:
if (!REF_CanUseSensor()) { /* sensor not available any more? */
mazeState = MAZE_STATE_INIT;
}
#if PL_CONFIG_USE_LEDS
McuLED_Off(LEDS_Left);
McuLED_On(LEDS_Right);
#endif
if (EVNT_EventIsSet(EVNT_MAZE_BUTTON_PRESS)) {
EVNT_ClearEvent(EVNT_MAZE_BUTTON_PRESS);
LF_StartFollowing();
mazeState = MAZE_STATE_FOLLOW_LINE;
}
break;
case MAZE_STATE_FOLLOW_LINE:
#if PL_CONFIG_USE_LEDS
McuLED_Off(LEDS_Left);
McuLED_Off(LEDS_Right);
#endif
if (!LF_IsFollowing()) {
mazeState = MAZE_STATE_IDLE;
}
if (EVNT_EventIsSet(EVNT_MAZE_BUTTON_PRESS)) {
EVNT_ClearEvent(EVNT_MAZE_BUTTON_PRESS);
LF_StopFollowing();
mazeState = MAZE_STATE_IDLE;
}
break;
} /* switch */
}
#endif
void MAZE_Init(void) {
const NVMC_RobotData *ptr;
ptr = NVMC_GetRobotData();
if (ptr!=NULL) {
(void)MAZE_SetHandRule(ptr->MazeLeftHandOnTheWall);
}
MAZE_ClearSolution();
}
#endif /* PL_CONFIG_APP_LINE_MAZE */

93
ADIS_Sumo_Styger/Sumo/Maze.h
Unescape View File

@ -0,0 +1,93 @@
/*
* Copyright (c) 2021, Erich Styger
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#ifndef MAZE_H_
#define MAZE_H_
#include "platform.h"
#if PL_CONFIG_APP_LINE_MAZE
#include "Turn.h"
/*!
* \brief Adds a new path while going forward through the maze
* \param kind New path to be added
*/
void MAZE_AddPath(TURN_Kind kind);
/*!
* \brief Tries to simplify the path, basically cutting dead end paths.
*/
void MAZE_SimplifyPath(void);
/*!
* \brief Returns TRUE if the maze has been solved (finish has been found)
* \return TRUE if finish has been found, so maze is solved
*/
bool MAZE_IsSolved(void);
/*!
* \brief Marks the maze as solved.
*/
void MAZE_SetSolved(void);
/*!
* This clears the solution, and MAZE_IsSolved() will return FALSE
*/
void MAZE_ClearSolution(void);
/*!
* \brief Used to get the solution turns, one after each other
* \param solvedIdx Solution index, starting with zero. The callee will increment the index.
* \return Solution turn
*/
TURN_Kind MAZE_GetSolvedTurn(uint8_t *solvedIdx);
/*!
* \brief Selects the new turn based.
* \param prev Line previous the intersection
* \param curr Line kind of the intersection.
* \return The new turn.
*/
TURN_Kind MAZE_SelectTurn(REF_LineKind prev, REF_LineKind curr);
/*!
* \brief Function which returns the current strategy
* \return Returns TRUE if using left-hand-on-the-wall, FALSE otherwise
*/
bool MAZE_IsLeftHandRule(void);
/*!
* \brief Performs a turn while going forward over a line.
* \param finished TRUE if reached finished area
* \pram deadEndGoBW TRUE if found dead end and going backward
* \return Returns TRUE while turn is still in progress.
*/
uint8_t MAZE_EvaluteTurn(bool *finished, bool *deadEndGoBw);
uint8_t MAZE_EvaluateTurnBw(void);
#if PL_CONFIG_USE_SHELL
#include "McuShell.h"
/*!
* \brief Module command line parser
* \param cmd Pointer to command string to be parsed
* \param handled Set to TRUE if command has handled by parser
* \param io Shell standard I/O handler
* \return Error code, ERR_OK if everything was ok
*/
uint8_t MAZE_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io);
#endif
uint8_t MAZE_SetHandRule(bool isLeft);
/*!
* \brief Module initialization.
*/
void MAZE_Init(void);
#endif /* PL_CONFIG_APP_LINE_MAZE */
#endif /* MAZE_H_ */

33
ADIS_Sumo_Styger/Sumo/Pid.c
Unescape View File

@ -158,21 +158,9 @@ static void PID_LineCfg(uint16_t currLine, uint16_t setLine, uint16_t currLineWi
speedL = speed; speedL = speed;
} }
} }
#elif 0 || PL_DO_MINT /* aggressive line following! */
} else {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* scale to base speed */
if (pid<0) { /* turn right */
speedR = speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = speed-pid; /* decrease speed */
}
}
#else #else
} else if (errorPercent <= 10) { /* pretty on center: move forward both motors with base speed */ } else if (errorPercent <= 10) { /* pretty on center: move forward both motors with base speed */
//speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */ speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
speed = 65*(0xffff/100);
pid = Limit(pid, -speed, speed); pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */ if (pid<0) { /* turn right */
speedR = speed; speedR = speed;
@ -183,8 +171,7 @@ static void PID_LineCfg(uint16_t currLine, uint16_t setLine, uint16_t currLineWi
} }
} else if (errorPercent <= 30) { } else if (errorPercent <= 30) {
/* outside left/right halve position from center, slow down one motor and speed up the other */ /* outside left/right halve position from center, slow down one motor and speed up the other */
// speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* 80% */ speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* 80% */
speed = 40*(0xffff/100);
pid = Limit(pid, -speed, speed); pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */ if (pid<0) { /* turn right */
speedR = speed+pid; /* decrease speed */ speedR = speed+pid; /* decrease speed */
@ -193,23 +180,9 @@ static void PID_LineCfg(uint16_t currLine, uint16_t setLine, uint16_t currLineWi
speedR = speed+pid; /* increase speed */ speedR = speed+pid; /* increase speed */
speedL = speed-pid; /* decrease speed */ speedL = speed-pid; /* decrease speed */
} }
#if 0
} else if (errorPercent <= 40) {
// speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %70 */
speed = 100*(0xffff/100);
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
}
#endif
} else { } else {
/* line is far to the left or right: use backward motor motion */ /* line is far to the left or right: use backward motor motion */
// speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %100 */ speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %100 */
speed = 100*(0xffff/100);
if (pid<0) { /* turn right */ if (pid<0) { /* turn right */
speedR = -speed+pid; /* decrease speed */ speedR = -speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */ speedL = speed-pid; /* increase speed */

793
ADIS_Sumo_Styger/Sumo/Reflectance.c
Unescape View File

@ -0,0 +1,793 @@
/**
* \file
* \brief Reflectance sensor driver implementation.
* \author Erich Styger, erich.styger@hslu.ch
* \license SPDX-License-Identifier: BSD-3-Clause
* This module implements the driver for the bot front sensor.
*/
#include "platform.h"
#if PL_CONFIG_USE_LINE_SENSOR
#include "Reflectance.h"
#include "McuRTOS.h"
#include "McuGPIO.h"
#include "McuUtility.h"
#include "McuWait.h"
#if PL_CONFIG_USE_BUZZER
#include "Buzzer.h"
#endif
#include "Application.h"
#if PL_CONFIG_HAS_NVM_CONFIG
#include "NVM_Config.h"
#endif
#include "Shell.h"
#include "Timer.h"
typedef enum {
REF_STATE_INIT,
REF_STATE_NOT_CALIBRATED,
REF_STATE_START_CALIBRATION,
REF_STATE_CALIBRATING,
REF_STATE_STOP_CALIBRATION,
REF_STATE_SAVE_CALIBRATION,
REF_STATE_READY
} RefStateType;
static volatile RefStateType refState = REF_STATE_INIT;
static struct {
bool isEnabled; /* if sensor is enabled or not. If not enabled, it does not do any measurements */
bool savePower; /* if true, the IR LEDs are turned off between measurements */
McuGPIO_Handle_t irOn; /* HIGH active, pin to turn on/off power to the IR LEDs */
McuGPIO_Handle_t sensor[REF_NOF_SENSORS]; /* pins for the IR sensor */
} REF_Sensor;
#define REF_USE_WHITE_LINE 0 /* if set to 1, then the robot is using a white (on black) line, otherwise a black (on white) line */
#define REF_START_STOP_CALIB 1 /* start/stop calibration commands */
#if REF_START_STOP_CALIB
static SemaphoreHandle_t REF_StartStopSem = NULL;
#endif
#define REF_TIMEOUT_TICKS (((TMR_GetRefelectanceTimerFrequencyHz()/1000)*REF_SENSOR_TIMEOUT_US)/1000) /* REF_SENSOR_TIMEOUT_US translated into timeout ticks */
typedef uint16_t SensorTimeType;
#define MAX_SENSOR_VALUE ((SensorTimeType)-1)
/* type of NVM Configuration data (in FLASH) */
typedef struct SensorCalibT_ {
SensorTimeType minVal[REF_NOF_SENSORS];
SensorTimeType maxVal[REF_NOF_SENSORS];
} SensorCalibT;
static SensorCalibT *SensorCalibMinMaxPtr; /* pointer to calibrated data in FLASH, NULL if not calibrated */
static SensorCalibT *SensorCalibMinMaxTmpPtr=NULL; /* temporary calibration data */
static SensorTimeType SensorRaw[REF_NOF_SENSORS]; /* raw sensor values */
static SensorTimeType SensorCalibrated[REF_NOF_SENSORS]; /* 0 means white/min value, 1000 means black/max value */
static bool REF_LedOn = TRUE;
static int16_t refCenterLineVal=0; /* 0 means no line, >0 means line is below sensor 0, 1000 below sensor 1 and so on */
static REF_LineKind refLineKind = REF_LINE_NONE;
static uint16_t refLineWidth = 0;
void REF_GetSensorValues(uint16_t *values, int nofValues) {
int i;
for(i=0;i<nofValues && i<REF_NOF_SENSORS;i++) {
values[i] = SensorCalibrated[i];
}
}
/*!
* \brief Measures the time until the sensor discharges
* \param raw Array to store the raw values.
* \return ERR_OVERFLOW if there is a timeout, ERR_OK otherwise
*/
static bool REF_MeasureRaw(SensorTimeType raw[REF_NOF_SENSORS], uint32_t timeoutCntVal) {
uint8_t cnt; /* number of sensor */
uint8_t i;
uint32_t timerVal;
if (!REF_Sensor.isEnabled) {
return ERR_DISABLED;
}
if (REF_Sensor.savePower) {
McuGPIO_SetHigh(REF_Sensor.irOn); /* turn IR LED's on */
McuWait_Waitus(200);
}
for(i=0;i<REF_NOF_SENSORS;i++) {
McuGPIO_SetAsOutput(REF_Sensor.sensor[i], true); /* turn I/O line as output */
McuGPIO_SetHigh(REF_Sensor.sensor[i]);
raw[i] = MAX_SENSOR_VALUE;
}
McuWait_Waitus(50); /* give at least 10 us to charge the capacitor */
taskENTER_CRITICAL();
for(i=0;i<REF_NOF_SENSORS;i++) {
McuGPIO_SetAsInput(REF_Sensor.sensor[i]); /* turn I/O line as input */
}
TMR_ResetReflectanceTimerCounter();
TMR_StartReflectanceTimer(); /* start counting */
do {
cnt = 0;
timerVal = TMR_GetReflectanceTimerCounter();
if (timerVal>timeoutCntVal) {
break; /* get out of while loop */
}
for(i=0;i<REF_NOF_SENSORS;i++) {
if (raw[i]==MAX_SENSOR_VALUE) { /* not measured yet? */
if (McuGPIO_IsLow(REF_Sensor.sensor[i])) {
raw[i] = timerVal;
}
} else { /* have value */
cnt++;
}
}
} while(cnt!=REF_NOF_SENSORS);
taskEXIT_CRITICAL();
TMR_StopReflectanceTimer();
if (REF_Sensor.savePower) {
McuGPIO_SetLow(REF_Sensor.irOn);; /* turn IR LED's off */
}
return ERR_OK;
}
static void REF_CalibrateMinMax(SensorTimeType min[REF_NOF_SENSORS], SensorTimeType max[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS]) {
int i;
if (REF_MeasureRaw(raw, REF_TIMEOUT_TICKS)==ERR_OK) { /* if timeout, do not count values */
for(i=0;i<REF_NOF_SENSORS;i++) {
if (raw[i] < min[i]) {
min[i] = raw[i];
}
if (raw[i]> max[i]) {
max[i] = raw[i];
}
}
}
}
static void ReadCalibrated(SensorTimeType calib[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS]) {
int i;
int32_t x, denominator;
uint32_t max;
max = 0; /* determine maximum timeout value */
for(i=0;i<REF_NOF_SENSORS;i++) {
if (SensorCalibMinMaxPtr->maxVal[i]>max) {
max = SensorCalibMinMaxPtr->maxVal[i];
}
}
if (max > REF_TIMEOUT_TICKS) { /* limit to timeout value */
max = REF_TIMEOUT_TICKS;
}
(void)REF_MeasureRaw(raw, max);
if (SensorCalibMinMaxPtr==NULL) { /* no calibration data? */
return;
}
for(i=0;i<REF_NOF_SENSORS;i++) {
x = 0;
denominator = SensorCalibMinMaxPtr->maxVal[i]-SensorCalibMinMaxPtr->minVal[i];
if (denominator!=0) {
x = (((int32_t)raw[i]-SensorCalibMinMaxPtr->minVal[i])*1000)/denominator;
}
if (x<0) {
x = 0;
} else if (x>1000) {
x = 1000;
}
calib[i] = x;
}
}
/*
* Operates the same as read calibrated, but also returns an
* estimated position of the robot with respect to a line. The
* estimate is made using a weighted average of the sensor indices
* multiplied by 1000, so that a return value of 1000 indicates that
* the line is directly below sensor 0, a return value of 2000
* indicates that the line is directly below sensor 1, 2000
* indicates that it's below sensor 2000, etc. Intermediate
* values indicate that the line is between two sensors. The
* formula is:
*
* 1000*value0 + 2000*value1 + 3000*value2 + ...
* --------------------------------------------
* value0 + value1 + value2 + ...
*
* By default, this function assumes a dark line (high values)
* surrounded by white (low values). If your line is light on
* black, set the optional second argument white_line to true. In
* this case, each sensor value will be replaced by (1000-value)
* before the averaging.
*/
#define RETURN_LAST_VALUE 0
static int ReadLine(SensorTimeType calib[REF_NOF_SENSORS], SensorTimeType raw[REF_NOF_SENSORS], bool white_line) {
int i;
unsigned long avg; /* this is for the weighted total, which is long */
/* before division */
unsigned int sum; /* this is for the denominator which is <= 64000 */
unsigned int mul; /* multiplication factor, 0, 1000, 2000, 3000 ... */
int value;
#if RETURN_LAST_VALUE
bool on_line = FALSE;
static int last_value=0; /* assume initially that the line is left. */
#endif
(void)raw; /* unused */
avg = 0;
sum = 0;
mul = 1000;
#if REF_SENSOR1_IS_LEFT
for(i=0;i<REF_NOF_SENSORS;i++) {
#else
for(i=REF_NOF_SENSORS-1;i>=0;i--) {
#endif
value = calib[i];
if(white_line) {
value = 1000-value;
}
/* only average in values that are above a noise threshold */
if(value > REF_MIN_NOISE_VAL) {
avg += ((long)value)*mul;
sum += value;
}
mul += 1000;
}
#if RETURN_LAST_VALUE
/* keep track of whether we see the line at all */
if(value > REF_MIN_LINE_VAL) {
on_line = TRUE;
}
if(!on_line) { /* If it last read to the left of center, return 0. */
if(last_value < endIdx*1000/2) {
return 0;
} else { /* If it last read to the right of center, return the max. */
return endIdx*1000;
}
}
last_value = avg/sum;
return last_value;
#else
if (sum>0) {
return avg/sum;
} else {
return avg;
}
#endif
}
unsigned char *REF_LineKindStr(REF_LineKind line) {
switch(line) {
case REF_LINE_NONE:
return (unsigned char *)"NONE";
case REF_LINE_STRAIGHT:
return (unsigned char *)"STRAIGHT";
case REF_LINE_LEFT:
return (unsigned char *)"LEFT";
case REF_LINE_RIGHT:
return (unsigned char *)"RIGHT";
case REF_LINE_FULL:
return (unsigned char *)"FULL";
case REF_LINE_AIR:
return (unsigned char *)"AIR";
default:
return (unsigned char *)"unknown";
} /* switch */
}
REF_LineKind REF_GetLineKind(REF_LineKindMode mode) {
if (mode==REF_LINE_KIND_MODE_ALL || mode==REF_LINE_KIND_MODE_MAZE || mode==REF_LINE_KIND_MODE_SUMO) {
return refLineKind;
} else if (mode==REF_LINE_KIND_MODE_LINE_FOLLOW) {
switch(refLineKind) {
case REF_LINE_NONE:
return REF_LINE_NONE;
case REF_LINE_LEFT:
case REF_LINE_RIGHT:
case REF_LINE_STRAIGHT:
return REF_LINE_STRAIGHT;
case REF_LINE_AIR:
case REF_LINE_FULL:
return REF_LINE_NONE;
default:
return REF_LINE_NONE;
}
}
return refLineKind;
}
static bool SensorsSaturated(void) {
#if 0
int i, cnt;
/* check if robot is in the air or does see something */
cnt = 0;
for(i=0;i<REF_NOF_SENSORS;i++) {
if (SensorRaw[i]==MAX_SENSOR_VALUE) { /* sensor not seeing anything? */
cnt++;
}
}
return (cnt==REF_NOF_SENSORS); /* all sensors see raw max value: not on the ground? */
#else
return FALSE;
#endif
}
static REF_LineKind ReadLineKind(SensorTimeType val[REF_NOF_SENSORS]) {
uint32_t sum, sumLeft, sumRight, outerLeft, outerRight, nofLines;
int i;
/* check if robot is in the air or does see something */
if (SensorsSaturated()) {
return REF_LINE_AIR;
}
for(i=0;i<REF_NOF_SENSORS;i++) {
if (val[i]<REF_MIN_LINE_VAL) { /* smaller value? White seen! */
break;
}
}
if (i==REF_NOF_SENSORS) { /* all sensors see 'black' */
return REF_LINE_FULL;
}
/* check the line type */
sum = 0; sumLeft = 0; sumRight = 0; nofLines = 0;
for(i=0;i<REF_NOF_SENSORS;i++) {
if (val[i]>REF_MIN_LINE_VAL) { /* count only line values */
nofLines++;
sum += val[i];
if (i<REF_NOF_SENSORS/2) {
#if REF_SENSOR1_IS_LEFT
sumLeft += val[i];
#else
sumRight += val[i];
#endif
} else {
#if REF_SENSOR1_IS_LEFT
sumRight += val[i];
#else
sumLeft += val[i];
#endif
}
}
}
#if REF_SENSOR1_IS_LEFT
outerLeft = val[0];
outerRight = val[REF_NOF_SENSORS-1];
#else
outerLeft = val[REF_NOF_SENSORS-1];
outerRight = val[0];
#endif
#define MIN_LEFT_RIGHT_SUM ((((REF_NOF_SENSORS)*2)/3)*REF_MIN_LINE_VAL) /* 2/3 of half the number of the sensors shall see a line */
if (outerLeft<REF_MIN_LINE_VAL && outerRight<REF_MIN_LINE_VAL && nofLines>0) {
/* both leftmost and rightmost sensors are seeing white, middle is seeing at least one black line */
return REF_LINE_STRAIGHT; /* straight line forward */
}
if (outerLeft>=REF_MIN_LINE_VAL && outerRight<REF_MIN_LINE_VAL && sumLeft>MIN_LEFT_RIGHT_SUM && sumRight<MIN_LEFT_RIGHT_SUM) {
return REF_LINE_LEFT; /* line going to the left side */
} else if (outerLeft<REF_MIN_LINE_VAL && outerRight>=REF_MIN_LINE_VAL && sumRight>MIN_LEFT_RIGHT_SUM && sumLeft<MIN_LEFT_RIGHT_SUM) {
return REF_LINE_RIGHT; /* line going to the right side */
} else if (outerLeft>=REF_MIN_LINE_VAL && outerRight>=REF_MIN_LINE_VAL && sumRight>MIN_LEFT_RIGHT_SUM && sumLeft>MIN_LEFT_RIGHT_SUM) {
return REF_LINE_FULL; /* full line */
} else if (sumRight==0 && sumLeft==0 && sum == 0) {
return REF_LINE_NONE; /* no line */
} else {
return REF_LINE_STRAIGHT; /* straight line forward */
}
}
uint16_t REF_LineWidth(void) {
return refLineWidth;
}
static uint16_t CalculateRefLineWidth(SensorTimeType calib[REF_NOF_SENSORS]) {
int32_t val;
int i;
val = 0;
for(i=0;i<REF_NOF_SENSORS;i++) {
if (calib[i]>=REF_MIN_NOISE_VAL) { /* sensor not seeing anything? */
val += calib[i];
}
}
return (uint16_t)val;
}
static void REF_Measure(void) {
ReadCalibrated(SensorCalibrated, SensorRaw);
refCenterLineVal = ReadLine(SensorCalibrated, SensorRaw, REF_USE_WHITE_LINE);
refLineWidth = CalculateRefLineWidth(SensorCalibrated);
if (refState==REF_STATE_READY) {
refLineKind = ReadLineKind(SensorCalibrated);
}
}
#if REF_START_STOP_CALIB
void REF_CalibrateStartStop(void) {
REF_Sensor.isEnabled = TRUE;
if (refState==REF_STATE_NOT_CALIBRATED || refState==REF_STATE_CALIBRATING || refState==REF_STATE_READY) {
(void)xSemaphoreGive(REF_StartStopSem);
}
}
#endif
bool REF_CanUseSensor(void) {
return refState==REF_STATE_READY;
}
uint16_t REF_GetLineValue(bool *onLine) {
*onLine = refCenterLineVal>0 && refCenterLineVal<REF_MAX_LINE_VALUE;
return refCenterLineVal;
}
#if PL_CONFIG_USE_SHELL
static uint8_t PrintHelp(const McuShell_StdIOType *io) {
McuShell_SendHelpStr((unsigned char*)"ref", (unsigned char*)"Group of Reflectance commands\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" help|status", (unsigned char*)"Print help or status information\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" enable|disable", (unsigned char*)"Enables or disables the reflectance measurement\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" calib (start|stop)", (unsigned char*)"Start/Stop calibrating while moving sensor over line\r\n", io->stdOut);
McuShell_SendHelpStr((unsigned char*)" save power (on|off)", (unsigned char*)"Saves power with turning off IR between measurements\r\n", io->stdOut);
return ERR_OK;
}
static unsigned char*REF_GetStateString(void) {
switch (refState) {
case REF_STATE_INIT: return (unsigned char*)"INIT";
case REF_STATE_NOT_CALIBRATED: return (unsigned char*)"NOT CALIBRATED";
case REF_STATE_START_CALIBRATION: return (unsigned char*)"START CALIBRATION";
case REF_STATE_CALIBRATING: return (unsigned char*)"CALIBRATING";
case REF_STATE_STOP_CALIBRATION: return (unsigned char*)"STOP CALIBRATION";
case REF_STATE_SAVE_CALIBRATION: return (unsigned char*)"SAVE CALIBRATION";
case REF_STATE_READY: return (unsigned char*)"READY";
default:
break;
} /* switch */
return (unsigned char*)"UNKNOWN";
}
static uint8_t PrintStatus(const McuShell_StdIOType *io) {
unsigned char buf[32];
int i;
McuShell_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"IR line sensor status\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" enabled", REF_Sensor.isEnabled?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
#if REF_USE_WHITE_LINE
McuShell_SendStatusStr((unsigned char*)" line", (unsigned char*)"white\r\n", io->stdOut);
#else
McuShell_SendStatusStr((unsigned char*)" line", (unsigned char*)"black\r\n", io->stdOut);
#endif
McuShell_SendStatusStr((unsigned char*)" state", REF_GetStateString(), io->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" save power", REF_Sensor.savePower?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
McuUtility_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
McuUtility_strcatNum16Hex(buf, sizeof(buf), REF_MIN_NOISE_VAL);
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
McuShell_SendStatusStr((unsigned char*)" min noise", buf, io->stdOut);
McuUtility_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
McuUtility_strcatNum16Hex(buf, sizeof(buf), REF_MIN_LINE_VAL);
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
McuShell_SendStatusStr((unsigned char*)" min line", buf, io->stdOut);
McuUtility_Num16uToStr(buf, sizeof(buf), REF_SENSOR_TIMEOUT_US);
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" us, 0x");
McuUtility_strcatNum16Hex(buf, sizeof(buf), REF_TIMEOUT_TICKS);
McuUtility_strcat(buf, sizeof(buf), (unsigned char*)" ticks\r\n");
McuShell_SendStatusStr((unsigned char*)" timeout", buf, io->stdOut);
McuShell_SendStatusStr((unsigned char*)" raw val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
McuShell_SendStr((unsigned char*)"0x", io->stdOut);
} else {
McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
McuShell_SendStr(buf, io->stdOut);
}
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
if (SensorCalibMinMaxPtr!=NULL) { /* have calibration data */
McuShell_SendStatusStr((unsigned char*)" min val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
McuShell_SendStr((unsigned char*)"0x", io->stdOut);
} else {
McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMaxPtr->minVal[i]);
McuShell_SendStr(buf, io->stdOut);
}
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
}
if (SensorCalibMinMaxPtr!=NULL) {
McuShell_SendStatusStr((unsigned char*)" max val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
McuShell_SendStr((unsigned char*)"0x", io->stdOut);
} else {
McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorCalibMinMaxPtr->maxVal[i]);
McuShell_SendStr(buf, io->stdOut);
}
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
}
if (SensorCalibMinMaxPtr!=NULL) { /* have calibration data */
McuShell_SendStatusStr((unsigned char*)" calib val", (unsigned char*)"", io->stdOut);
#if REF_SENSOR1_IS_LEFT
for (i=0;i<REF_NOF_SENSORS;i++) {
if (i==0) {
#else
for (i=REF_NOF_SENSORS-1;i>=0;i--) {
if (i==REF_NOF_SENSORS-1) {
#endif
McuShell_SendStr((unsigned char*)"0x", io->stdOut);
} else {
McuShell_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; McuUtility_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
McuShell_SendStr(buf, io->stdOut);
}
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
}
McuShell_SendStatusStr((unsigned char*)" line pos", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; McuUtility_strcatNum16s(buf, sizeof(buf), refCenterLineVal);
McuShell_SendStr(buf, io->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" line width", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; McuUtility_strcatNum16s(buf, sizeof(buf), refLineWidth);
McuShell_SendStr(buf, io->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
McuShell_SendStatusStr((unsigned char*)" line kind", REF_LineKindStr(refLineKind), io->stdOut);
McuShell_SendStr((unsigned char*)"\r\n", io->stdOut);
return ERR_OK;
}
uint8_t REF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
if (McuUtility_strcmp((char*)cmd, McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, "ref help")==0) {
*handled = TRUE;
return PrintHelp(io);
} else if ((McuUtility_strcmp((char*)cmd, McuShell_CMD_STATUS)==0) || (McuUtility_strcmp((char*)cmd, "ref status")==0)) {
*handled = TRUE;
return PrintStatus(io);
} else if (McuUtility_strcmp((char*)cmd, "ref enable")==0) {
REF_Sensor.isEnabled = TRUE;
*handled = TRUE;
return ERR_OK;
} else if (McuUtility_strcmp((char*)cmd, "ref disable")==0) {
REF_Sensor.isEnabled = FALSE;
*handled = TRUE;
return ERR_OK;
} else if (McuUtility_strcmp((char*)cmd, "ref calib start")==0) {
if (refState==REF_STATE_NOT_CALIBRATED || refState==REF_STATE_READY) {
APP_StateStartCalibrate();
} else {
McuShell_SendStr((unsigned char*)"ERROR: cannot start calibration, must not be calibrating or be ready.\r\n", io->stdErr);
return ERR_FAILED;
}
*handled = TRUE;
return ERR_OK;
} else if (McuUtility_strcmp((char*)cmd, "ref calib stop")==0) {
if (refState==REF_STATE_CALIBRATING) {
APP_StateStopCalibrate();
} else {
McuShell_SendStr((unsigned char*)"ERROR: can only stop if calibrating.\r\n", io->stdErr);
return ERR_FAILED;
}
*handled = TRUE;
return ERR_OK;
} else if (McuUtility_strcmp((char*)cmd, "ref save power on")==0) {
REF_Sensor.savePower = true;
McuGPIO_SetLow(REF_Sensor.irOn);; /* turn IR LED's off */
*handled = TRUE;
return ERR_OK;
} else if (McuUtility_strcmp((char*)cmd, "ref save power off")==0) {
REF_Sensor.savePower = false;
*handled = TRUE;
return ERR_OK;
}
return ERR_OK;
}
#endif /* PL_CONFIG_USE_SHELL */
static void REF_StateMachine(void) {
int i;
switch (refState) {
case REF_STATE_INIT:
#if PL_CONFIG_HAS_NVM_CONFIG
SensorCalibMinMaxPtr = NVMC_GetReflectanceData();
if (SensorCalibMinMaxPtr!=NULL) { /* use calibration data from FLASH */
refState = REF_STATE_READY;
} else {
#if PL_CONFIG_USE_SHELL
SHELL_SendString((unsigned char*)"no calibration data present.\r\n");
#endif
refState = REF_STATE_NOT_CALIBRATED;
}
#else
refState = REF_STATE_NOT_CALIBRATED;
#endif
break;
case REF_STATE_NOT_CALIBRATED:
vTaskDelay(pdMS_TO_TICKS(80)); /* no need to sample that fast: this gives 80+20=100 ms */
(void)REF_MeasureRaw(SensorRaw, REF_TIMEOUT_TICKS);
#if REF_START_STOP_CALIB
if (xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
case REF_STATE_START_CALIBRATION:
#if PL_CONFIG_USE_SHELL
SHELL_SendString((unsigned char*)"start calibration...\r\n");
#endif
if (SensorCalibMinMaxTmpPtr!=NULL) {
refState = REF_STATE_INIT; /* error case */
break;
}
SensorCalibMinMaxTmpPtr = pvPortMalloc(sizeof(SensorCalibT));
if (SensorCalibMinMaxTmpPtr!=NULL) { /* success */
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorCalibMinMaxTmpPtr->minVal[i] = MAX_SENSOR_VALUE;
SensorCalibMinMaxTmpPtr->maxVal[i] = 0;
SensorCalibrated[i] = 0;
}
refState = REF_STATE_CALIBRATING;
} else {
refState = REF_STATE_INIT; /* error case */
}
break;
case REF_STATE_CALIBRATING:
vTaskDelay(pdMS_TO_TICKS(80)); /* no need to sample that fast: this gives 80+20=100 ms */
if (SensorCalibMinMaxTmpPtr!=NULL) { /* safety check */
REF_CalibrateMinMax(SensorCalibMinMaxTmpPtr->minVal, SensorCalibMinMaxTmpPtr->maxVal, SensorRaw);
} else {
refState = REF_STATE_INIT; /* error case */
break;
}
#if PL_CONFIG_USE_BUZZER
(void)BUZ_Beep(300, 50);
#endif
#if REF_START_STOP_CALIB
if (xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_STOP_CALIBRATION;
}
#endif
break;
case REF_STATE_STOP_CALIBRATION:
#if PL_CONFIG_USE_SHELL
SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
#endif
refState = REF_STATE_SAVE_CALIBRATION;
break;
case REF_STATE_SAVE_CALIBRATION:
#if PL_CONFIG_HAS_NVM_CONFIG
if (NVMC_SaveReflectanceData((void*)SensorCalibMinMaxTmpPtr, sizeof(SensorCalibT))!=ERR_OK) {
#if PL_CONFIG_USE_SHELL
SHELL_SendString((unsigned char*)"FAILED storing to FLASH!?!\r\n");
#endif
}
/* free memory */
vPortFree(SensorCalibMinMaxTmpPtr);
SensorCalibMinMaxTmpPtr = NULL;
SensorCalibMinMaxPtr = NVMC_GetReflectanceData();
if (SensorCalibMinMaxPtr!=NULL) { /* use calibration data from FLASH */
#if PL_CONFIG_USE_SHELL
SHELL_SendString((unsigned char*)"calibration data saved.\r\n");
#endif
refState = REF_STATE_READY;
break;
} else {
refState = REF_STATE_INIT; /* error case */
}
#else
refState = REF_STATE_READY;
#endif
break;
case REF_STATE_READY:
REF_Measure();
#if REF_START_STOP_CALIB
if (xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
} /* switch */
}
static void ReflTask(void *pvParameters) {
(void)pvParameters; /* not used */
for(;;) {
REF_StateMachine();
vTaskDelay(5/portTICK_PERIOD_MS);
}
}
void REF_Init(void) {
McuGPIO_Config_t gpioConfig;
McuGPIO_HwPin_t hw[REF_NOF_SENSORS] =
{
{.gpio=GPIOD, .port=PORTD, .pin=2},
{.gpio=GPIOD, .port=PORTD, .pin=3},
{.gpio=GPIOD, .port=PORTD, .pin=4},
{.gpio=GPIOD, .port=PORTD, .pin=5},
{.gpio=GPIOD, .port=PORTD, .pin=6},
{.gpio=GPIOD, .port=PORTD, .pin=7},
};
REF_Sensor.isEnabled = true;
REF_Sensor.savePower = true;
McuGPIO_GetDefaultConfig(&gpioConfig);
/* IR on/off: PTD1, high active */
gpioConfig.hw.gpio = GPIOD;
gpioConfig.hw.port = PORTD;
gpioConfig.hw.pin = 1;
gpioConfig.isInput = false;
gpioConfig.isHighOnInit = false;
REF_Sensor.irOn = McuGPIO_InitGPIO(&gpioConfig);
REF_LedOn = false;
/* IR sensors */
gpioConfig.isInput = true;
gpioConfig.isHighOnInit = false;
for(int i=0; i<REF_NOF_SENSORS; i++) {
gpioConfig.hw = hw[i];
REF_Sensor.sensor[i] = McuGPIO_InitGPIO(&gpioConfig);
}
#if REF_START_STOP_CALIB
vSemaphoreCreateBinary(REF_StartStopSem);
if (REF_StartStopSem==NULL) { /* semaphore creation failed */
for(;;){} /* error */
}
(void)xSemaphoreTake(REF_StartStopSem, 0); /* empty token */
vQueueAddToRegistry(REF_StartStopSem, "RefStartStopSem");
#if configUSE_TRACE_HOOKS
vTraceSetQueueName(REF_StartStopSem, "RefStartStopSem");
#endif
#endif
refState = REF_STATE_INIT;
#if PL_REMOTE_STOP_LINE
refLineKind = REF_LINE_FULL; /* need an initial state, assume on black so the robot does not move */
#else
refLineKind = REF_LINE_NONE;
#endif
refCenterLineVal = 0;
if (xTaskCreate(ReflTask, "Refl", 550/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+4, NULL) != pdPASS) {
for(;;){} /* error */
}
}
#endif /* PL_CONFIG_USE_LINE_SENSOR */

127
ADIS_Sumo_Styger/Sumo/Reflectance.h
Unescape View File

@ -0,0 +1,127 @@
/**
* \file
* \brief Reflectance sensor driver interface.
* \author Erich Styger, erich.styger@hslu.ch
* \license SPDX-License-Identifier: BSD-3-Clause
* This module implements a driver for the reflectance sensor array.
*/
#ifndef REFLECTANCE_H_
#define REFLECTANCE_H_
#include "platform.h"
#include <stdint.h>
#include <stdbool.h>
#if PL_CONFIG_USE_LINE_SENSOR
#if PL_IS_ZUMO_ROBOT
#define REF_SENSOR1_IS_LEFT 0 /* if sensor 1 is on the left side */
#define REF_MIN_LINE_VAL 0x60 /* minimum value indicating a line */
#define REF_MIN_NOISE_VAL 0x40 /* values below this are not added to the weighted sum */
#define REF_SENSOR_TIMEOUT_US 1200 /* after this time, consider no reflection (black) */
#elif PL_IS_ROUND_ROBOT
#define REF_SENSOR1_IS_LEFT 0 /* if sensor 1 is on the left side */
#define REF_MIN_LINE_VAL 0x20 /* minimum value indicating a line */
#define REF_MIN_NOISE_VAL 0x0F /* values below this are not added to the weighted sum */
#define REF_SENSOR_TIMEOUT_US 3000 /* after this time, consider no reflection (black) */
#elif PL_IS_TRACK_ROBOT
#define REF_SENSOR1_IS_LEFT 1 /* if sensor 1 is on the left side */
#define REF_MIN_LINE_VAL 0x20 /* minimum value indicating a line */
#define REF_MIN_NOISE_VAL 0x0F /* values below this are not added to the weighted sum */
#define REF_SENSOR_TIMEOUT_US 3000 /* after this time, consider no reflection (black) */
#elif PL_IS_INTRO_ZUMO_ROBOT
#define REF_SENSOR1_IS_LEFT 1 /* if sensor 1 is on the left side */
#define REF_MIN_LINE_VAL 0x20 /* minimum value indicating a line */
#define REF_MIN_NOISE_VAL 0x10 /* values below this are not added to the weighted sum */
#define REF_SENSOR_TIMEOUT_US 3000 /* after this time, consider no reflection (black) */
#elif PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22
#define REF_SENSOR1_IS_LEFT 1 /* if sensor 1 is on the left side */
#if PL_DO_MINT /* different color/reflection for MINT environment */
#define REF_MIN_LINE_VAL 0x120 /* minimum value indicating a line */
#define REF_MIN_NOISE_VAL 0x80 /* values below this are not added to the weighted sum */
#else
#define REF_MIN_LINE_VAL 0x100 /* minimum value indicating a line */
#define REF_MIN_NOISE_VAL 0x40 /* values below this are not added to the weighted sum */
#endif
#define REF_SENSOR_TIMEOUT_US 300 /* after this time, consider no reflection (black). Must be smaller than the timeout period of the RefCnt timer! */
#else
#error "unknown configuration!"
#endif
#if PL_CONFIG_USE_SHELL
#include "McuShell.h"
/*!
* \brief Shell parser routine.
* \param cmd Pointer to command line string.
* \param handled Pointer to status if command has been handled. Set to TRUE if command was understood.
* \param io Pointer to stdio handle
* \return Error code, ERR_OK if everything was ok.
*/
uint8_t REF_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io);
#endif
#if (PL_IS_ZUMO_ROBOT || PL_IS_INTRO_ZUMO_ROBOT || PL_IS_INTRO_ZUMO_ROBOT2 || PL_IS_INTRO_ZUMO_K22)
#define REF_NOF_SENSORS 6
#elif (PL_IS_ROUND_ROBOT || PL_IS_TRACK_ROBOT)
#define REF_NOF_SENSORS 8
#else
#error "unknown configuration!"
#endif
#define REF_MIDDLE_LINE_VALUE ((REF_NOF_SENSORS+1)*1000/2)
#define REF_MAX_LINE_VALUE ((REF_NOF_SENSORS+1)*1000) /* maximum value for REF_GetLine() */
typedef enum {
REF_LINE_NONE=0, /* no line, sensors do not see a line */
REF_LINE_STRAIGHT=1, /* forward line |, sensors see a line underneath */
REF_LINE_LEFT=2, /* left half of sensors see line */
REF_LINE_RIGHT=3, /* right half of sensors see line */
REF_LINE_FULL=4, /* all sensors see a line */
REF_LINE_AIR=5, /* all sensors have a timeout value. Robot is not on ground at all? */
REF_NOF_LINES /* Sentinel */
} REF_LineKind;
typedef enum {
REF_LINE_KIND_MODE_LINE_FOLLOW, /* returns REF_LINE_NONE, REF_LINE_STRAIGHT or REF_LINE_FULL */
REF_LINE_KIND_MODE_ALL, /* returns all different line kinds */
REF_LINE_KIND_MODE_MAZE, /* returns all different line kinds */
REF_LINE_KIND_MODE_SUMO, /* returns all different line kinds */
} REF_LineKindMode;
REF_LineKind REF_GetLineKind(REF_LineKindMode mode);
void REF_DumpCalibrated(void);
unsigned char *REF_LineKindStr(REF_LineKind line);
uint16_t REF_GetLineValue(bool *onLine);
uint16_t REF_LineWidth(void);
void REF_GetSensorValues(uint16_t *values, int nofValues);
/*!
* \brief Starts or stops the calibration.
*/
void REF_CalibrateStartStop(void);
/*!
* \brief Function to find out if we can use the sensor (means: it is calibrated and not currently calibrating)
* \return TRUE if the sensor is ready.
*/
bool REF_CanUseSensor(void);
/*!
* \brief Driver Deinitialization.
*/
void REF_Deinit(void);
/*!
* \brief Driver Initialization.
*/
void REF_Init(void);
#endif /* PL_CONFIG_USE_LINE_SENSOR */
#endif /* REFLECTANCE_H_ */

2
ADIS_Sumo_Styger/source/platform.h
Unescape View File

@ -51,7 +51,7 @@
/* robot specific features */ /* robot specific features */
#define PL_CONFIG_USE_LEDS (1) #define PL_CONFIG_USE_LEDS (1)
#define PL_CONFIG_USE_MOTORS (1) #define PL_CONFIG_USE_MOTORS (1)
#define PL_CONFIG_USE_LINE_SENSOR (0) /* if line sensor is used or not: NOTE: currently this affects the ESP32 gateway too much! */ #define PL_CONFIG_USE_LINE_SENSOR (1) /* if line sensor is used or not: NOTE: currently this affects the ESP32 gateway too much! */
#define PL_CONFIG_USE_BUZZER (1) #define PL_CONFIG_USE_BUZZER (1)
#define PL_CONFIG_USE_QUADRATURE (1 && PL_CONFIG_USE_MOTORS) #define PL_CONFIG_USE_QUADRATURE (1 && PL_CONFIG_USE_MOTORS)
#define PL_CONFIG_USE_TACHO (1 && PL_CONFIG_USE_QUADRATURE) #define PL_CONFIG_USE_TACHO (1 && PL_CONFIG_USE_QUADRATURE)

Write Preview
Loading…
Cancel
Save