private static bool TryUpdateFromSerialData(SensorData sensorData, byte[] data)
{
if (data.Length < 12)
return false;
if ((char)data[0] != ArduinoPrefix.IncomingData)
return false;
if (!ValidateChecksum(data))
return false;
var firstByte = data[1];
sensorData.BallInDribbler = (1 & firstByte) == 1;
sensorData.BeaconIrLeftInView = (2 & firstByte) == 2;
sensorData.BeaconIrRightInView = (4 & firstByte) == 4;
sensorData.IsPowered = (8 & firstByte) == 8;
sensorData.IrChannel = data[2];
sensorData.EstimatedGlobalX = GetShortFromBytes(data, 3);
sensorData.EstimatedGlobalY = GetShortFromBytes(data, 5);
sensorData.EstimatedGlobalDirection = GetShortFromBytes(data, 7);
sensorData.BeaconServoDirection = GetShortFromBytes(data, 9);
return true;
}
public MainLogicProcessor()
{
VisionData = new VisionData();
SensorData = new SensorData();
stateMachine = new StateMachine<State, Trigger>(State.LookingForBall);
stateMachine.Configure(State.LookingForBall)
.Permit(Trigger.CameraLockedOnBall, State.ClosingInOnBall)
.Permit(Trigger.BallCaught, State.FindingGoal)
.OnEntry(StopTimer);
stateMachine.Configure(State.ClosingInOnBall)
.Permit(Trigger.CameraLostBall, State.LookingForBall)
.Permit(Trigger.BallCaught, State.FindingGoal)
.Permit(Trigger.Timeout, State.LookingForBall)
.OnEntry(() => StartTimer(10000, Trigger.Timeout))
.OnExit(StopTimer);
stateMachine.Configure(State.FindingGoal)
.Permit(Trigger.CoilgunLaunched, State.LookingForBall)
.Permit(Trigger.BallLost, State.LookingForBall)
.Permit(Trigger.Timeout, State.LookingForBall)
.OnEntry(() => StartTimer(5000, Trigger.Timeout))
.OnExit(StopTimer);
}
public static SensorData GetSensorDataFromBytes(byte[] data)
{
var sensorData = new SensorData();
if (data.Length == 0)
return sensorData;
byte previous = 0;
int i;
for (i = 0; i < data.Length; i++)
{
byte current = data[i];
if (previous == '\r' && current == '\n')
break;
previous = current;
}
var bytes = data.Take(i - 1).ToArray();
if (!TryUpdateFromSerialData(sensorData, bytes))
return null;
return sensorData;
}
public MainLogicProcessor()
{
VisionData = new VisionData();
SensorData = new SensorData();
LogicState = new LogicState();
stateMachine = new StateMachine<State, Trigger>(State.Idle);
stateMachine.OnUnhandledTrigger((state, trigger) => { });
stateMachine.Configure(State.Idle)
.Permit(Trigger.PoweredUp, State.Starting)
.OnEntry(() => Commander.SetColors(Colors.Yellow));
stateMachine.Configure(State.Starting)
.Permit(Trigger.Finished, State.LookingForBall)
.Permit(Trigger.BallCaught, State.FindingGoal)
.OnEntry(() =>
{
stopwatchTime = stopwatch.ElapsedMilliseconds;
SoundClipPlayer.PlayIntro();
Commander.SetColors(Colors.Cyan);
});
stateMachine.Configure(State.LookingForBall)
.Permit(Trigger.CameraLockedOnBall, State.ClosingInOnBall)
.Permit(Trigger.BallCaught, State.FindingGoal)
.OnEntry(
() =>
{
Commander.SetColors(Colors.Blue);
stopwatchTime = stopwatch.ElapsedMilliseconds;
});
stateMachine.Configure(State.ClosingInOnBall)
.Permit(Trigger.CameraLostBall, State.LookingForBall)
.Permit(Trigger.BallCaught, State.FindingGoal)
.Permit(Trigger.Timeout, State.LookingForBall)
.OnEntry(() =>
{
StartTimer(10000, Trigger.Timeout);
Commander.SetColors(Colors.Red);
})
.OnExit(StopTimer);
stateMachine.Configure(State.FindingGoal)
.Permit(Trigger.CoilgunLaunched, State.LookingForBall)
.Permit(Trigger.BallLost, State.LookingForBall)
.Permit(Trigger.Timeout, State.LookingForBall)
.OnEntry(() =>
{
LogicState.FindingGoal = true;
StartTimer(5000, LaunchBall);
Commander.SetColors(Colors.Magenta);
})
.OnExit(
() =>
{
LogicState.FindingGoal = false;
StopTimer();
});
stopwatch.Start();
}
