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