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