void TaskThreadProcess() { while (true) { switch (state) { case TaskPhareStates.Init: state = TaskPhareStates.Attente; break; case TaskPhareStates.Attente: break; /**********************Deplacement vers phare**************************************/ case TaskPhareStates.DeplacementToPhare: if (parentManager.Team == Equipe.Bleue) { parentManager.robotDestination = new PointD(1.0, -0.90); parentManager.robotOrientation = -Math.PI / 2; } else if (parentManager.Team == Equipe.Jaune) { parentManager.robotDestination = new PointD(-1.0, -0.90); parentManager.robotOrientation = -Math.PI / 2; } state = TaskPhareStates.DeplacementToPhareAttente; StartSw(); break; case TaskPhareStates.DeplacementToPhareAttente: if (parentManager.isDeplacementFinished || sw.ElapsedMilliseconds > 5000) { state = TaskPhareStates.PrepareServo; StopSw(); } break; /**********************Préparation des servos**************************************/ case TaskPhareStates.PrepareServo: servoPositionsRequested = new Dictionary <ServoId, int>(); if (parentManager.Team == Equipe.Bleue) { servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsPush.BrasGauche); } else if (parentManager.Team == Equipe.Jaune) { servoPositionsRequested.Add(ServoId.BrasDroit, (int)TaskBrasPositionsPush.BrasDroit); } OnHerkulexPositionRequest(servoPositionsRequested); state = TaskPhareStates.PrepareServoAttente; StartSw(); break; case TaskPhareStates.PrepareServoAttente: if (sw.ElapsedMilliseconds > 500) { StopSw(); state = TaskPhareStates.ActivatePhare; } break; /**********************Deplacement activation phare**************************************/ case TaskPhareStates.ActivatePhare: if (parentManager.Team == Equipe.Bleue) { parentManager.robotDestination = new PointD(1.35, -0.90); parentManager.robotOrientation = -Math.PI / 2; } else if (parentManager.Team == Equipe.Jaune) { parentManager.robotDestination = new PointD(-1.35, -0.90); parentManager.robotOrientation = -Math.PI / 2; } state = TaskPhareStates.ActivatePhareAttente; StartSw(); break; case TaskPhareStates.ActivatePhareAttente: if (parentManager.isDeplacementFinished || sw.ElapsedMilliseconds > 5000) { state = TaskPhareStates.ReplyServo; StopSw(); } break; /**********************Reply servos**************************************/ case TaskPhareStates.ReplyServo: servoPositionsRequested = new Dictionary <ServoId, int>(); if (parentManager.Team == Equipe.Bleue) { servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsInit.BrasGauche); } else if (parentManager.Team == Equipe.Jaune) { servoPositionsRequested.Add(ServoId.BrasDroit, (int)TaskBrasPositionsInit.BrasDroit); } OnHerkulexPositionRequest(servoPositionsRequested); StartSw(); state = TaskPhareStates.ReplyServoAttente; break; case TaskPhareStates.ReplyServoAttente: if (sw.ElapsedMilliseconds > 500) { StopSw(); state = TaskPhareStates.Finished; } break; /**********************Finished task**************************************/ case TaskPhareStates.Finished: isFinished = true; state = TaskPhareStates.Attente; break; } Thread.Sleep(10); } }
public void Start() { state = TaskPhareStates.DeplacementToPhare; isFinished = false; StopSw(); }
public void Pause() { state = TaskPhareStates.Attente; isFinished = false; StopSw(); }
public void Init() { state = TaskPhareStates.Init; isFinished = false; StopSw(); }