static uint8_t HandleDataRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_HAS_SHELL
uint8_t buf[32];
CLS1_ConstStdIOTypePtr io = CLS1_GetStdio();
#endif
uint8_t val;
(void)size;
(void)packet;
switch(type) {
case RAPP_MSG_TYPE_DATA: /* generic data message */
*handled = TRUE;
val = *data; /* get data value */
#if PL_HAS_DRIVE
DRV_EnableDisable(TRUE);
#endif
#if PL_HAS_SHELL
SHELL_SendString((unsigned char*)"Data: ");
SHELL_SendString(data);
SHELL_SendString((unsigned char*)" from addr 0x");
buf[0] = '\0';
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(buf, sizeof(buf), srcAddr);
#else
UTIL1_strcatNum16Hex(buf, sizeof(buf), srcAddr);
#endif
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
#endif /* PL_HAS_SHELL */
return ERR_OK;
break;
default:
break;
} /* switch */
return ERR_OK;
}
/*!
* \brief Performs a turn.
* \return Returns TRUE while turn is still in progress.
*/
uint8_t MAZE_EvaluteTurn(bool *finished) {
REF_LineKind historyLineKind, currLineKind;
TURN_Kind turn;
*finished = FALSE;
currLineKind = REF_GetLineKind();
if (currLineKind==REF_LINE_NONE) { /* nothing, must be dead end */
turn = TURN_LEFT180;
} else {
MAZE_ClearSensorHistory(); /* clear history values */
MAZE_SampleSensorHistory(); /* store current values */
TURN_Turn(TURN_STEP_LINE_FW_POST_LINE, MAZE_SampleTurnStopFunction); /* do the line and beyond in one step */
historyLineKind = MAZE_HistoryLineKind(); /* new read new values */
currLineKind = REF_GetLineKind();
turn = MAZE_SelectTurn(historyLineKind, currLineKind);
}
if (turn==TURN_FINISHED) {
*finished = TRUE;
LF_StopFollowing();
SHELL_SendString((unsigned char*)"MAZE: finished!\r\n");
return ERR_OK;
} else if (turn==TURN_STRAIGHT) {
/*! \todo Extend if necessary */
SHELL_SendString((unsigned char*)"going straight\r\n");
return ERR_OK;
} else if (turn==TURN_STOP) { /* should not happen here? */
LF_StopFollowing();
SHELL_SendString((unsigned char*)"Failure, stopped!!!\r\n");
return ERR_FAILED; /* error case */
} else { /* turn or do something */
/*! \todo Extend if necessary */
return ERR_OK; /* turn finished */
}
}
void REF_SaveCalibData(void){
FRTOS1_taskENTER_CRITICAL();
if(NVMC_SaveReflectanceData(&SensorCalibMinMax, sizeof(SensorCalibMinMax)) == ERR_OK){
SHELL_SendString("Calib ok");
}else{
SHELL_SendString("An error occurred");
}
FRTOS1_taskEXIT_CRITICAL();
}
static void REF_StateMachine(void) {
int i;
switch (refState) {
case REF_STATE_INIT:
SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");
refState = REF_STATE_NOT_CALIBRATED;
break;
// SW1 pressen, dann kommt er da rein.
case REF_STATE_NOT_CALIBRATED:
REF_MeasureRaw(SensorRaw);
/*! \todo Add a new event to your event module...*/
if (EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION)) {
EVNT_ClearEvent(EVNT_REF_START_STOP_CALIBRATION);
refState = REF_STATE_START_CALIBRATION;
break;
}
break;
case REF_STATE_START_CALIBRATION:
SHELL_SendString((unsigned char*)"start calibration...\r\n");
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorCalibMinMax.minVal[i] = MAX_SENSOR_VALUE;
SensorCalibMinMax.maxVal[i] = 0;
SensorCalibrated[i] = 0;
}
refState = REF_STATE_CALIBRATING;
break;
case REF_STATE_CALIBRATING:
REF_CalibrateMinMax(SensorCalibMinMax.minVal, SensorCalibMinMax.maxVal, SensorRaw);
if (EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION)) {
EVNT_ClearEvent(EVNT_REF_START_STOP_CALIBRATION);
refState = REF_STATE_STOP_CALIBRATION;
}
break;
case REF_STATE_STOP_CALIBRATION:
SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
refState = REF_STATE_READY;
break;
case REF_STATE_READY:
REF_Measure();
if (EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION)) {
EVNT_ClearEvent(EVNT_REF_START_STOP_CALIBRATION);
refState = REF_STATE_START_CALIBRATION;
}
break;
} /* switch */
}
RefStateType REF_GetCalibData(void){
FRTOS1_taskENTER_CRITICAL();
SensorCalibMinMax = *(SensorCalibT*)NVMC_GetReflectanceData();
if ((void*)&SensorCalibMinMax == NULL){
SHELL_SendString("No calibration data found");
FRTOS1_taskEXIT_CRITICAL();
return (RefStateType) REF_STATE_NOT_CALIBRATED;
}
else{
SHELL_SendString("Calibration data loaded");
FRTOS1_taskEXIT_CRITICAL();
return (RefStateType) REF_STATE_READY;
}
return (RefStateType) REF_STATE_INIT;
}
int Callback(const char *section, const char *key, const char *value, const void *userdata)
{
(void)userdata; /* this parameter is not used in this example */
SHELL_SendString((unsigned char*)" [");
SHELL_SendString((unsigned char*)section);
SHELL_SendString((unsigned char*)"]\t");
SHELL_SendString((unsigned char*)key);
SHELL_SendString((unsigned char*)"=");
SHELL_SendString((unsigned char*)value);
SHELL_SendString((unsigned char*)"\r\n");
return 1;
}
static void RemoteTask (void *pvParameters) {
(void)pvParameters;
#if PL_CONFIG_HAS_JOYSTICK
(void)APP_GetXY(&midPointX, &midPointY, NULL, NULL);
#endif
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
for(;;) {
if (REMOTE_isOn) {
#if PL_CONFIG_HAS_JOYSTICK
if (REMOTE_useJoystick) {
uint8_t buf[2];
int16_t x, y;
int8_t x8, y8;
/* send periodically messages */
APP_GetXY(&x, &y, &x8, &y8);
buf[0] = x8;
buf[1] = y8;
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), x8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), y8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_JOYSTICK_XY, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
FRTOS1_vTaskDelay(200/portTICK_PERIOD_MS);
} else {
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
}
} /* for */
}
static void REF_StateMachine(void) {
int i;
switch (refState) {
case REF_STATE_INIT:
#if PL_CONFIG_EST
{
SensorCalibT *ptr;
ptr = (SensorCalibT*)NVMC_GetReflectanceData();
if (ptr!=NULL) { /* valid data */
SensorCalibMinMax = *ptr;
refState = REF_STATE_READY;
} else {
refState = REF_STATE_NOT_CALIBRATED;
}
}
#else
SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");
refState = REF_STATE_NOT_CALIBRATED;
#endif
break;
case REF_STATE_NOT_CALIBRATED:
REF_MeasureRaw(SensorRaw);
/*! \todo Add a new event to your event module...*/
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
case REF_STATE_START_CALIBRATION:
SHELL_SendString((unsigned char*)"start calibration...\r\n");
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorCalibMinMax.minVal[i] = MAX_SENSOR_VALUE;
SensorCalibMinMax.maxVal[i] = 0;
SensorCalibrated[i] = 0;
}
refState = REF_STATE_CALIBRATING;
break;
case REF_STATE_CALIBRATING:
REF_CalibrateMinMax(SensorCalibMinMax.minVal, SensorCalibMinMax.maxVal, SensorRaw);
#if PL_CONFIG_HAS_BUZZER
startBuzzer(Middle,200);
#endif
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_STOP_CALIBRATION;
}
#endif
break;
case REF_STATE_STOP_CALIBRATION:
SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
#if PL_CONFIG_EST
if (NVMC_SaveReflectanceData(&SensorCalibMinMax, sizeof(SensorCalibMinMax))!=ERR_OK) {
SHELL_SendString((unsigned char*)"Flashing calibration data FAILED!\r\n");
} else {
SHELL_SendString((unsigned char*)"Stored calibration data.\r\n");
}
#endif
refState = REF_STATE_READY;
break;
case REF_STATE_READY:
REF_Measure();
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
} /* switch */
}
static portTASK_FUNCTION(RemoteTask, pvParameters) {
#if PL_HAS_WATCHDOG
int i;
#endif
(void)pvParameters;
#if PL_HAS_JOYSTICK
(void)APP_GetXY(&midPointX, &midPointY, NULL, NULL);
#endif
for(;;) {
if (REMOTE_isOn) {
#if PL_HAS_ACCEL
if (REMOTE_useAccelerometer) {
#if PL_HAS_KEYS
uint8_t buf[7];
uint8_t keys;
#else /* PL_HAS_KEYS */
uint8_t buf[6];
#endif /* PL_HAS_KEYS */
int16_t x, y, z;
/* send periodically accelerometer messages */
#if PL_HAS_KEYS
APP_GetKeys(&keys);
#endif /* PL_HAS_KEYS */
ACCEL_GetValues(&x, &y, &z);
buf[0] = (uint8_t)(x&0xFF);
buf[1] = (uint8_t)(x>>8);
buf[2] = (uint8_t)(y&0xFF);
buf[3] = (uint8_t)(y>>8);
buf[4] = (uint8_t)(z&0xFF);
buf[5] = (uint8_t)(z>>8);
#if PL_HAS_KEYS
buf[6] = keys;
#endif /* PL_HAS_KEYS */
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), x);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), y);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" z: ");
UTIL1_strcatNum16s(txtBuf, sizeof(txtBuf), z);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_ACCEL, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
#if PL_HAS_JOYSTICK
if (REMOTE_useJoystick) {
#if PL_HAS_KEYS
uint8_t buf[3];
uint8_t keys;
#else /* PL_HAS_KEYS */
uint8_t buf[2];
#endif /* PL_HAS_KEYS */
int16_t x, y;
int8_t x8, y8;
/* send periodically accelerometer messages */
#if PL_HAS_KEYS
APP_GetKeys(&keys);
#endif /* PL_HAS_KEYS */
APP_GetXY(&x, &y, &x8, &y8);
buf[0] = x8;
buf[1] = y8;
#if PL_HAS_KEYS
buf[2] = keys;
#endif /* PL_HAS_KEYS */
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), x8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), y8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_JOYSTICK_XY, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
#if PL_HAS_WATCHDOG
for(i=0; i<2; i++) { /* do it in smaller steps */
WDT_IncTaskCntr(WDT_TASK_ID_REMOTE, 100);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
}
#else
FRTOS1_vTaskDelay(200/portTICK_RATE_MS);
#endif
} else {
#if PL_HAS_WATCHDOG
for(i=0; i<10; i++) { /* do it in smaller steps */
WDT_IncTaskCntr(WDT_TASK_ID_REMOTE, 100);
FRTOS1_vTaskDelay(100/portTICK_RATE_MS);
}
#else
FRTOS1_vTaskDelay(1000/portTICK_RATE_MS);
#endif
}
} /* for */
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t direction, speed;
int16_t x1000, y1000;
*handled = TRUE;
direction = *data; /* get x data value */
speed = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
//x1000 = scaleJoystickTo1K(x);
//y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(direction, speed, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, disable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
REMOTE_SetOnOff(FALSE);
DRV_SetSpeed(0,0); /* turn off motors */
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
REMOTE_SetOnOff(TRUE);
DRV_SetMode(DRV_MODE_SPEED);
SHELL_SendString("Remote ON\r\n");
} else if (val=='C') { /* red 'C' button */
/*! \todo add functionality */
} else if (val=='A') { /* green 'A' button */
/*! \todo add functionality */
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
static void RemoteTask (void *pvParameters) {
(void)pvParameters;
#if PL_CONFIG_HAS_JOYSTICK
(void)APP_GetXY(&midPointX, &midPointY, NULL, NULL);
#endif
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
for(;;) {
if (REMOTE_isOn) {
#if PL_CONFIG_HAS_JOYSTICK
if (REMOTE_useJoystick) {
uint8_t buf[2];
int16_t x, y;
int8_t x8, y8;
/* send periodically messages */
APP_GetXY(&x, &y, &x8, &y8);
if(x8 > 100)
buf[0] = 1;
else if(x8 < -100)
buf[0] = 2;
else
buf[0] = 0;
//buf[0] = x8;
if(normalSpeedPressed) {
if(fastSpeedPressed) {
buf[1] = 2; // fast speed
} else {
buf[1] = 1; // normal speed
}
} else if (retourSpeedPressed) {
buf[1] = 3; // retour
} else {
buf[1] = 0; // stand
}
if(startLineFollow == 1) {
buf[1] = 4; // left handed
startLineFollow = 0;
}
if(startLineFollow == 2) {
buf[1] = 5; // right handed
startLineFollow = 0;
}
//buf[1] = y8;
if (REMOTE_isVerbose) {
uint8_t txtBuf[48];
UTIL1_strcpy(txtBuf, sizeof(txtBuf), (unsigned char*)"TX: x: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), x8);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" y: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), y8);
//UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" to addr 0x");
#if RNWK_SHORT_ADDR_SIZE==1
//UTIL1_strcatNum8Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#else
//UTIL1_strcatNum16Hex(txtBuf, sizeof(txtBuf), RNETA_GetDestAddr());
#endif
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" 0/R/L: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), buf[0]);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)" 0/N/F/R: ");
UTIL1_strcatNum8s(txtBuf, sizeof(txtBuf), buf[1]);
UTIL1_strcat(txtBuf, sizeof(txtBuf), (unsigned char*)"\r\n");
SHELL_SendString(txtBuf);
}
(void)RAPP_SendPayloadDataBlock(buf, sizeof(buf), RAPP_MSG_TYPE_JOYSTICK_XY, RNETA_GetDestAddr(), RPHY_PACKET_FLAGS_REQ_ACK);
LED1_Neg();
}
#endif
FRTOS1_vTaskDelay(20/portTICK_PERIOD_MS);
} else {
FRTOS1_vTaskDelay(1000/portTICK_PERIOD_MS);
}
} /* for */
}
static void REF_StateMachine(void) {
int i;
switch (refState) {
case REF_STATE_INIT:
#if NVM_ENABLED
refState = REF_GetCalibData();
#else
refState = REF_STATE_NOT_CALIBRATED;
SHELL_SendString("INFO: Sensor not calibrated");
#endif
break;
case REF_STATE_NOT_CALIBRATED:
REF_MeasureRaw(SensorRaw);
/*! \todo Add a new event to your event module...*/
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
case REF_STATE_START_CALIBRATION:
SHELL_SendString((unsigned char*)"start calibration...\r\n");
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorCalibMinMax.minVal[i] = MAX_SENSOR_VALUE;
SensorCalibMinMax.maxVal[i] = 0;
SensorCalibrated[i] = 0;
}
refState = REF_STATE_CALIBRATING;
break;
case REF_STATE_CALIBRATING:
REF_CalibrateMinMax(SensorCalibMinMax.minVal, SensorCalibMinMax.maxVal, SensorRaw);
#if PL_CONFIG_HAS_BUZZER
(void)BUZ_Beep(300, 20);
#endif
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_STOP_CALIBRATION;
}
#endif
break;
case REF_STATE_STOP_CALIBRATION:
SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
#if NVM_ENABLED
SHELL_SendString((unsigned char*)"saving calib data...\r\n");
REF_SaveCalibData();
#endif
refState = REF_STATE_READY;
break;
case REF_STATE_READY:
REF_Measure();
#if REF_START_STOP_CALIB
if (FRTOS1_xSemaphoreTake(REF_StartStopSem, 0)==pdTRUE) {
refState = REF_STATE_START_CALIBRATION;
}
#endif
break;
} /* switch */
}
int TestMiniIni(void) {
char str[100];
long n;
int s, k;
char section[50];
/* string reading */
n = ini_gets("first", "string", "dummy", str, sizearray(str), inifile);
Check(n==4 && UTIL1_strcmp(str,"noot")==0);
n = ini_gets("second", "string", "dummy", str, sizearray(str), inifile);
Check(n==4 && UTIL1_strcmp(str,"mies")==0);
n = ini_gets("first", "undefined", "dummy", str, sizearray(str), inifile);
Check(n==5 && UTIL1_strcmp(str,"dummy")==0);
/* ----- */
n = ini_gets("", "string", "dummy", str, sizearray(str), inifile2);
Check(n==4 && UTIL1_strcmp(str,"noot")==0);
n = ini_gets(NULL, "string", "dummy", str, sizearray(str), inifile2);
Check(n==4 && UTIL1_strcmp(str,"noot")==0);
/* ----- */
SHELL_SendString((unsigned char*)"1. String reading tests passed\n");
/* value reading */
n = ini_getl("first", "val", -1, inifile);
Check(n==1);
n = ini_getl("second", "val", -1, inifile);
Check(n==2);
n = ini_getl("first", "undefined", -1, inifile);
Check(n==-1);
/* ----- */
n = ini_getl(NULL, "val", -1, inifile2);
Check(n==1);
/* ----- */
SHELL_SendString((unsigned char*)"2. Value reading tests passed\n");
/* string writing */
n = ini_puts("first", "alt", "flagged as \"correct\"", inifile);
Check(n==1);
n = ini_gets("first", "alt", "dummy", str, sizearray(str), inifile);
Check(n==20 && UTIL1_strcmp(str,"flagged as \"correct\"")==0);
/* ----- */
n = ini_puts("second", "alt", "correct", inifile);
Check(n==1);
n = ini_gets("second", "alt", "dummy", str, sizearray(str), inifile);
Check(n==7 && UTIL1_strcmp(str,"correct")==0);
/* ----- */
n = ini_puts("third", "test", "correct", inifile);
Check(n==1);
n = ini_gets("third", "test", "dummy", str, sizearray(str), inifile);
Check(n==7 && UTIL1_strcmp(str,"correct")==0);
/* ----- */
n = ini_puts("second", "alt", "overwrite", inifile);
Check(n==1);
n = ini_gets("second", "alt", "dummy", str, sizearray(str), inifile);
Check(n==9 && UTIL1_strcmp(str,"overwrite")==0);
/* ----- */
n = ini_puts(NULL, "alt", "correct", inifile2);
Check(n==1);
n = ini_gets(NULL, "alt", "dummy", str, sizearray(str), inifile2);
Check(n==7 && UTIL1_strcmp(str,"correct")==0);
/* ----- */
SHELL_SendString((unsigned char*)"3. String writing tests passed\n");
/* section/key enumeration */
SHELL_SendString((unsigned char*)"4. Section/key enumeration, file contents follows\n");
for (s = 0; ini_getsection(s, section, sizearray(section), inifile) > 0; s++) {
SHELL_SendString((unsigned char*)" [");
SHELL_SendString((unsigned char*)section);
SHELL_SendString((unsigned char*)"]\r\n");
for (k = 0; ini_getkey(section, k, str, sizearray(str), inifile) > 0; k++) {
SHELL_SendString((unsigned char*)"\t");
SHELL_SendString((unsigned char*)str);
SHELL_SendString((unsigned char*)"\r\n");
} /* for */
} /* for */
/* browsing through the file */
SHELL_SendString((unsigned char*)"5. browse through all settings, file contents follows\n");
ini_browse(Callback, NULL, inifile);
/* string deletion */
n = ini_puts("first", "alt", NULL, inifile);
Check(n==1);
n = ini_puts("second", "alt", NULL, inifile);
Check(n==1);
n = ini_puts("third", NULL, NULL, inifile);
Check(n==1);
/* ----- */
n = ini_puts(NULL, "alt", NULL, inifile2);
Check(n==1);
SHELL_SendString((unsigned char*)"6. String deletion tests passed\n");
return 0;
}
void Check(bool ok) {
if (!ok) {
SHELL_SendString((unsigned char*)"MinINI ERR: Failed!\r\n");
}
}
static void APP_EventHandler(EVNT_Handle event) {
switch(event) {
case EVNT_INIT:
/*LED1_On();
WAIT1_Waitms(50);
LED1_Off();
LED2_On();
WAIT1_Waitms(50);
LED2_Off();
LED3_On();
WAIT1_Waitms(50);
LED3_Off();*/
break;
case EVENT_LED_HEARTBEAT:
//LED1_Neg();
break;
#if PL_NOF_KEYS >= 1
case EVNT_SW1_PRESSED:
lastKeyPressed = 1;
#if PL_HAS_SHELL
SHELL_SendString("SW1 pressed!\r\n");
#endif
#if PL_HAS_BUZZER
BUZ_Beep(300, 500);
#endif
break;
case EVNT_SW1_LPRESSED:
#if PL_HAS_SHELL
SHELL_SendString("SW1 long pressed!\r\n");
#endif
break;
case EVNT_SW1_RELEASED:
#if PL_HAS_SHELL
SHELL_SendString("SW1 released!\r\n");
#endif
break;
#endif
#if PL_NOF_LEDS_ROBO
case EVNT_SW2_PRESSED:
lastKeyPressed = 2;
#if PL_HAS_SHELL
SHELL_SendString("SW2 pressed!\r\n");
EVNT_SetEvent(EVNT_REF_START_STOP_CALIBRATION);
#endif
break;
case EVNT_SW2_LPRESSED:
LED_U1_Neg();
#if PL_HAS_SHELL
SHELL_SendString("SW2 long pressed!\r\n");
#endif
break;
case EVNT_SW2_RELEASED:
LED_U1_Neg();
#if PL_HAS_SHELL
SHELL_SendString("SW2 released!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 3
case EVNT_SW3_PRESSED:
lastKeyPressed = 3;
#if PL_HAS_SHELL
SHELL_SendString("SW3 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 4
case EVNT_SW4_PRESSED:
lastKeyPressed = 4;
#if PL_HAS_SHELL
SHELL_SendString("SW4 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 5
case EVNT_SW5_PRESSED:
lastKeyPressed = 5;
#if PL_HAS_SHELL
SHELL_SendString("SW5 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 6
case EVNT_SW6_PRESSED:
lastKeyPressed = 6;
#if PL_HAS_SHELL
SHELL_SendString("SW6 pressed!\r\n");
#endif
break;
#endif
#if PL_NOF_KEYS >= 7
case EVNT_SW7_PRESSED:
lastKeyPressed = 7;
#if PL_HAS_SHELL
SHELL_SendString("SW7 pressed!\r\n");
#endif
break;
#endif
default:
break;
}
}
static void REF_StateMachine(void) {
int i;
switch (refState) {
case REF_STATE_INIT:
#if PL_HAS_CONFIG_NVM
/* If Calibration Data are saved, read it */
for (i = 0; i<REF_NOF_SENSORS;i++){
SensorCalibMinMax.minVal[i] = *((uint16_t*)NVMC_GetReflectanceData()+i);
SensorCalibMinMax.minVal[i+REF_NOF_SENSORS] = *((uint16_t*)NVMC_GetReflectanceData()+(i+REF_NOF_SENSORS));
}
SHELL_SendString((unsigned char*)"INFO: Loaded calibration data from FLASH...Gordon.\r\n");
refState = REF_STATE_READY;
#else
SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");
refState = REF_STATE_NOT_CALIBRATED;
break;
#endif
case REF_STATE_NOT_CALIBRATED:
REF_MeasureRaw(SensorRaw);
if (EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION)) {
EVNT_ClearEvent(EVNT_REF_START_STOP_CALIBRATION);
refState = REF_STATE_START_CALIBRATION;
break;
}
break;
case REF_STATE_START_CALIBRATION:
SHELL_SendString((unsigned char*)"start calibration...\r\n");
for(i=0;i<REF_NOF_SENSORS;i++) {
SensorCalibMinMax.minVal[i] = MAX_SENSOR_VALUE;
SensorCalibMinMax.maxVal[i] = 0;
SensorCalibrated[i] = 0;
}
refState = REF_STATE_CALIBRATING;
break;
case REF_STATE_CALIBRATING:
REF_CalibrateMinMax(SensorCalibMinMax.minVal, SensorCalibMinMax.maxVal, SensorRaw);
if (EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION)) {
EVNT_ClearEvent(EVNT_REF_START_STOP_CALIBRATION);
refState = REF_STATE_STOP_CALIBRATION;
}
break;
case REF_STATE_STOP_CALIBRATION:
/* Save Calibration Data */
NVMC_SaveReflectanceData(&SensorCalibMinMax, sizeof(SensorCalibMinMax));
SHELL_SendString((unsigned char*)"...stopping calibration. Files written to Flash\r\n");
refState = REF_STATE_READY;
break;
case REF_STATE_READY:
REF_Measure();
if (EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION)) {
EVNT_ClearEvent(EVNT_REF_START_STOP_CALIBRATION);
refState = REF_STATE_START_CALIBRATION;
}
break;
} /* switch */
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
KEY_DATA val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-5 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
switch(val) {
case A_p:
SHELL_ParseCmd((unsigned char*)"buzzer play tune");
break;
case A_lp:
nitro = TRUE;
break;
case A_r:
nitro = FALSE;
break;
case B_p:
SHELL_ParseCmd((unsigned char*)"line start");
break;
case B_lp:
break;
case B_r:
break;
case C_p:
break;
case C_lp:
break;
case C_r:
break;
case D_p:
SHELL_ParseCmd((unsigned char*)"line stop");
SHELL_ParseCmd((unsigned char*)"drive mode speed");
break;
case D_lp:
break;
case D_r:
break;
case E_p:
SHELL_ParseCmd((unsigned char*)"line righthand");
break;
case E_lp:
break;
case E_r:
break;
case F_p:
SHELL_ParseCmd((unsigned char*)"line lefthand");
break;
case F_lp:
break;
case F_r:
break;
case G_p:
break;
case G_lp:
break;
case G_r:
break;
default:
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
break;
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-15 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button - disable remote, drive mode none */
DRV_SetMode(DRV_MODE_NONE);
REMOTE_SetOnOff(FALSE);
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
REMOTE_SetOnOff(TRUE);
#if PL_CONFIG_HAS_DRIVE
DRV_SetMode(DRV_MODE_SPEED);
#endif
SHELL_SendString("Remote ON\r\n");
}
#if PL_CONFIG_HAS_LINE_MAZE
else if (val=='A') { /* green 'A' button */
SHELL_SendString("Button A pressed\r\n");
// Start Maze solving
if(!LF_IsFollowing()){
LF_StartFollowing();
}
} else if (val=='K') { /* green A button longpress -> 'K' */
SHELL_SendString("Clear Maze\r\n");
// Clear old maze solution, ready for restart
MAZE_ClearSolution();
} else if (val =='B'){ /* yellow 'B' button */
SHELL_SendString("Right hand rule!\r\n");
LF_SetRule(FALSE);
} else if (val=='E') { /* button 'E' pressed */
SHELL_SendString("Stop Following! \r\n");
if(LF_IsFollowing()){
LF_StopFollowing();
}
} else if (val=='D') { /* blue 'D' button */
SHELL_SendString("Left hand rule!\r\n");
LF_SetRule(TRUE);
}
#endif
else if (val=='C') { /* red 'C' button */
NITRO = TRUE;
SHELL_SendString("Nitrooooooo!!!\r\n");
BUZ_Beep(1000,1000);
} else if (val=='J') { /* button 'C' released */
NITRO = FALSE;
SHELL_SendString("Stop Nitro \r\n");
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t x, y;
int16_t x1000, y1000;
*handled = TRUE;
x = *data; /* get x data value */
y = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"x/y: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)x);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)y);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
/* filter noise around zero */
if (x>-5 && x<5) {
x = 0;
}
if (y>-5 && y<5) {
y = 0;
}
x1000 = scaleJoystickTo1K(x);
y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(y1000, x1000, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, toggle remote*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
DRV_SetMode(DRV_MODE_SPEED);
} else if (val=='G') { /* center joystick button: horn*/
SHELL_ParseCmd((unsigned char*)"buzzer buz 2000 300");
} else if (val=='A')
{
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(STRAIGHT_HAND);
LF_StartFollowing();
} else if (val=='C') { /* 'C' button: motor stop*/
DRV_SetMode(DRV_MODE_STOP);
} else if (val=='B') { /* 'B' button: start right-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(RIGHT_HAND);
LF_StartFollowing();
} else if (val=='D') { /* 'D' button: start left-hand algorithm */
MAZE_ClearSolution();
MAZE_SetSolveAlgorithm(LEFT_HAND);
LF_StartFollowing();
} else if (val=='E') {
REF_CalibrateStartStop();
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
static portTASK_FUNCTION(TraceTask, pvParameters) {
unsigned char buf[32];
(void)pvParameters;
//ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */
for(;;) {
if (traceChannel==TRACE_TO_NONE) {
/* do nothing */
} else if (traceChannel==TRACE_TO_SHELL) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => ");
#if PL_HAS_MOTOR
if (traceMotor) {
buf[0] = '\0';
if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x");
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x");
}
UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT)));
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_MOTOR_TACHO
if (traceTacho) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:");
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ' ');
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_QUADRATURE
if (traceQuad) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_ACCEL
if (traceAccel) {
int16_t x, y, z;
buf[0] = '\0';
ACCEL_GetValues(&x, &y, &z);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n");
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */
}
FRTOS1_vTaskDelay(20/portTICK_RATE_MS);
} /* for */
}
