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 */
}
void REF_Danger(void){
int i = 0;
uint8_t sensor = 0;
bool temp;
if(!EVNT_EventIsSet(EVNT_DONT_FALL_DOWN))
for( i=0;i<REF_NOF_SENSORS;i++) {
if(SensorCalibrated[i] < 1000-THRESHOLD_BLK && SensorCalibrated[i]!=0 ){
sensor = i;
if(fightOn){
fight_state = WHITE_LINE_DETECTION;
FRTOS1_vTaskSuspend(checkRefl);
}
if(remoteOn){
EVNT_SetEvent(EVNT_REMOTE_DEACTIVATE);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -30);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -30);
vTaskDelay(500/TRG_TICKS_MS);
EVNT_SetEvent(EVNT_REMOTE_ACTIVATE);
}
else {
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT) , 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
EVNT_SetEvent(EVNT_DONT_FALL_DOWN);
FRTOS1_vTaskSuspend(checkRefl);
}
}
}
}
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 */
}
/*! \brief Handles the Events */
static void APP_EventHandler(EVNT_Handle event) {
uint8_t err;
switch(event) {
case EVNT_INIT:
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
LED2_On();
vTaskDelay(50/portTICK_RATE_MS);
LED2_Off();
LED3_On();
vTaskDelay(50/portTICK_RATE_MS);
LED3_Off();
break;
case EVENT_LED_HEARTBEAT:
LED2_Neg();
break;
case EVNT_SW1_PRESSED:
lastKeyPressed = 1;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
//EVNT_SetEvent(EVNT_REF_START_STOP_CALIBRATION);
#if PL_HAS_FIGHT_MODE
FIGHT_SetState(FIGHT_STATE_COUNTDOWN);
break;
#endif
#if PL_HAS_LINE_SENSOR
case EVNT_REF_START_STOP_CALIBRATION: //Create event again, because the event is handled in the "Reflectance.c" directly.
if(!EVNT_EventIsSet(EVNT_REF_START_STOP_CALIBRATION))
{
EVNT_SetEvent(EVNT_REF_START_STOP_CALIBRATION);
}
break;
#endif
#if PL_IS_FRDM
case EVNT_SW2_PRESSED:
lastKeyPressed = 2;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
break;
case EVNT_SW3_PRESSED:
lastKeyPressed = 3;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
break;
case EVNT_SW4_PRESSED:
lastKeyPressed = 4;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
break;
case EVNT_SW5_PRESSED:
lastKeyPressed = 5;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
break;
case EVNT_SW6_PRESSED:
lastKeyPressed = 6;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
break;
case EVNT_SW7_PRESSED:
lastKeyPressed = 7;
LED1_On();
vTaskDelay(50/portTICK_RATE_MS);
LED1_Off();
break;
#endif
default:
break;
}
}
