static void CheckButton(void) {
uint32_t timeTicks; /* time in ticks */
#define BUTTON_CNT_MS 100 /* iteration count for button */
bool autoCalibrate = FALSE;
if (SW1_GetVal()==0) { /* button pressed */
/* short press (1 beep): start or stop line following if calibrated
* 1 s press (2 beep): calibrate manually
* 2 s press (3 beep): calibrate with auto-move
* 3 s press (4 beep or more): clear path
* */
FRTOS1_vTaskDelay(50/portTICK_RATE_MS); /* simple debounce */
if (SW1_GetVal()==0) { /* still pressed */
LEDG_On();
timeTicks = 0;
while(SW1_GetVal()==0 && timeTicks<=6000/BUTTON_CNT_MS) {
FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
if ((timeTicks%(1000/BUTTON_CNT_MS))==0) {
#if PL_HAS_BUZZER
BUZ_Beep(300, 200);
#endif
}
timeTicks++;
} /* wait until released */
autoCalibrate = FALSE;
if (timeTicks<1000/BUTTON_CNT_MS) { /* less than 1 second */
CLS1_SendStr((unsigned char*)"button press.\r\n", CLS1_GetStdio()->stdOut);
StateMachine(TRUE); /* <1 s, short button press, according to state machine */
} else if (timeTicks>=(1000/BUTTON_CNT_MS) && timeTicks<(2000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"calibrate.\r\n", CLS1_GetStdio()->stdOut);
APP_StateStartCalibrate(); /* 1-2 s: start calibration by hand */
} else if (timeTicks>=(2000/BUTTON_CNT_MS) && timeTicks<(3000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"auto calibrate.\r\n", CLS1_GetStdio()->stdOut);
APP_StateStartCalibrate(); /* 2-3 s: start auto calibration */
autoCalibrate = TRUE;
} else if (timeTicks>=(3000/BUTTON_CNT_MS)) {
CLS1_SendStr((unsigned char*)"delete solution.\r\n", CLS1_GetStdio()->stdOut);
MAZE_ClearSolution();
}
while (SW1_GetVal()==0) { /* wait until button is released */
FRTOS1_vTaskDelay(BUTTON_CNT_MS/portTICK_RATE_MS);
}
if (autoCalibrate) {
CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", CLS1_GetStdio()->stdOut);
/* perform automatic calibration */
WAIT1_WaitOSms(1500); /* wait some time */
TURN_Turn(TURN_LEFT90);
TURN_Turn(TURN_RIGHT90);
TURN_Turn(TURN_RIGHT90);
TURN_Turn(TURN_LEFT90);
TURN_Turn(TURN_STOP);
APP_StateStopCalibrate();
CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", CLS1_GetStdio()->stdOut);
}
}
} /* if */
}
static void AutoCalibrateReflectance(const CLS1_StdIOType *io) {
CLS1_SendStr((unsigned char*)"start auto-calibration...\r\n", io->stdOut);
/* perform automatic calibration */
APP_StateStartCalibrate(io);
TURN_Turn(TURN_LEFT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_RIGHT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_RIGHT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_LEFT90);
WAIT1_WaitOSms(500); /* wait some time */
TURN_Turn(TURN_STOP);
APP_StateStopCalibrate(io);
CLS1_SendStr((unsigned char*)"auto-calibration finished.\r\n", io->stdOut);
}
static void StateMachine(bool buttonPress) {
static uint8_t cnt;
switch (appState) {
case APP_STATE_INIT:
LEDG_Off();
LEDR_On();
if (buttonPress) {
APP_StateStartCalibrate();
}
break;
case APP_STATE_CALIBRATE:
cnt++;
if (cnt>50) {
cnt = 0;
LEDR_Neg();
}
if (buttonPress) {
APP_StateStopCalibrate();
}
break;
case APP_STATE_IDLE:
LEDR_Off();
LEDG_On();
if (buttonPress) {
LF_StartFollowing();
appState = APP_STATE_FOLLOW;
}
break;
case APP_STATE_FOLLOW:
LEDR_Off();
LEDG_Off();
if (!LF_IsFollowing()) {
appState = APP_STATE_IDLE;
}
if (buttonPress) {
LF_StopFollowing();
appState = APP_STATE_IDLE;
}
break;
} /* switch */
}
static void StateMachine(bool buttonPress) {
#if PL_APP_FOLLOW_OBSTACLE || PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
static uint8_t cnt;
#endif
switch (appState) {
case APP_STATE_INIT:
LEDG_Off();
LEDR_On();
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
if (buttonPress) {
APP_StateStartCalibrate();
}
#elif PL_APP_FOLLOW_OBSTACLE
appState = APP_STATE_IDLE;
#endif
break;
#if PL_APP_FOLLOW_OBSTACLE
case APP_STATE_FOLLOW_OBSTACLE:
cnt++;
if (cnt>50) {
cnt = 0;
LEDR_Neg();
}
if (buttonPress) {
followObstacle = FALSE;
appState = APP_STATE_IDLE;
}
break;
#endif
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
case APP_STATE_CALIBRATE:
cnt++;
if (cnt>50) {
cnt = 0;
LEDR_Neg();
}
if (buttonPress) {
APP_StateStopCalibrate();
}
break;
#endif
case APP_STATE_IDLE:
LEDR_Off();
LEDG_On();
if (buttonPress) {
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
LF_StartFollowing();
appState = APP_STATE_FOLLOW_LINE;
#elif PL_APP_FOLLOW_OBSTACLE
followObstacle = TRUE;
appState = APP_STATE_FOLLOW_OBSTACLE;
#endif
}
break;
#if PL_APP_LINE_FOLLOWING || PL_APP_LINE_MAZE
case APP_STATE_FOLLOW_LINE:
LEDR_Off();
LEDG_Off();
if (!LF_IsFollowing()) {
appState = APP_STATE_IDLE;
}
if (buttonPress) {
LF_StopFollowing();
appState = APP_STATE_IDLE;
}
break;
#endif
} /* switch */
}
static uint8_t PrintStatus(const CLS1_StdIOType *io) {
unsigned char buf[24];
int i;
CLS1_SendStatusStr((unsigned char*)"reflectance", (unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" IR led on", ledON?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrating", doMinMaxCalibration?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" calibrated", isCalibrated?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_NOISE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min noise", buf, io->stdOut);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), REF_MIN_LINE_VAL);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" min line", buf, io->stdOut);
CLS1_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
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorRaw[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_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
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorMin[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_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
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorMax[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_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
CLS1_SendStr((unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorCalibrated[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
CLS1_SendStatusStr((unsigned char*)" line val", (unsigned char*)"", io->stdOut);
buf[0] = '\0'; UTIL1_strcatNum16s(buf, sizeof(buf), refCenterLineVal);
CLS1_SendStr(buf, io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#if PL_APP_LINE_MAZE
#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
CLS1_SendStatusStr((unsigned char*)" history", (unsigned char*)"0x", io->stdOut);
} else {
CLS1_SendStr((unsigned char*)" 0x", io->stdOut);
}
buf[0] = '\0'; UTIL1_strcatNum16Hex(buf, sizeof(buf), SensorHistory[i]);
CLS1_SendStr(buf, io->stdOut);
}
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
#endif
CLS1_SendStatusStr((unsigned char*)" line kind", REF_LineKindStr(refLineKind), io->stdOut);
CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
return ERR_OK;
}
byte REF_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "ref help")==0) {
*handled = TRUE;
return PrintHelp(io);
} else if ((UTIL1_strcmp((char*)cmd, CLS1_CMD_STATUS)==0) || (UTIL1_strcmp((char*)cmd, "ref status")==0)) {
*handled = TRUE;
return PrintStatus(io);
} else if (UTIL1_strcmp((char*)cmd, "ref calib on")==0) {
APP_StateStartCalibrate();
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref calib off")==0) {
APP_StateStopCalibrate();
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref led on")==0) {
ledON = TRUE;
*handled = TRUE;
return ERR_OK;
} else if (UTIL1_strcmp((char*)cmd, "ref led off")==0) {
ledON = FALSE;
*handled = TRUE;
return ERR_OK;
}
return ERR_OK;
}
uint16_t REF_GetLineValue(bool *onLine) {
*onLine = refCenterLineVal>0 && refCenterLineVal<REF_MAX_LINE_VALUE;
return refCenterLineVal;
}
static portTASK_FUNCTION(ReflTask, pvParameters) {
(void)pvParameters; /* not used */
for(;;) {
if (doMinMaxCalibration) {
REF_CalibrateMinMax(SensorMin, SensorMax, SensorRaw);
#if PL_HAS_BUZZER
BUZ_Beep(300, 50);
#endif
} else {
REF_Measure();
}
FRTOS1_vTaskDelay(10/portTICK_RATE_MS);
}
}
void REF_Init(void) {
refLineKind = REF_LINE_NONE;
refCenterLineVal = 0;
mutexHandle = FRTOS1_xSemaphoreCreateMutex();
if (mutexHandle==NULL) {
for(;;);
}
timerHandle = RefCnt_Init(NULL);
REF_InitSensorValues();
if (FRTOS1_xTaskCreate(ReflTask, (signed portCHAR *)"Refl", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY+1+2, NULL) != pdPASS) {
for(;;){} /* error */
}
}