void TaskThreadProcess()
{
while (true)
{
switch (state)
{
case TaskBrasState.Init:
servoPositionsRequested = new Dictionary <ServoId, int>();
servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsInit.BrasGauche);
OnHerkulexPositionRequest(servoPositionsRequested);
state = TaskBrasState.Attente;
break;
case TaskBrasState.Attente:
//On ne sort pas de cet état sans un forçage extérieur vers un autree état
break;
case TaskBrasState.PrehensionGobelet:
servoPositionsRequested = new Dictionary <ServoId, int>();
OnPilotageVentouse((byte)PilotageVentouse.BrasGauche, 50);
servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsPrehensionGobelet.BrasGauche);
OnHerkulexPositionRequest(servoPositionsRequested);
state = TaskBrasState.PrehensionGobeletEnCours;
sw.Reset();
sw.Start();
break;
case TaskBrasState.PrehensionGobeletEnCours:
if (sw.ElapsedMilliseconds > 500)
{
if (ventouseBrasGaucheCurrent > 0.8) //mesuré 0.6A à vide et 1.35A en charge
{
sw.Stop();
sw.Reset();
state = TaskBrasState.StockageEnHauteur;
}
else
{
sw.Stop();
state = TaskBrasState.PrehensionGobeletEnCours;
}
}
break;
//case TaskBrasState.StockageSurSupport:
// servoPositionsRequested = new Dictionary<ServoId, int>();
// servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsStockageSurSupport.BrasGauche);
// OnHerkulexPositionRequest(servoPositionsRequested);
// Thread.Sleep(500);
// OnPilotageVentouse((byte)PilotageVentouse.BrasGauche, 0);
// Thread.Sleep(500);
// isSupportGaucheFull = true;
// state = TaskBrasState.PrehensionGobelet;
// break;
case TaskBrasState.StockageEnHauteur:
servoPositionsRequested = new Dictionary <ServoId, int>();
servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsStockageEnHauteur.BrasGauche);
OnHerkulexPositionRequest(servoPositionsRequested);
state = TaskBrasState.StockageEnHauteurEnCours;
sw.Reset();
sw.Start();
break;
case TaskBrasState.StockageEnHauteurEnCours:
if (sw.ElapsedMilliseconds > 500)
{
sw.Stop();
sw.Reset();
state = TaskBrasState.Finished;
}
break;
case TaskBrasState.Depose:
servoPositionsRequested = new Dictionary <ServoId, int>();
servoPositionsRequested.Add(ServoId.BrasGauche, (int)TaskBrasPositionsDepose.BrasGauche);
OnHerkulexPositionRequest(servoPositionsRequested);
state = TaskBrasState.Finished;
sw.Reset();
sw.Start();
break;
case TaskBrasState.DeposeEnCours:
if (sw.ElapsedMilliseconds > 500)
{
sw.Stop();
sw.Reset();
state = TaskBrasState.Finished;
}
break;
case TaskBrasState.Finished:
isFineshed = true;
break;
default:
break;
}
Thread.Sleep(100);
}
}
public void OnStartPrehension(object sender, BoolEventArgs e)
{
Init();
state = TaskBrasState.PrehensionGobelet;
}
public void OnStopPrehension(object sender, BoolEventArgs e)
{
state = TaskBrasState.Attente;
}
