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();
}
