void TaskThreadProcess() { while (true) { switch (state) { case TaskWindFlagStates.Init: state = TaskWindFlagStates.Attente; break; case TaskWindFlagStates.Attente: break; /**********************Deplacement vers drapeau droit**************************************/ case TaskWindFlagStates.DeplacementToFlag: if (parentManager.Team == Equipe.Bleue) { parentManager.robotDestination = new PointD(1.4, 0.90); parentManager.robotOrientation = Math.PI / 2; } else if (parentManager.Team == Equipe.Jaune) { parentManager.robotDestination = new PointD(-1.4, 0.90); parentManager.robotOrientation = Math.PI / 2; } state = TaskWindFlagStates.DeplacementToFlagAttente; StartSw(); break; case TaskWindFlagStates.DeplacementToFlagAttente: if (parentManager.isDeplacementFinished || sw.ElapsedMilliseconds > 5000) { state = TaskWindFlagStates.PrepareServo; StopSw(); } break; /**********************Préparation des servos**************************************/ case TaskWindFlagStates.PrepareServo: servoPositionsRequested = new Dictionary <ServoId, int>(); if (parentManager.Team == Equipe.Bleue) { servoPositionsRequested.Add(ServoId.BrasDroit, (int)TaskBrasPositionsPush.BrasDroit); } else if (parentManager.Team == Equipe.Jaune) { servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsPush.BrasGauche); } OnHerkulexPositionRequest(servoPositionsRequested); state = TaskWindFlagStates.PrepareServoAttente; StartSw(); break; case TaskWindFlagStates.PrepareServoAttente: if (sw.ElapsedMilliseconds > 500) { StopSw(); state = TaskWindFlagStates.PushFirstFlag; } break; /**********************Deplacement vers drapeau droit**************************************/ case TaskWindFlagStates.PushFirstFlag: if (parentManager.Team == Equipe.Bleue) { parentManager.robotDestination = new PointD(1.0, 0.87); parentManager.robotOrientation = Math.PI / 2; } else if (parentManager.Team == Equipe.Jaune) { parentManager.robotDestination = new PointD(-1.0, 0.87); parentManager.robotOrientation = Math.PI / 2; } state = TaskWindFlagStates.PushFirstFlagAttente; StartSw(); break; case TaskWindFlagStates.PushFirstFlagAttente: if (parentManager.isDeplacementFinished || sw.ElapsedMilliseconds > 5000) { state = TaskWindFlagStates.PrepareServo2; StopSw(); } break; /**********************Preparation des servo 2 eme flag**************************************/ case TaskWindFlagStates.PrepareServo2: servoPositionsRequested = new Dictionary <ServoId, int>(); if (parentManager.Team == Equipe.Bleue) { servoPositionsRequested.Add(ServoId.BrasDroit, (int)TaskBrasPositionsInit.BrasDroit); servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsPush.BrasGauche); } else if (parentManager.Team == Equipe.Jaune) { servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsInit.BrasGauche); servoPositionsRequested.Add(ServoId.BrasDroit, (int)TaskBrasPositionsPush.BrasDroit); } OnHerkulexPositionRequest(servoPositionsRequested); StartSw(); state = TaskWindFlagStates.PrepareServo2Attente; break; case TaskWindFlagStates.PrepareServo2Attente: if (sw.ElapsedMilliseconds > 500) { StopSw(); state = TaskWindFlagStates.PushSecondFlag; } break; /**********************Push flag 2**************************************/ case TaskWindFlagStates.PushSecondFlag: if (parentManager.Team == Equipe.Bleue) { parentManager.robotDestination = new PointD(0.75, 0.87); parentManager.robotOrientation = Math.PI / 2; } else if (parentManager.Team == Equipe.Jaune) { parentManager.robotDestination = new PointD(-0.75, 0.87); parentManager.robotOrientation = Math.PI / 2; } state = TaskWindFlagStates.PushSecondFlagAttente; StartSw(); break; case TaskWindFlagStates.PushSecondFlagAttente: if (parentManager.isDeplacementFinished || sw.ElapsedMilliseconds > 5000) { state = TaskWindFlagStates.ReplyServo; StopSw(); } break; /**********************Reply servos**************************************/ case TaskWindFlagStates.ReplyServo: servoPositionsRequested = new Dictionary <ServoId, int>(); servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsInit.BrasGauche); servoPositionsRequested.Add(ServoId.BrasDroit, (int)TaskBrasPositionsInit.BrasDroit); servoPositionsRequested.Add(ServoId.BrasCentral, (int)TaskBrasPositionsInit.BrasCentral); OnHerkulexPositionRequest(servoPositionsRequested); StartSw(); state = TaskWindFlagStates.ReplyServoAttente; break; case TaskWindFlagStates.ReplyServoAttente: if (sw.ElapsedMilliseconds > 500) { StopSw(); state = TaskWindFlagStates.Finished; } break; /**********************Finished task**************************************/ case TaskWindFlagStates.Finished: isFinished = true; state = TaskWindFlagStates.Attente; break; } Thread.Sleep(10); } }
public void Start() { state = TaskWindFlagStates.DeplacementToFlag; isFinished = false; StopSw(); }
public void Pause() { state = TaskWindFlagStates.Attente; isFinished = false; StopSw(); }
public void Init() { state = TaskWindFlagStates.Init; isFinished = false; StopSw(); }