/*FUNCTION**********************************************************************
*
* Function Name : OSA_EventWait
* Description : This function checks the event's status, if it meets the wait
* condition, return kStatus_OSA_Success, otherwise, timeout will be used for
* wait. The parameter timeout indicates how long should wait in milliseconds.
* Pass OSA_WAIT_FOREVER to wait indefinitely, pass 0 will return the value
* kStatus_OSA_Timeout immediately if wait condition is not met. The event flags
* will be cleared if the event is auto clear mode. Flags that wakeup waiting
* task could be obtained from the parameter setFlags.
* This function returns kStatus_OSA_Success if wait condition is met, returns
* kStatus_OSA_Timeout if wait condition is not met within the specified
* 'timeout', returns kStatus_OSA_Error if any errors occur during waiting.
*
*END**************************************************************************/
osa_status_t OSA_EventWait(event_t *pEvent,
event_flags_t flagsToWait,
bool waitAll,
uint32_t timeout,
event_flags_t *setFlags)
{
INT8U err, opt;
/* Prepare wait options base on wait type and clear type. */
if (waitAll)
{
opt = OS_FLAG_WAIT_SET_ALL;
}
else
{
opt = OS_FLAG_WAIT_SET_ANY;
}
if (kEventAutoClear == pEvent->clearMode)
{
opt |= OS_FLAG_CONSUME;
}
/* If timeout is 0, try to wait. */
if (0U == timeout)
{
*setFlags = OSFlagAccept (pEvent->pGroup, flagsToWait, opt, &err);
}
else
{
/* If timeout is not 0, convert it to tickes. */
timeout = wait_timeout_msec_to_tick(timeout);
*setFlags = OSFlagPend(pEvent->pGroup, flagsToWait, opt, timeout, &err);
}
if (OS_ERR_NONE == err)
{
return kStatus_OSA_Success;
}
else if ((OS_ERR_TIMEOUT == err) ||
(OS_ERR_FLAG_NOT_RDY == err))
{
return kStatus_OSA_Timeout;
}
else
{
return kStatus_OSA_Error;
}
}
// ------------------------------------------------------------------------------------------------
INT8U StrategyColorA_GetNextAction(StructCmd *NextAction)
{
static int NextActionID = 1;
int CurrentActionID = 0;
INT8U Err = 0;
OS_FLAGS CurrentFlag = 0;
OS_FLAGS StrategyFlagsToSet = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
p->CmdType = CmdType_Blocking;
// Set CurrentID to NextID
CurrentActionID = NextActionID;
// Read Next Action
switch(CurrentActionID)
{
// StructuredFileLoopBegin
// LoopID = 0
case 1: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 2; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 600; break;
case 2: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 3; p->Cmd = Sensors_ArmsDeployment; break;
case 3: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 100; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1904; p->Param3 = 1660; p->Param4 = -90; break;
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 1
case 100: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 101; p->Cmd = Sensors_ArmsOpenOneCD; break;
case 101: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 102; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 130; break;
case 102: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 103; p->Cmd = Sensors_ArmsOpenTotem; break;
case 103: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 104; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 150; break;
case 104: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 105; p->Cmd = Sensors_ArmsCloseTotem; break;
case 105: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 106; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -140; break;
case 106: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 107; p->Cmd = Sensors_ArmsOpenUp; break;
case 107: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 108; p->Cmd = Sensors_ArmsOpenDown; break;
case 108: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 109; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_SPEED; p->Param2 = RIGHT_WHEEL; p->Param4 = -35; break;
case 109: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 111; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 550; break;
case 110: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 111; p->Cmd = Sensors_ArmsClose; break;
case 111: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 112; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_SPEED; p->Param2 = LEFT_WHEEL; p->Param4 = -10; break;
case 112: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 113; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 210; break;
case 113: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 114; p->Cmd = Sensors_ArmsUngrab; break;
case 114: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = 115; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -330; break;
case 115: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; NextActionID = -1; p->Cmd = Sensors_ArmsOpenDown; break;
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 2
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 3
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 4
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 5
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 6
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 7
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 8
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 9
// StructuredFileLoopEnd
// StructuredFileLoopBegin
// LoopID = 10
// StructuredFileLoopEnd
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Check for Wait command --------------------------------------------
if(App_Wait == p->Cmd)
{
// Execute the wait command
OSTimeDlyHMSM(p->Param1, p->Param2, p->Param3, p->Param4);
return StrategyColorA_GetNextAction(p);
}
// Check for conditionnal system command -----------------------------
if(App_IfGoto_System == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if((CurrentFlag & (p->Param1)) != 0)
NextActionID = (int)(p->Param2);
else
NextActionID = (int)(p->Param3);
return StrategyColorA_GetNextAction(p);
}
// Check for conditionnal strategy command ---------------------------
if(App_IfGoto_System == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if((CurrentFlag & (p->Param1)) != 0)
NextActionID = (int)(p->Param2);
else
NextActionID = (int)(p->Param3);
return StrategyColorA_GetNextAction(p);
}
// Set / Clear Strategy Flags --------------------------------------
if(App_SetStrategyFlags == p->Cmd)
{
StrategyFlagsToSet = p->Param1;
if(p->Param2 != 0)
OSFlagPost(AppStrategyFlags, StrategyFlagsToSet, OS_FLAG_SET, &Err);
else
OSFlagPost(AppStrategyFlags, StrategyFlagsToSet, OS_FLAG_CLR, &Err);
}
// Create the MvtSimple Command --------------------------------------
if(MvtSimple_MoveInMM == p->Cmd)
LibMoving_MoveInMM(p->Param2, p->Param1, p);
if(MvtSimple_RotateInDeg == p->Cmd)
LibMoving_RotateInDeg(p->Param4, p->Param1, p);
if(MvtSimple_RotateToAngleInDeg == p->Cmd)
LibMoving_RotateToAngleInDeg(p->Param4, p->Param1, p);
// Angle Conversion --------------------------------------------------
if((Mvt_UsePivotMode == p->Cmd) || (Mvt_UseMixedMode == p->Cmd) || (Mvt_UseSpline == p->Cmd))
{
p->Param4 = AppConvertDegInRad(p->Param4);
}
return ERR__NO_ERROR;
}
// ------------------------------------------------------------------------------------------------
INT8U StrategyColorB_GetNextAction(StructCmd *NextAction)
{
static int NextActionID = 0;
static int TimeoutID = -1;
static int SubStrategyReturnID = -1;
int CurrentActionID = 0;
INT8U Err = 0;
OS_FLAGS CurrentFlag = 0;
OS_FLAGS StrategyFlagsToSet = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
p->CmdType = CmdType_Blocking;
// Set CurrentID to NextID
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_WAIT_SET_ANY, &Err);
if(((CurrentFlag & APP_PARAM_APPFLAG_ACTION_TIMEOUT) == APP_PARAM_APPFLAG_ACTION_TIMEOUT) && (TimeoutID != -1))
{
CurrentActionID = TimeoutID;
// Clear Timeout Flag
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_CLR, &Err);
}
else
CurrentActionID = NextActionID;
// Check if NextID must be change due to previous order
if(CurrentActionID == 0)
{
// We have to check previous order
if(SubStrategyReturnID > 0)
{
CurrentActionID = SubStrategyReturnID; // Change NextID to return to the correct ID after SubStrategy loop
SubStrategyReturnID = -1;
}
else
{
CurrentActionID = 1; // There is no pending action, we go to the first state
}
}
// Read Next Action
switch(CurrentActionID)
{
// SUB_STRATEGY_BEGIN_LOOP
case 1:
p->CmdType = CmdType_Blocking;
p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE);
NextActionID = 2100;
TimeoutID = 2300;
p->Cmd = Mvt_UseDistOnly;
p->Param1 = DEFAULT_SPEED;
p->Param2 = 0;
p->Param3 = 0;
p->Param4 = 180;
break; // SubStrategyName = Glass2
// SUB_STRATEGY_END_LOOP
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Check for Wait command --------------------------------------------
if(App_Wait == p->Cmd)
{
// Nothing to do. All is done in TaskMain
}
// Check for conditionnal command -----------------------------
if((App_IfGoto_System == p->Cmd) || (App_IfGoto_Strategy == p->Cmd))
{
// Read the current Flags
if(App_IfGoto_System == p->Cmd)
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
else
CurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if(NextActionID >= 0)
SubStrategyReturnID = NextActionID;
// If condition is true, we go to the ID given by Param2, otherwise we use Param3
if(((p->Param1 == -1) || (CurrentFlag & (p->Param1)) != 0))
{
NextActionID = (int)(p->Param2);
}
else
{
NextActionID = (int)(p->Param3);
}
}
// Set / Clear Strategy Flags --------------------------------------
if(App_SetStrategyFlags == p->Cmd)
{
// Nothing to do. All is done in TaskMain
}
// Create the MvtSimple Command --------------------------------------
if(MvtSimple_MoveInMM == p->Cmd)
LibMoving_MoveInMM(p->Param2, p->Param1, p);
if(MvtSimple_RotateInDeg == p->Cmd)
LibMoving_RotateInDeg(p->Param4, p->Param1, p);
if(MvtSimple_RotateToAngleInDeg == p->Cmd)
LibMoving_RotateToAngleInDeg(p->Param4, p->Param1, p);
// Angle Conversion --------------------------------------------------
if((Mvt_UsePivotMode == p->Cmd) || (Mvt_UseMixedMode == p->Cmd) || (Mvt_UseSpline == p->Cmd))
{
p->Param4 = AppConvertDegInRad(p->Param4);
}
return ERR__NO_ERROR;
}
// ------------------------------------------------------------------------------------------------
INT8U StrategyColorB_GetNextAction(StructCmd *NextAction)
{
static int NextActionID = 0;
static int TimeoutID = -1;
static int SubStrategyReturnID = -1;
int CurrentActionID = 0;
INT8U Err = 0;
OS_FLAGS CurrentFlag = 0;
OS_FLAGS StrategyFlagsToSet = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
p->CmdType = CmdType_Blocking;
// Set CurrentID to NextID
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_WAIT_SET_ANY, &Err);
if(((CurrentFlag & APP_PARAM_APPFLAG_ACTION_TIMEOUT) == APP_PARAM_APPFLAG_ACTION_TIMEOUT) && (TimeoutID != -1))
{
CurrentActionID = TimeoutID;
// Clear Timeout Flag
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_CLR, &Err);
}
else
CurrentActionID = NextActionID;
// Check if NextID must be change due to previous order
if(CurrentActionID == 0)
{
// We have to check previous order
if(SubStrategyReturnID > 0)
{
CurrentActionID = SubStrategyReturnID; // Change NextID to return to the correct ID after SubStrategy loop
SubStrategyReturnID = -1;
}
else
{
CurrentActionID = 1; // There is no pending action, we go to the first state
}
}
// Read Next Action
switch(CurrentActionID)
{
// SUB_STRATEGY_BEGIN_LOOP
case 1: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 50; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = MainStrategy
case 50: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 75; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_GLASS_1_DONE; p->Param2 = 100; p->Param3 = 55; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 55: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 60; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GLASS_1_DONE; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 60: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1000; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 75: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 3000; p->Param3 = 0; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 100: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 125; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_GLASS_2_DONE; p->Param2 = 150; p->Param3 = 110; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 110: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 115; TimeoutID = 150; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 2400; p->Param3 = 1100; p->Param4 = 0; break; // SubStrategyName = MainStrategy
case 115: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 117; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GLASS_2_DONE; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 117: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2000; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 125: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 3000; p->Param3 = 0; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 150: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 175; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_1_DONE + APP_PARAM_STRATEGYFLAG_GIFT_1_TRY); p->Param2 = 200; p->Param3 = 155; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 155: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 160; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_1_TRY; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 160: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 165; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 165: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 4000; TimeoutID = 940; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 2400; p->Param3 = 450; p->Param4 = -90; break; // SubStrategyName = MainStrategy
case 175: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 180; p->Param3 = 0; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 180: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_1_DONE; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 200: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 225; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_2_DONE + APP_PARAM_STRATEGYFLAG_GIFT_2_TRY); p->Param2 = 250; p->Param3 = 205; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 205: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 210; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_2_TRY + APP_PARAM_STRATEGYFLAG_GLASS_2_DONE); p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 210: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 215; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 215: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 4000; TimeoutID = 940; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1800; p->Param3 = 450; p->Param4 = -90; break; // SubStrategyName = MainStrategy
case 225: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 230; p->Param3 = 0; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 230: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_2_DONE; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 250: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 275; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_3_DONE + APP_PARAM_STRATEGYFLAG_GIFT_3_TRY); p->Param2 = 300; p->Param3 = 255; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 255: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 260; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_3_TRY; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 260: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 265; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 265: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 4000; TimeoutID = 940; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1200; p->Param3 = 450; p->Param4 = -90; break; // SubStrategyName = MainStrategy
case 275: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 280; p->Param3 = 0; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 280: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_3_DONE; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 300: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 325; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_4_DONE + APP_PARAM_STRATEGYFLAG_GIFT_4_TRY); p->Param2 = 950; p->Param3 = 305; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 305: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 310; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_4_TRY; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 310: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 315; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 315: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 4000; TimeoutID = 942; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 600; p->Param3 = 450; p->Param4 = -90; break; // SubStrategyName = MainStrategy
case 325: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 330; p->Param3 = 0; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 330: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 335; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_GIFT_4_DONE; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 335: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 950; TimeoutID = -1; p->Cmd = MvtSimple_RotateToAngleInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 0; break; // SubStrategyName = MainStrategy
case 940: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 0; TimeoutID = 0; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -200; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 942: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 960; TimeoutID = 0; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -200; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 950: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 955; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_1_TRY + APP_PARAM_STRATEGYFLAG_GIFT_2_TRY + APP_PARAM_STRATEGYFLAG_GIFT_3_TRY + APP_PARAM_STRATEGYFLAG_GIFT_4_TRY); p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 955: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 0; TimeoutID = 960; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 2400; p->Param3 = 600; p->Param4 = 180; break; // SubStrategyName = MainStrategy
case 960: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 970; TimeoutID = 0; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1500; p->Param3 = 800; p->Param4 = 180; break; // SubStrategyName = MainStrategy
case 970: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = (APP_PARAM_STRATEGYFLAG_GIFT_1_TRY + APP_PARAM_STRATEGYFLAG_GIFT_2_TRY + APP_PARAM_STRATEGYFLAG_GIFT_3_TRY + APP_PARAM_STRATEGYFLAG_GIFT_4_TRY); p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 3000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 3010; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = Depose
case 3010: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 3020; TimeoutID = -1; p->Cmd = MvtSimple_RotateToAngleInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 180; break; // SubStrategyName = Depose
case 3020: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 3029; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_UP; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Depose
case 3029: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 3030; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 500; break; // SubStrategyName = Depose
case 3030: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 3040; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -300; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Depose
case 3040: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 0; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 300; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Depose
case 4000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4005; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = Gift
case 4005: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4015; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_FALSE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4015: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4020; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_UP; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4020: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4025; TimeoutID = -1; p->Cmd = Sensors_SetArmsStatus; p->Param1 = ARM_CLOSED; p->Param2 = ARM_FRONT; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4025: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4030; TimeoutID = 4035; p->Cmd = MvtSimple_MoveInMM; p->Param1 = 40; p->Param2 = 350; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4030: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4035; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4035: p->CmdType = CmdType_NonBlocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4040; TimeoutID = -1; p->Cmd = Sensors_SetArmsStatus; p->Param1 = ARM_CLOSED; p->Param2 = ARM_CLOSED; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4040: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4045; TimeoutID = 4090; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -200; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4045: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 0; TimeoutID = 4090; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Gift
case 4090: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = MvtSimple_RotateToAngleInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 180; break; // SubStrategyName = Gift
case 1000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1010; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = Glass1
case 1010: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_LONG_REAR); NextActionID = 1100; TimeoutID = 1300; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 1180; p->Param3 = 547; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1100: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1110; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_DOWN; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1110: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1120; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 500; break; // SubStrategyName = Glass1
case 1120: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT); NextActionID = 1130; TimeoutID = -1; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_PIVOT_SPEED; p->Param2 = LEFT_WHEEL; p->Param3 = NotUsed; p->Param4 = 90; break; // SubStrategyName = Glass1
case 1130: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 1140; TimeoutID = -1; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_PIVOT_SPEED; p->Param2 = RIGHT_WHEEL; p->Param3 = NotUsed; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1140: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 1150; TimeoutID = 1400; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2300; p->Param3 = 817; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1150: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 1160; TimeoutID = 1600; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 2720; p->Param3 = 630; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1160: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 1170; TimeoutID = -1; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2350; p->Param3 = 630; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1170: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1300: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 1100; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1400: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 1401; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1401: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1410; TimeoutID = -1; p->Cmd = MvtSimple_RotateInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 90; break; // SubStrategyName = Glass1
case 1410: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT); NextActionID = 1411; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1403; p->Param3 = 400; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1411: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 1160; TimeoutID = 1500; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2720; p->Param3 = 630; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1500: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 1501; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -100; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1501: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1502; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_UP; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1502: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1500; p->Param3 = 1000; p->Param4 = 0; break; // SubStrategyName = Glass1
case 1600: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 1601; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -300; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass1
case 1601: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1610; TimeoutID = -1; p->Cmd = MvtSimple_RotateInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 90; break; // SubStrategyName = Glass1
case 1610: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 1411; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1403; p->Param3 = 400; p->Param4 = 0; break; // SubStrategyName = Glass1
case 2000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2010; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = Glass2
case 2010: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_LONG_REAR); NextActionID = 2100; TimeoutID = 2300; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 1180; p->Param3 = 1050; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2100: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2110; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_DOWN; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2110: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2120; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 500; break; // SubStrategyName = Glass2
case 2120: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 2130; TimeoutID = -1; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_PIVOT_SPEED; p->Param2 = RIGHT_WHEEL; p->Param3 = NotUsed; p->Param4 = -90; break; // SubStrategyName = Glass2
case 2130: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT); NextActionID = 2140; TimeoutID = -1; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_PIVOT_SPEED; p->Param2 = LEFT_WHEEL; p->Param3 = NotUsed; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2140: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 2145; TimeoutID = 2400; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2350; p->Param3 = 1400; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2145: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2150; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 2650; p->Param3 = 1400; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2150: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 2160; TimeoutID = 2600; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2270; p->Param3 = 1400; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2160: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2300: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 2100; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2400: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 2401; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2401: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2410; TimeoutID = -1; p->Cmd = MvtSimple_RotateInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 90; break; // SubStrategyName = Glass2
case 2410: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT); NextActionID = 2411; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1600; p->Param3 = 550; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2411: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 2412; TimeoutID = 2500; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2675; p->Param3 = 600; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2412: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 2420; TimeoutID = -1; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 2300; p->Param3 = 600; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2420: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2500: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 2501; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -100; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2501: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2502; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_UP; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = Glass2
case 2502: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1500; p->Param3 = 1000; p->Param4 = 0; break; // SubStrategyName = Glass2
case 2600: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT); NextActionID = 2420; TimeoutID = 2500; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 2350; p->Param3 = 630; p->Param4 = 0; break; // SubStrategyName = Glass2
// SUB_STRATEGY_END_LOOP
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Check for Wait command --------------------------------------------
if(App_Wait == p->Cmd)
{
// Nothing to do. All is done in TaskMain
}
// Check for conditionnal command -----------------------------
if((App_IfGoto_System == p->Cmd) || (App_IfGoto_Strategy == p->Cmd))
{
// Read the current Flags
if(App_IfGoto_System == p->Cmd)
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
else
CurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if(NextActionID >= 0)
SubStrategyReturnID = NextActionID;
// If condition is true, we go to the ID given by Param2, otherwise we use Param3
if(((p->Param1 == -1) || (CurrentFlag & (p->Param1)) != 0))
{
NextActionID = (int)(p->Param2);
}
else
{
NextActionID = (int)(p->Param3);
}
}
// Set / Clear Strategy Flags --------------------------------------
if(App_SetStrategyFlags == p->Cmd)
{
// Nothing to do. All is done in TaskMain
}
// Create the MvtSimple Command --------------------------------------
if(MvtSimple_MoveInMM == p->Cmd)
LibMoving_MoveInMM(p->Param2, p->Param1, p);
if(MvtSimple_RotateInDeg == p->Cmd)
LibMoving_RotateInDeg(p->Param4, p->Param1, p);
if(MvtSimple_RotateToAngleInDeg == p->Cmd)
LibMoving_RotateToAngleInDeg(p->Param4, p->Param1, p);
// Angle Conversion --------------------------------------------------
if((Mvt_UsePivotMode == p->Cmd) || (Mvt_UseMixedMode == p->Cmd) || (Mvt_UseSpline == p->Cmd))
{
p->Param4 = AppConvertDegInRad(p->Param4);
}
return ERR__NO_ERROR;
}
ADI_OSAL_STATUS adi_osal_EventPend (ADI_OSAL_EVENT_HANDLE const hEventGroup,
ADI_OSAL_EVENT_FLAGS nRequestedEvents,
ADI_OSAL_EVENT_FLAG_OPTION eGetOption,
ADI_OSAL_TICKS nTimeoutInTicks,
ADI_OSAL_EVENT_FLAGS *pnReceivedEvents)
{
INT8U nErr;
INT8U nWaitOption = OS_FLAG_WAIT_SET_ALL;
OS_FLAGS nRetValue;
ADI_OSAL_STATUS eRetStatus;
#pragma diag(push)
#pragma diag(suppress:misra_rule_11_4 : "typecasting is necessary to convert the handle type into a native uCOS pointer")
OS_FLAG_GRP *hEventNative = (OS_FLAG_GRP*) hEventGroup;
#pragma diag(pop)
#ifdef OSAL_DEBUG
if ( (nTimeoutInTicks > ADI_OSAL_MAX_TIMEOUT) &&
(nTimeoutInTicks != ADI_OSAL_TIMEOUT_FOREVER) )
{
return (ADI_OSAL_BAD_TIME);
}
if ((CALLED_FROM_AN_ISR) || (CALLED_IN_SCHED_LOCK_REGION))
{
return (ADI_OSAL_CALLER_ERROR);
}
if ( (eGetOption != ADI_OSAL_EVENT_FLAG_ANY) &&
(eGetOption != ADI_OSAL_EVENT_FLAG_ALL) )
{
return (ADI_OSAL_BAD_OPTION);
}
if ((NULL == hEventGroup) || (ADI_OSAL_INVALID_EVENT_GROUP == hEventGroup))
{
return(ADI_OSAL_BAD_HANDLE);
}
#endif /* OSAL_DEBUG */
if(eGetOption == ADI_OSAL_EVENT_FLAG_ANY)
{
nWaitOption = OS_FLAG_WAIT_SET_ANY;
}
switch (nTimeoutInTicks)
{
case ADI_OSAL_TIMEOUT_NONE:
nRetValue = OSFlagAccept(hEventNative, (OS_FLAGS) nRequestedEvents, nWaitOption, &nErr);
break;
case ADI_OSAL_TIMEOUT_FOREVER:
nRetValue = OSFlagPend(hEventNative, (OS_FLAGS) nRequestedEvents, nWaitOption, 0u, &nErr);
break;
default:
#pragma diag(push)
#pragma diag(suppress:misra_rule_10_1_c : "we have checked that the timeout is 16-bit or less in debug so we can cast it without losing information")
nRetValue = OSFlagPend(hEventNative, (OS_FLAGS) nRequestedEvents, nWaitOption, (INT16U) nTimeoutInTicks, &nErr);
break;
#pragma diag(pop)
}
switch (nErr)
{
case OS_ERR_NONE:
eRetStatus = ADI_OSAL_SUCCESS;
break;
case OS_ERR_TIMEOUT:
eRetStatus = ADI_OSAL_TIMEOUT;
break;
#ifdef OSAL_DEBUG
case OS_ERR_EVENT_TYPE: /* FALLTHROUGH */
case OS_ERR_FLAG_INVALID_PGRP:
eRetStatus = ADI_OSAL_BAD_HANDLE;
break;
case OS_ERR_PEND_ISR:
eRetStatus = ADI_OSAL_CALLER_ERROR;
break;
case OS_ERR_FLAG_WAIT_TYPE:
eRetStatus = ADI_OSAL_BAD_OPTION;
break;
#endif /* OSAL_DEBUG */
default:
eRetStatus = ADI_OSAL_FAILED;
break;
}
/* uC/OS also sets its return value (nRetValue) to be 0 in case of error */
*pnReceivedEvents = (uint32_t) nRetValue;
return( eRetStatus );
}
// ------------------------------------------------------------------------------------------------
INT8U StrategyColorA_GetNextAction(StructCmd *NextAction)
{
static int NextActionID = 0;
static int TimeoutID = -1;
static int SubStrategyReturnID = -1;
int CurrentActionID = 0;
INT8U Err = 0;
OS_FLAGS CurrentFlag = 0;
OS_FLAGS StrategyFlagsToSet = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
p->CmdType = CmdType_Blocking;
// Set CurrentID to NextID
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_WAIT_SET_ANY, &Err);
if(((CurrentFlag & APP_PARAM_APPFLAG_ACTION_TIMEOUT) == APP_PARAM_APPFLAG_ACTION_TIMEOUT) && (TimeoutID != -1))
{
CurrentActionID = TimeoutID;
// Clear Timeout Flag
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_CLR, &Err);
}
else
CurrentActionID = NextActionID;
// Check if NextID must be change due to previous order
if(CurrentActionID == 0)
{
// We have to check previous order
if(SubStrategyReturnID > 0)
CurrentActionID = SubStrategyReturnID; // Change NextID to return to the correct ID after SubStrategy loop
else
CurrentActionID = 1; // There is no pending action, we go to the first state
}
// Read Next Action
switch(CurrentActionID)
{
// SUB_STRATEGY_BEGIN_LOOP
case 1: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1000; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = MainStrategy
case 2: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2000; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 500; p->Param3 = 630; p->Param4 = 180; break; // SubStrategyName = MainStrategy
case 2000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2010; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = DV
case 2010: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 2020; TimeoutID = -1; p->Cmd = MvtSimple_RotateToAngleInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 0; break; // SubStrategyName = DV
case 2020: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2029; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_UP; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = DV
case 2029: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2030; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 500; break; // SubStrategyName = DV
case 2030: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2040; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = DV
case 2040: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT); NextActionID = -1; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = DV
case 1000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1010; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = V1
case 1010: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_LONG_REAR); NextActionID = 1100; TimeoutID = 1300; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 1820; p->Param3 = 547; p->Param4 = 180; break; // SubStrategyName = V1
case 1100: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1110; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_DOWN; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = V1
case 1110: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1120; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 500; break; // SubStrategyName = V1
case 1120: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT); NextActionID = 1130; TimeoutID = -1; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_PIVOT_SPEED; p->Param2 = RIGHT_WHEEL; p->Param3 = NotUsed; p->Param4 = 90; break; // SubStrategyName = V1
case 1130: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 1140; TimeoutID = -1; p->Cmd = Mvt_UsePivotMode; p->Param1 = DEFAULT_PIVOT_SPEED; p->Param2 = LEFT_WHEEL; p->Param3 = NotUsed; p->Param4 = 180; break; // SubStrategyName = V1
case 1140: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT); NextActionID = 1150; TimeoutID = 1400; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 700; p->Param3 = 817; p->Param4 = 180; break; // SubStrategyName = V1
case 1150: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT + APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT); NextActionID = 1160; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 280; p->Param3 = 630; p->Param4 = 180; break; // SubStrategyName = V1
case 1160: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 2; TimeoutID = -1; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 500; p->Param3 = 630; p->Param4 = 180; break; // SubStrategyName = V1
case 1300: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT); NextActionID = 1100; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = 150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = V1
case 1400: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 1401; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -150; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = V1
case 1401: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1410; TimeoutID = -1; p->Cmd = MvtSimple_RotateInDeg; p->Param1 = DEFAULT_SPEED; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = 90; break; // SubStrategyName = V1
case 1410: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT + APP_PARAM_STRATEGYFLAG_COLLISION_LEFT); NextActionID = 1411; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1597; p->Param3 = 400; p->Param4 = 180; break; // SubStrategyName = V1
case 1411: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_FRONT); NextActionID = 1160; TimeoutID = 1500; p->Cmd = Mvt_UseSpline; p->Param1 = DEFAULT_SPEED; p->Param2 = 280; p->Param3 = 630; p->Param4 = 180; break; // SubStrategyName = V1
case 1500: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_COLLISION_REAR); NextActionID = 1501; TimeoutID = -1; p->Cmd = MvtSimple_MoveInMM; p->Param1 = DEFAULT_SPEED; p->Param2 = -100; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = V1
case 1501: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1502; TimeoutID = -1; p->Cmd = Sensors_SetHoopLevel; p->Param1 = HOOP_LEVEL_UP; p->Param2 = NotUsed; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = V1
case 1502: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2; TimeoutID = -1; p->Cmd = Mvt_UseMixedMode; p->Param1 = DEFAULT_SPEED; p->Param2 = 1500; p->Param3 = 1000; p->Param4 = 180; break; // SubStrategyName = V1
// SUB_STRATEGY_END_LOOP
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Check for Wait command --------------------------------------------
if(App_Wait == p->Cmd)
{
// Execute the wait command
OSTimeDlyHMSM(p->Param1, p->Param2, p->Param3, p->Param4);
return StrategyColorA_GetNextAction(p);
}
// Check for conditionnal system command -----------------------------
if(App_IfGoto_System == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if(NextActionID >= 0)
SubStrategyReturnID = NextActionID;
// If condition is true, we go to the ID given by Param2, otherwise we use Param3
if((CurrentFlag & (p->Param1)) != 0)
{
NextActionID = (int)(p->Param2);
}
else
{
NextActionID = (int)(p->Param3);
}
return StrategyColorA_GetNextAction(p);
}
// Check for conditionnal strategy command ---------------------------
if(App_IfGoto_Strategy == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if(NextActionID >= 0)
SubStrategyReturnID = NextActionID;
// If condition is true, we go to the ID given by Param2, otherwise we use Param3
if((CurrentFlag & (p->Param1)) != 0)
{
NextActionID = (int)(p->Param2);
}
else
{
NextActionID = (int)(p->Param3);
}
return StrategyColorA_GetNextAction(p);
}
// Set / Clear Strategy Flags --------------------------------------
if(App_SetStrategyFlags == p->Cmd)
{
StrategyFlagsToSet = p->Param1;
if(p->Param2 != 0)
OSFlagPost(AppStrategyFlags, StrategyFlagsToSet, OS_FLAG_SET, &Err);
else
OSFlagPost(AppStrategyFlags, StrategyFlagsToSet, OS_FLAG_CLR, &Err);
return StrategyColorA_GetNextAction(p);
}
// Create the MvtSimple Command --------------------------------------
if(MvtSimple_MoveInMM == p->Cmd)
LibMoving_MoveInMM(p->Param2, p->Param1, p);
if(MvtSimple_RotateInDeg == p->Cmd)
LibMoving_RotateInDeg(p->Param4, p->Param1, p);
if(MvtSimple_RotateToAngleInDeg == p->Cmd)
LibMoving_RotateToAngleInDeg(p->Param4, p->Param1, p);
// Angle Conversion --------------------------------------------------
if((Mvt_UsePivotMode == p->Cmd) || (Mvt_UseMixedMode == p->Cmd) || (Mvt_UseSpline == p->Cmd))
{
p->Param4 = AppConvertDegInRad(p->Param4);
}
return ERR__NO_ERROR;
}
// ------------------------------------------------------------------------------------------------
INT8U StrategyColorA_GetNextAction(StructCmd *NextAction)
{
static int NextActionID = 0;
static int TimeoutID = -1;
static int SubStrategyReturnID = -1;
int CurrentActionID = 0;
INT8U Err = 0;
OS_FLAGS CurrentFlag = 0;
OS_FLAGS StrategyFlagsToSet = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
p->CmdType = CmdType_Blocking;
// Set CurrentID to NextID
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_WAIT_SET_ANY, &Err);
if(((CurrentFlag & APP_PARAM_APPFLAG_ACTION_TIMEOUT) == APP_PARAM_APPFLAG_ACTION_TIMEOUT) && (TimeoutID != -1))
{
CurrentActionID = TimeoutID;
// Clear Timeout Flag
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_CLR, &Err);
}
else
CurrentActionID = NextActionID;
// Check if NextID must be change due to previous order
if(CurrentActionID == 0)
{
// We have to check previous order
if(SubStrategyReturnID > 0)
CurrentActionID = SubStrategyReturnID; // Change NextID to return to the correct ID after SubStrategy loop
else
CurrentActionID = 1; // There is no pending action, we go to the first state
}
// Read Next Action
switch(CurrentActionID)
{
// SUB_STRATEGY_BEGIN_LOOP
case 1: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = MainStrategy
case 2: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 3; TimeoutID = -1; p->Cmd = App_SetStrategyFlags; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = OS_TRUE; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 3: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 4; TimeoutID = -1; p->Cmd = App_IfGoto_Strategy; p->Param1 = APP_PARAM_STRATEGYFLAG_SUBSTRATEGY_RESULT; p->Param2 = 2000; p->Param3 = 1000; p->Param4 = NotUsed; break; // SubStrategyName = MainStrategy
case 4: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = -1; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 10; p->Param4 = 0; break; // SubStrategyName = MainStrategy
case 1000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 1001; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = OpenArm
case 1001: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = Sensors_SetArmsStatus; p->Param1 = ARM_OPEN; p->Param2 = ARM_OPEN; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = OpenArm
case 2000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 2001; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = FrontArm
case 2001: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = Sensors_SetArmsStatus; p->Param1 = ARM_FRONT; p->Param2 = ARM_FRONT; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = FrontArm
case 3000: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 3001; TimeoutID = -1; p->Cmd = App_Wait; p->Param1 = 0; p->Param2 = 0; p->Param3 = 0; p->Param4 = 1; break; // SubStrategyName = ArmClose
case 3001: p->CmdType = CmdType_Blocking; p->ActiveSensorsFlag = (APP_PARAM_STRATEGYFLAG_NONE); NextActionID = 0; TimeoutID = -1; p->Cmd = Sensors_SetArmsStatus; p->Param1 = ARM_CLOSED; p->Param2 = ARM_CLOSED; p->Param3 = NotUsed; p->Param4 = NotUsed; break; // SubStrategyName = ArmClose
// SUB_STRATEGY_END_LOOP
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Check for Wait command --------------------------------------------
if(App_Wait == p->Cmd)
{
// Execute the wait command
OSTimeDlyHMSM(p->Param1, p->Param2, p->Param3, p->Param4);
return StrategyColorA_GetNextAction(p);
}
// Check for conditionnal system command -----------------------------
if(App_IfGoto_System == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if(NextActionID >= 0)
SubStrategyReturnID = NextActionID;
// If condition is true, we go to the ID given by Param2, otherwise we use Param3
if((CurrentFlag & (p->Param1)) != 0)
{
NextActionID = (int)(p->Param2);
}
else
{
NextActionID = (int)(p->Param3);
}
return StrategyColorA_GetNextAction(p);
}
// Check for conditionnal strategy command ---------------------------
if(App_IfGoto_Strategy == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
if(NextActionID >= 0)
SubStrategyReturnID = NextActionID;
// If condition is true, we go to the ID given by Param2, otherwise we use Param3
if((CurrentFlag & (p->Param1)) != 0)
{
NextActionID = (int)(p->Param2);
}
else
{
NextActionID = (int)(p->Param3);
}
return StrategyColorA_GetNextAction(p);
}
// Set / Clear Strategy Flags --------------------------------------
if(App_SetStrategyFlags == p->Cmd)
{
StrategyFlagsToSet = p->Param1;
if(p->Param2 != 0)
OSFlagPost(AppStrategyFlags, StrategyFlagsToSet, OS_FLAG_SET, &Err);
else
OSFlagPost(AppStrategyFlags, StrategyFlagsToSet, OS_FLAG_CLR, &Err);
return StrategyColorA_GetNextAction(p);
}
// Create the MvtSimple Command --------------------------------------
if(MvtSimple_MoveInMM == p->Cmd)
LibMoving_MoveInMM(p->Param2, p->Param1, p);
if(MvtSimple_RotateInDeg == p->Cmd)
LibMoving_RotateInDeg(p->Param4, p->Param1, p);
if(MvtSimple_RotateToAngleInDeg == p->Cmd)
LibMoving_RotateToAngleInDeg(p->Param4, p->Param1, p);
// Angle Conversion --------------------------------------------------
if((Mvt_UsePivotMode == p->Cmd) || (Mvt_UseMixedMode == p->Cmd) || (Mvt_UseSpline == p->Cmd))
{
p->Param4 = AppConvertDegInRad(p->Param4);
}
return ERR__NO_ERROR;
}
// ------------------------------------------------------------------------------------------------
INT8U Strategy_GetNextAction(EnumColor CurrentColor, StructCmd *NextAction)
{
static unsigned int CurrentActionID = 0;
INT8U Err = 0;
OS_FLAGS CurrentFlag = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
NextAction->CmdType = CmdType_Blocking;
switch(CurrentColor)
{
case c_ColorA: // Blue #############################################################
switch(CurrentActionID)
{
// case 0: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
// case 1: OSTimeDlyHMSM(0, 0, 1, 0); NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN_RIGHT_ONLY; CurrentActionID++; break;
// case 2: OSTimeDlyHMSM(0, 0, 1, 0); NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN_LEFT_ONLY; CurrentActionID++; break;
// case 3: OSTimeDlyHMSM(0, 0, 1, 0); NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
// case 4: OSTimeDlyHMSM(0, 0, 1, 0); NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_GRAB; CurrentActionID++; break;
// case 0: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = 50; NextAction->Param2 = 400; NextAction->Param3 = 0; NextAction->Param4 = AppConvertDegInRad(0.0); NextAction->CmdType = CmdType_Blocking; break;
// case 1: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = 50; NextAction->Param2 = 400; NextAction->Param3 = 400; NextAction->Param4 = AppConvertDegInRad(90.0); NextAction->CmdType = CmdType_Blocking; break;
// case 2: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = 50; NextAction->Param2 = 0; NextAction->Param3 = 400; NextAction->Param4 = AppConvertDegInRad(180.0); NextAction->CmdType = CmdType_Blocking; break;
// case 3: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = 50; NextAction->Param2 = 0; NextAction->Param3 = 0; NextAction->Param4 = AppConvertDegInRad(0.0); NextAction->CmdType = CmdType_Blocking; break;
case 0: NextAction->Cmd = Mvt_Simple; LibMoving_MoveInMM(450, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
case 1: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN_RIGHT_ONLY; CurrentActionID++; break;
case 2: NextAction->Cmd = Mvt_Simple; LibMoving_RotateInDeg(30, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
case 3: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
case 4: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 700; NextAction->Param3 = 310; NextAction->Param4 = AppConvertDegInRad(45.0); NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_FRONT_SENSORS; CurrentActionID++; break;
case 5: // Select next action from flags
NextAction->Cmd = Cmd_NotSet;
OSTimeDlyHMSM(0, 0, 0, 100);
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_GP2_HOLDER, OS_FLAG_WAIT_SET_ANY, &Err);
if((CurrentFlag & APP_PARAM_APPFLAG_GP2_HOLDER) == APP_PARAM_APPFLAG_GP2_HOLDER)
CurrentActionID = 100;
else
CurrentActionID = 6;
break;
case 6: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 1500; NextAction->Param3 = 350; NextAction->Param4 = AppConvertDegInRad(0.0); NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_FRONT_SENSORS; CurrentActionID++; break;
case 7: // Select next action from flags
NextAction->Cmd = Cmd_NotSet;
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_GP2_HOLDER, OS_FLAG_WAIT_SET_ANY, &Err);
if(CurrentFlag & APP_PARAM_APPFLAG_GP2_HOLDER == APP_PARAM_APPFLAG_GP2_HOLDER)
CurrentActionID = 200;
else
CurrentActionID = 8;
break;
case 8: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 1500; NextAction->Param3 = 1400; NextAction->Param4 = AppConvertDegInRad(90.0); NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_FRONT_SENSORS; CurrentActionID++; break;
case 9: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_GRAB; CurrentActionID++; break;
case 10: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 1500; NextAction->Param3 = 1750; NextAction->Param4 = AppConvertDegInRad(135.0); NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_FRONT_SENSORS; CurrentActionID++; break;
case 11: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
case 12: NextAction->Cmd = Mvt_Simple; LibMoving_MoveInMM(-500, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID = 255; break;
// Sub strategy 1
case 100: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_GRAB; CurrentActionID++; break;
case 101: NextAction->Cmd = Mvt_Simple; LibMoving_RotateInDeg(90, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
case 102: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 729.4; NextAction->Param3 = 420.6; NextAction->Param4 = AppConvertDegInRad(135.0); NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_FRONT_SENSORS; CurrentActionID++; break;
case 103: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
case 104: NextAction->Cmd = Mvt_Simple; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 800; NextAction->Param3 = 350; NextAction->Param4 = USE_CURRENT_VALUE; NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_BACK_SENSORS; CurrentActionID++; break;
case 105: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_CLOSE; CurrentActionID++; break;
case 106: NextAction->Cmd = Mvt_Simple; LibMoving_MoveToAngleInDeg(0, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
case 107: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID = 6; break;
// Sub strategy 2
case 200: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_GRAB; CurrentActionID++; break;
case 201: NextAction->Cmd = Mvt_Simple; LibMoving_RotateInDeg(-45, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
case 202: NextAction->Cmd = Mvt_UseMixedMode; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 1576.6; NextAction->Param3 = 279.4; NextAction->Param4 = AppConvertDegInRad(135.0); NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_FRONT_SENSORS; CurrentActionID++; break;
case 203: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
case 204: NextAction->Cmd = Mvt_Simple; NextAction->Param1 = APP_HOMOL_ROBOT_SPEED; NextAction->Param2 = 1500; NextAction->Param3 = 350; NextAction->Param4 = USE_CURRENT_VALUE; NextAction->CmdType = CmdType_Blocking; NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_BACK_SENSORS; CurrentActionID = 8; break;
case 205: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_CLOSE; CurrentActionID++; break;
case 206: NextAction->Cmd = Mvt_Simple; LibMoving_MoveToAngleInDeg(90, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
case 207: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID = 8; break;
case 255:
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Update ActionID
// CurrentActionID++;
break;
case c_ColorB: // Red ##############################################################
switch(CurrentActionID)
{
// case 0: NextAction->Cmd = Mvt_Simple; LibMoving_MoveInMM(450, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
// case 1: NextAction->Cmd = Mvt_Simple; LibMoving_MoveInMM(-450, APP_HOMOL_ROBOT_SPEED, NextAction); CurrentActionID++; break;
// case 1: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
// case 0: NextAction->Cmd = App_SetNewPos; NextAction->CmdType = CmdType_NonBlocking; NextAction->Param2 = 1500.0; NextAction->Param3 = 1400.0; NextAction->Param4 = AppConvertDegInRad(90.0); NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; CurrentActionID++; break;
// case 1: NextAction->Cmd = Sensors_SetHolderStatus; NextAction->Param1 = HOLDER_OPEN; CurrentActionID++; break;
case 0: NextAction->Cmd = App_SetNewPos; NextAction->CmdType = CmdType_NonBlocking; NextAction->Param2 = 1875.0; NextAction->Param3 = 1575.0; NextAction->Param4 = AppConvertDegInRad(0.0); NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; CurrentActionID++; break;
case 1: NextAction->Cmd = Sensors_SetHolderStatus;
NextAction->Param1 = HOLDER_GRAB;
CurrentActionID=1201;
break;
case 1200: NextAction->Cmd = Mvt_Simple;
NextAction->Param1 = APP_DEFAULT_ROBOT_SPEED; NextAction->Param2 = 1675; NextAction->Param3 = 1575; NextAction->Param4 = AppConvertDegInRad(0.0);
NextAction->CmdType = CmdType_Blocking;
NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_BACK_SENSORS;
CurrentActionID++;
break;
case 1201: NextAction->Cmd = Mvt_Simple;
LibMoving_MoveToAngleInDeg(-90, APP_DEFAULT_ROBOT_SPEED, NextAction);
CurrentActionID++;
break;
case 1202: NextAction->Cmd = Mvt_Simple;
LibMoving_MoveToAngleInDeg(-180, APP_DEFAULT_ROBOT_SPEED, NextAction);
CurrentActionID++;
break;
case 1203: NextAction->Cmd = Sensors_SetHolderStatus;
NextAction->Param1 = HOLDER_GRIP;
CurrentActionID++;
break;
case 1204: NextAction->Cmd = Sensors_SetHolderLevel;
NextAction->Param1 = HOLDER_LEVEL_HIGH;
CurrentActionID++;
break;
case 1205: NextAction->Cmd = Mvt_UseMixedMode;
NextAction->Param1 = APP_DEFAULT_ROBOT_SPEED; NextAction->Param2 = 1675; NextAction->Param3 = 1750; NextAction->Param4 = AppConvertDegInRad(90.0);
NextAction->CmdType = CmdType_Blocking;
NextAction->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE;
CurrentActionID++;
break;
case 1206: NextAction->Cmd = Sensors_SetHolderStatus;
NextAction->Param1 = HOLDER_OPEN;
CurrentActionID++;
break;
// case 1: NextAction->Cmd = Sensors_SetHolderLevel; NextAction->Param1 = HOLDER_LEVEL_HIGH; CurrentActionID++; break;
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
// Update ActionID
//CurrentActionID++;
break;
default: // Not Set ##########################################################
break;
}
return ERR__NO_ERROR;
}
// ------------------------------------------------------------------------------------------------
// TaskMvt_Main()
// ------------------------------------------------------------------------------------------------
void TaskMvt_Main(void *p_arg)
{
// Vars
StructCmd CurrentPath[APP_MOVING_SEQ_LEN]; // Data used for storing path cmd
StructCmd CurrentCmd; // Data for storing current order from TaskMain (to be done)
StructCmd CurrentCmdForTest; // Data for testing current order from TaskMain (to be done)
StructCmd StopCmd; // Command used for stopping the current mvt
StructCmd LastSetpointSent; // Data to store last setpoint sent to TaskAsser
StructPos CurrentPos; // Data used for storing current pos from TaskOdo
OS_FLAGS SystemCurrentFlag; // Var to read system current flag
OS_FLAGS StrategyCurrentFlag; // Var to read strategy current flag
OS_FLAGS CurrentActivatedSensors; // Sensors used for collision
OS_FLAGS SensorsCurrentStatus; // Var to store current sensors status
INT8U CurrentState; // Var used for storing current state for state machine
INT8U NextState; // Var used for storing next state for state machine
INT8U Err; // Var to get error status
INT8U i; // Iterator
INT8U EscapeFrontFlag; // Flag used to select which side to use during an escape seq (front)
INT8U EscapeRearFlag; // Flag used to select which side to use during an escape seq (rear)
INT8S CurrentSetpoint; // Pointer to Current setpoint to reach
INT16U MvtTimeout; // Counter to detect blocked mvt
INT8S CollisionTimeout; // Counter to detect a blocking movment (due to a GP2 detection)
unsigned int LastMainCmdId; // Var to store last command received from TaskMain
unsigned int SensorsPreviousStatus; // Var to store previous sensors status to detect Sensors status modification
#ifdef _TARGET_440H
char Debug_State[4];
#endif
// Init
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
memset(&CurrentCmd, 0, 1 * sizeof(StructCmd));
memset(&CurrentPos, 0, 1 * sizeof(StructPos));
// Define stop cmd
StopCmd.Cmd = Mvt_Stop;
StopCmd.Param1 = 40;
StopCmd.Param2 = USE_CURRENT_VALUE;
StopCmd.Param3 = USE_CURRENT_VALUE;
StopCmd.Param4 = USE_CURRENT_VALUE;
StopCmd. ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE;
SystemCurrentFlag = 0;
StrategyCurrentFlag = 0;
CurrentState = 0;
NextState = 0;
Err = 0;
EscapeFrontFlag = 0;
EscapeRearFlag = 0;
CurrentSetpoint = -1;
MvtTimeout = 0;
LastMainCmdId = 0;
SensorsCurrentStatus = 0;
SensorsPreviousStatus = 0;
CurrentActivatedSensors = 0;
#ifdef _TARGET_440H
memset(Debug_State, 0, 4*sizeof(char));
#endif
AppDebugMsg("OUFFF TEAM 2013 : Mvt online\n");
// MAIN LOOP ==================================================================================
do
{
// Proc Release
OSTimeDlyHMSM(0, 0, 0, 10);
// Update current state
CurrentState = NextState;
Debug_MvtState = CurrentState;
// Check FLAGS for MvtTask
SystemCurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
#ifdef _TARGET_440H
sprintf(Debug_State, "%03d", CurrentState);
Set_Line_Information( 1, 0, Debug_State, 3);
OSTimeDlyHMSM(0, 0, DELAY_S_BETWEEN_NEXT_STATE, DELAY_MS_BETWEEN_NEXT_STATE);
#endif
TaskDebug_UpdateValueInt(TASKDEBUG_ID_MVT_STATE, CurrentState);
// State machine
switch(CurrentState)
{
// CASE 000 ---------------------------------------------------------------------------
case 0: // Init state
// Check for 'start button' status
if((SystemCurrentFlag & APP_PARAM_APPFLAG_START_BUTTON) != 0)
{
// Start button has been pressed
NextState = 1;
}
else
{
// Start button is still inactive
NextState = 0;
// Check if initial position has been changed
// Ask for Mutex
OSMutexPend(App_MutexCmdToTaskMvt, WAIT_FOREVER, &Err);
{
// Get current Cmd
if(App_SetNewPos == App_CmdToTaskMvt.Cmd)
{
TaskMvt_SendSetpointToTaskAsser(&App_CmdToTaskMvt);
// Clear this msg
memset(&App_CmdToTaskMvt, 0, sizeof(StructCmd) * 1);
LastMainCmdId = App_CmdToTaskMvtId;
}
}
OSMutexPost(App_MutexCmdToTaskMvt);
// Place here code for executing Cmd (if necessary)
}
break;
// CASE 001 ---------------------------------------------------------------------------
case 1: // Read Timer flag
if((SystemCurrentFlag & APP_PARAM_APPFLAG_TIMER_STATUS) == APP_PARAM_APPFLAG_TIMER_STATUS)
NextState = 255; // Time is up
else
NextState = 2; // Time is running
break;
// CASE 002 ---------------------------------------------------------------------------
case 2: // Read Msg from Main
// Check Last CmdID used, if a new msg is ready, use it
if(App_CmdToTaskMvtId > LastMainCmdId)
{
// Ask for Mutex
OSMutexPend(App_MutexCmdToTaskMvt, WAIT_FOREVER, &Err);
{
// Get current Cmd
memcpy(&CurrentCmd, &App_CmdToTaskMvt, sizeof(StructCmd));
}
OSMutexPost(App_MutexCmdToTaskMvt);
// Update last CmdID used
LastMainCmdId = App_CmdToTaskMvtId;
// Analyse Cmd
switch(CurrentCmd.Cmd)
{
case Mvt_Stop:
NextState = 254;
break;
case Mvt_UseAngleOnly:
case MvtSimple_RotateInDeg:
case MvtSimple_MoveInMM:
case MvtSimple_RotateToAngleInDeg:
case Mvt_UseDistOnly:
case Mvt_UseMixedMode:
case Mvt_UsePivotMode:
case App_SetNewPos:
case Mvt_UseSpline:
NextState = 253;
break;
default:
// Invalid Command
NextState = 3;
break;
}
}
else
{ // There is no msg available
NextState = 3;
}
break;
// CASE 003 ---------------------------------------------------------------------------
case 3: // Read Bumpers Status
// Read the current sensors status
SensorsCurrentStatus = StrategyCurrentFlag & CurrentActivatedSensors;
// One sensor has been activated
// Which sensors are active
if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_FRONT) != 0)
{ // Front sensors are activated
NextState = 5;
}
else if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT) != 0)
{ // Front Left sensors are activated
NextState = 14;
}
else if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT) != 0)
{ // Left sensors are activated
NextState = 15;
}
else if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_LEFT) != 0)
{ // Left sensors are activated
NextState = 8;
}
else if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT) != 0)
{ // Right sensors are activated
NextState = 7;
}
else if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_REAR) != 0)
{ // Back sensors are activated
NextState = 6;
}
else if((SensorsCurrentStatus & APP_PARAM_STRATEGYFLAG_COLLISION_LONG_REAR) != 0)
{ // Back sensors are activated with long detection value
NextState = 13;
}
else
{ // There is no sensor activated
NextState = 4;
}
break;
// CASE 004 ---------------------------------------------------------------------------
case 4: // Read Mvt Flag
// Read Current Pos
if(AppGetCurrentPos(&CurrentPos) == ERR__NO_ERROR)
{ // Check CurrentState Value
if(CurrentPos.CurrentState == CURRENT_STATE__STOP)
{
NextState = 9;
}
else
{
MvtTimeout = 0;
NextState = 1;
}
}
else
{ // We are unable to read current pos
NextState = 1;
}
break;
// CASE 005 ---------------------------------------------------------------------------
case 5: // Escape Seq (Front)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
CollisionTimeout = 0; // Clear the current timer
do
{
CollisionTimeout++;
OSTimeDlyHMSM(0, 0, 0, 500);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
}while(((StrategyCurrentFlag & APP_PARAM_STRATEGYFLAG_COLLISION_FRONT) == APP_PARAM_STRATEGYFLAG_COLLISION_FRONT) && (CollisionTimeout < APP_COLLISION_TIMEOUT));
if(CollisionTimeout >= APP_COLLISION_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
{
// Send again the expected position to TaskAsser
TaskMvt_SendSetpointToTaskAsser(&LastSetpointSent);
NextState = 9;
}
break;
// CASE 006 ---------------------------------------------------------------------------
case 6: // Escape Seq (Back)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
CollisionTimeout = 0; // Clear the current timer
do
{
CollisionTimeout++;
OSTimeDlyHMSM(0, 0, 0, 500);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
}while(((StrategyCurrentFlag & APP_PARAM_STRATEGYFLAG_COLLISION_REAR) == APP_PARAM_STRATEGYFLAG_COLLISION_REAR) && (CollisionTimeout < APP_COLLISION_TIMEOUT));
if(CollisionTimeout >= APP_COLLISION_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
{
// Send again the expected position to TaskAsser
TaskMvt_SendSetpointToTaskAsser(&LastSetpointSent);
NextState = 9;
}
break;
// CASE 007 ---------------------------------------------------------------------------
case 7: // Escape Seq (Right)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
CollisionTimeout = 0; // Clear the current timer
do
{
CollisionTimeout++;
OSTimeDlyHMSM(0, 0, 0, 500);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
}while(((StrategyCurrentFlag & APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT) == APP_PARAM_STRATEGYFLAG_COLLISION_RIGHT) && (CollisionTimeout < APP_COLLISION_TIMEOUT));
if(CollisionTimeout >= APP_COLLISION_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
{
// Send again the expected position to TaskAsser
TaskMvt_SendSetpointToTaskAsser(&LastSetpointSent);
NextState = 9;
}
LED_Off(3);
break;
// CASE 008 ---------------------------------------------------------------------------
case 8: // Escape Seq (Left)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
CollisionTimeout = 0; // Clear the current timer
do
{
CollisionTimeout++;
OSTimeDlyHMSM(0, 0, 0, 500);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
}while(((StrategyCurrentFlag & APP_PARAM_STRATEGYFLAG_COLLISION_LEFT) == APP_PARAM_STRATEGYFLAG_COLLISION_LEFT) && (CollisionTimeout < APP_COLLISION_TIMEOUT));
if(CollisionTimeout >= APP_COLLISION_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
{
// Send again the expected position to TaskAsser
TaskMvt_SendSetpointToTaskAsser(&LastSetpointSent);
NextState = 9;
}
break;
// CASE 009 ---------------------------------------------------------------------------
case 9: // Is temporary setpoint reached ?
// Check if we have a setpoint to reach
if((CurrentSetpoint >= 0) && (LibMoving_IsSetpointReached(CurrentPath + CurrentSetpoint) == OS_TRUE))
{
// Setpoint has been reached
NextState = 10;
}
else
{
if(CurrentSetpoint >= 0)
MvtTimeout++;
else
MvtTimeout = 0;
// Activate or Desactivate timeout action
if(APP_MVT_TIMEOUT_ENABLED == OS_FALSE)
MvtTimeout = 0;
if(MvtTimeout > APP_MVT_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
NextState = 1;
}
break;
// CASE 010 ---------------------------------------------------------------------------
case 10: // Is final setpoint reached ?
AppGetCurrentPos(&CurrentPos);
memcpy(&CurrentCmdForTest, &CurrentCmd, sizeof(StructCmd));
if(USE_CURRENT_VALUE == CurrentCmd.Param2)
CurrentCmdForTest.Param2 = CurrentPos.x;
if(USE_CURRENT_VALUE == CurrentCmd.Param3)
CurrentCmdForTest.Param3 = CurrentPos.y;
if(USE_CURRENT_VALUE == CurrentCmd.Param4)
CurrentCmdForTest.Param4 = CurrentPos.angle;
// Check if we have a setpoint to reach
if(LibMoving_IsSetpointReached(&CurrentCmdForTest) == OS_TRUE)
{
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
// Clear all escape flags
EscapeFrontFlag = 0;
EscapeRearFlag = 0;
NextState = 1;
}
else
{ // Search for next setpoint
NextState = 11;
}
break;
// CASE 011 ---------------------------------------------------------------------------
case 11: // Does next setpoint exist ?
if(CurrentSetpoint > 0)
{ // Load next Setpoint
NextState = 12;
}
else
{ // There is no other setpoint in memory, We have to compute a new path
NextState = 253;
}
break;
// CASE 012 ---------------------------------------------------------------------------
case 12: // Use next setpoint
CurrentSetpoint--;
if(CurrentSetpoint >= 0)
{
// Send new command to TaskAsser
TaskMvt_SendSetpointToTaskAsser(CurrentPath + CurrentSetpoint);
memcpy(&LastSetpointSent, CurrentPath + CurrentSetpoint, sizeof(StructCmd));
CurrentActivatedSensors = CurrentPath[CurrentSetpoint].ActiveSensorsFlag;
// Clear the timeout flag
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_CLR, &Err);
}
NextState = 1;
break;
// CASE 013 ---------------------------------------------------------------------------
case 13: // Rear sensor (Long detection)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
break;
// CASE 014 ---------------------------------------------------------------------------
case 14: // Escape Seq (Front Left)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
CollisionTimeout = 0; // Clear the current timer
do
{
CollisionTimeout++;
OSTimeDlyHMSM(0, 0, 0, 500);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
}while(((StrategyCurrentFlag & APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT) == APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_LEFT) && (CollisionTimeout < APP_COLLISION_TIMEOUT));
if(CollisionTimeout >= APP_COLLISION_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
{
// Send again the expected position to TaskAsser
TaskMvt_SendSetpointToTaskAsser(&LastSetpointSent);
NextState = 9;
}
break;
// CASE 015 ---------------------------------------------------------------------------
case 15: // Escape Seq (Front Right)
// Ask for stopping Mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
CollisionTimeout = 0; // Clear the current timer
do
{
CollisionTimeout++;
OSTimeDlyHMSM(0, 0, 0, 500);
StrategyCurrentFlag = OSFlagAccept(AppStrategyFlags, APP_PARAM_STRATEGYFLAG_ALL, OS_FLAG_WAIT_SET_ANY, &Err);
}while(((StrategyCurrentFlag & APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT) == APP_PARAM_STRATEGYFLAG_COLLISION_FRONT_RIGHT) && (CollisionTimeout < APP_COLLISION_TIMEOUT));
if(CollisionTimeout >= APP_COLLISION_TIMEOUT)
{
// Set flag to indicate the last action finished with a timeout
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_TIMEOUT, OS_FLAG_SET, &Err);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
}
else
{
// Send again the expected position to TaskAsser
TaskMvt_SendSetpointToTaskAsser(&LastSetpointSent);
NextState = 9;
}
break;
// CASE 253 ---------------------------------------------------------------------------
case 253: // Compute new traj
// First step, clear all current orders
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
// Compute New traj
LibMoving_ComputeNewPath(&CurrentCmd, CurrentPath, &CurrentSetpoint);
// Check if a path has been found ?
if(CurrentSetpoint > 0)
{
// A path has been found, Send new order
NextState = 12;
}
else
{
// There is no possible path, we do nothing
NextState = 254; // STOP action
}
break;
// CASE 254 ---------------------------------------------------------------------------
case 254: // Ask for stopping mvt
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
// Indicates we are stopped if current action is a blocking mvt
if(CmdType_Blocking == CurrentCmd.CmdType)
OSFlagPost(AppFlags, APP_PARAM_APPFLAG_ACTION_STATUS, OS_FLAG_SET, &Err);
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
memcpy(&CurrentCmd, &StopCmd, 1 * sizeof(StructCmd));
CurrentActivatedSensors = APP_PARAM_APPFLAG_NONE;
NextState = 1;
break;
// CASE 255 ---------------------------------------------------------------------------
case 255: // End
// Disable all current path
CurrentSetpoint = -1;
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
memset(&CurrentCmd, 0, 1 * sizeof(StructCmd));
TaskMvt_SendSetpointToTaskAsser(&StopCmd);
break;
// DEFAULT ----------------------------------------------------------------------------
default: // Undefined state
// Current state is undefined. Clear current path and ask for stopping mvt
memset(CurrentPath, 0, APP_MOVING_SEQ_LEN * sizeof(StructCmd));
NextState = 254; // Ask for Stopping action
break;
}
// We loop until timer is set
}while(CurrentState != 255);
// ===============================================================================================
#ifdef _TARGET_440H
Set_Line_Information( 1, 0, "XXX", 3);
#endif
// Time is up : Wait for ever
while(OS_TRUE)
{
OSTimeDlyHMSM(1, 0, 0, 0);
}
return;
}
// ------------------------------------------------------------------------------------------------
INT8U Strategy_GetNextAction(EnumColor CurrentColor, StructCmd *NextAction)
{
static INT16U CurrentActionID = 0;
INT16U Err = 0;
OS_FLAGS CurrentFlag = 0;
StructCmd *p = NextAction;
if(NULL == NextAction)
return ERR__INVALID_PARAM;
// Set all actions as blocking actions
NextAction->CmdType = CmdType_Blocking;
switch(CurrentColor)
{
case c_ColorA: // Red #############################################################
switch(CurrentActionID)
{
case 0: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 580.0; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 1: p->Cmd = MvtSimple_RotateToAngleInDeg; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param4 = 90.0; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; CurrentActionID++; break;
case 2: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = -367; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
case 3: p->Cmd = Mvt_UsePivotMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = RIGHT_WHEEL; p->Param4 = 180.0; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
case 4: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = -250; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
case 5: p->Cmd = Mvt_UseMixedMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 2500; p->Param3 = 1000; p->Param4 = -145; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 6: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 500; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 7: p->Cmd = Mvt_UseMixedMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 2260; p->Param3 = 500; p->Param4 = 90; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 8: p->Cmd = Mvt_UseMixedMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED * 0.5; p->Param2 = 2260; p->Param3 = 65; p->Param4 = 90; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
break;
case c_ColorB: // Purple ##############################################################
switch(CurrentActionID)
{
case 0: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 580.0; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 1: p->Cmd = MvtSimple_RotateToAngleInDeg; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param4 = 90.0; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_NONE; CurrentActionID++; break;
case 2: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = -367; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
case 3: p->Cmd = Mvt_UsePivotMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = LEFT_WHEEL; p->Param4 = 0; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
case 4: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = -250; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
case 5: p->Cmd = Mvt_UseMixedMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 500; p->Param3 = 1000; p->Param4 = -55; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 6: p->Cmd = MvtSimple_MoveInMM; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 500; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 7: p->Cmd = Mvt_UseMixedMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED; p->Param2 = 740; p->Param3 = 500; p->Param4 = 90; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_FRONT; CurrentActionID++; break;
case 8: p->Cmd = Mvt_UseMixedMode; p->CmdType = CmdType_Blocking; p->Param1 = APP_HOMOL_ROBOT_SPEED * 0.5; p->Param2 = 740; p->Param3 = 65; p->Param4 = 90; p->ActiveSensorsFlag = APP_PARAM_APPFLAG_SENSORS_BACK; CurrentActionID++; break;
default:
return ERR__NO_MORE_DATA_AVAILABLE;
break;
}
break;
default: // Not Set ##########################################################
break;
}
// Check for Wait command --------------------------------------------
if(App_Wait == p->Cmd)
{
// Execute the wait command
OSTimeDlyHMSM(p->Param1, p->Param2, p->Param3, p->Param4);
return Strategy_GetNextAction(CurrentColor, p);
}
// Check for conditionnal command ------------------------------------
if(App_IfGoto == p->Cmd)
{
// Read the current Flags
CurrentFlag = OSFlagAccept(AppFlags, APP_PARAM_APPFLAG_START_BUTTON, OS_FLAG_WAIT_SET_ANY, &Err);
if((CurrentFlag & (p->Param1)) != 0)
CurrentActionID = (int)(p->Param2);
else
CurrentActionID = (int)(p->Param3);
return Strategy_GetNextAction(CurrentColor, p);
}
// Create the MvtSimple Command --------------------------------------
if(MvtSimple_MoveInMM == p->Cmd)
LibMoving_MoveInMM(p->Param2, p->Param1, p);
if(MvtSimple_RotateInDeg == p->Cmd)
LibMoving_RotateInDeg(p->Param4, p->Param1, p);
if(MvtSimple_RotateToAngleInDeg == p->Cmd)
LibMoving_RotateToAngleInDeg(p->Param4, p->Param1, p);
// Angle Conversion --------------------------------------------------
if((Mvt_UsePivotMode == p->Cmd) || (Mvt_UseMixedMode == p->Cmd) || (Mvt_UseSpline == p->Cmd))
{
p->Param4 = AppConvertDegInRad(p->Param4);
}
return ERR__NO_ERROR;
}
