/*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; }