void COR_Process(){
switch(containerRecognizer.state){
case COR_FSM_OBSERVANT:
// US_Measure(); // in our recent version, we do not need an ultrasonic sensor anymore
// debugging
containerRecognizer.state = COR_FSM_SURFACESCAN;
break;
case COR_FSM_SURFACESCAN:
if (SCN_IsAContainer()){
motionController.steps_left_until_stop = stepsAfterIR; // n of Steps we have to go after we recognized a container
containerRecognizer.state = COR_FSM_RECOGNIZECOLOR;
} else{
containerRecognizer.state = COR_FSM_OBSERVANT;
motionController.target_common_period = SER_GetPeriod(motionController.master_command_speed);
}
break;
case COR_FSM_RECOGNIZECOLOR:
if(motionController.state == MOT_FSM_STOP){ // wait until we stand still (at the right place)
COL_TurnOnLED();
RTOS_Wait(5);
uint8_t i = 0;
for (i; i < 5; i++){ // Measure several times the color values to get a better value
COL_ReadColors();
RTOS_Wait(1);
}
if(COL_RightContainer()){
containerRecognizer.state = COR_FSM_PICKUP;
}
else{
// containerRecognizer.state = COR_FSM_OBSERVANT;
motionController.target_common_period = SER_GetPeriod(motionController.master_command_speed);
}
RTOS_Wait(5);
COL_TurnOffLED();
}
break;
case COR_FSM_PICKUP:
SRV_pickUp();
// debug
//containerRecognizer.active = 0;
motionController.target_common_period = SER_GetPeriod(motionController.master_command_speed);
containerRecognizer.state = COR_FSM_OBSERVANT;
break;
}
}
/*
* This Task runs with a frequency of 2 Hz and measures the battery voltage. If the voltage is too low, we decide to shut down.
* */
void vWatchDogTask(){
for(;;){
ADC_IR_BATT_MeasureChan(TRUE, 1);
ADC_IR_BATT_GetChanValue16(1, &voltage16_t);
if (voltage16_t <= LowBatteryLight){
if (voltage16_t <= LowBatteryBlink){
Battery_low_NegVal(); // if the battery voltage is in a critical range, the LOW_BAT LED blinks with the frequency in witch this function runns
if (voltage16_t <= ShutOffVoltage){
WDG_ShutOff();
}
else{
Shut_OFF_ClrVal();
}
}
else{
Battery_low_ClrVal();
Battery_low_SetVal();
Shut_OFF_ClrVal();
}
}
else{
Battery_low_ClrVal();
Shut_OFF_ClrVal();
}
RTOS_Wait(500); // wait 500 ms between measurements
}
}
void vRemoteTask(void* pvParameters) {
(void)pvParameters; /* not used */
(void)RADIO_PowerUp();
for(;;) {
(void)RADIO_Process();
RTOS_Wait(5);
}
}
void vContainerRecognizerTask(/*void* pvParameters*/){
// cannot reach the color-sensor
// We only can initialize the color-sensor, if it is connected. Otherwise, we get errors and cannot debug.
COL_Init(); // Has to be implemented here, because we needd interrupts for the i2c. And these interrupts are disabled because of the Setup from RTOS. They get enabled with the RTOS_startShedule!
for(;;){
RTOS_Wait(5);
if(containerRecognizer.active){
COR_Process();
}
else{
// FRTOS1_taskYIELD();
}
}
}
