public bool Update(ref Controller logitechController, ref SerialCommsHandler NUC_SerialConnection)
{
switch (orientState)
{
case OrientState.INIT:
orientState = OrientState.LOCATING_VISION_TARGETS;
turningTime = stopwatch.DurationMs;
break;
case OrientState.LOCATING_VISION_TARGETS:
if (LocateVisionTargets(ref logitechController, ref NUC_SerialConnection))
{
orientState = OrientState.ORIENTING_TOWARDS_MINING_ZONE;
}
else if (stopwatch.DurationMs - turningTime > TURNING_TIMOUT)
{
orientState = OrientState.UNBURYING;
unburyingTime = stopwatch.DurationMs;
}
break;
case OrientState.UNBURYING:
if (Unburying(ref logitechController, ref NUC_SerialConnection))
{
turningTime = stopwatch.DurationMs;
orientState = OrientState.LOCATING_VISION_TARGETS;
}
break;
case OrientState.ORIENTING_TOWARDS_MINING_ZONE:
logitechController.ResetValues();
Debug.Print("DONE");
break;
default:
break;
}
return(false);
}
public Orient()
{
orientState = OrientState.INIT;
stopwatch = new CTRE.Phoenix.Stopwatch();
stopwatch.Start();
}
