From 3739cd8690f3be0742b64d02c78f290b46969248 Mon Sep 17 00:00:00 2001 From: Patrick Fairbank Date: Sat, 4 Apr 2020 22:48:20 -0700 Subject: [PATCH] Implement PLC integration for all outputs. --- field/arena.go | 54 ++++++++++++++++++++++++++++++++++++++------ game/match_timing.go | 2 +- game/power_port.go | 2 +- plc/plc.go | 37 ++++++++++++++++++++++++++++-- 4 files changed, 84 insertions(+), 11 deletions(-) diff --git a/field/arena.go b/field/arena.go index d0ae2a9..b5fc54a 100644 --- a/field/arena.go +++ b/field/arena.go @@ -766,7 +766,7 @@ func (arena *Arena) handlePlcInput() { if arena.Plc.IsEnabled() { // Handle power ports. redPortCells, bluePortCells := arena.Plc.GetPowerPorts() - redPowerPort := arena.RedRealtimeScore.powerPort + redPowerPort := &arena.RedRealtimeScore.powerPort redPowerPort.UpdateState(redPortCells, redScore.CellCountingStage(teleopStarted), matchStartTime, currentTime) redScore.AutoCellsBottom = redPowerPort.AutoCellsBottom redScore.AutoCellsOuter = redPowerPort.AutoCellsOuter @@ -774,7 +774,7 @@ func (arena *Arena) handlePlcInput() { redScore.TeleopCellsBottom = redPowerPort.TeleopCellsBottom redScore.TeleopCellsOuter = redPowerPort.TeleopCellsOuter redScore.TeleopCellsInner = redPowerPort.TeleopCellsInner - bluePowerPort := arena.BlueRealtimeScore.powerPort + bluePowerPort := &arena.BlueRealtimeScore.powerPort bluePowerPort.UpdateState(bluePortCells, blueScore.CellCountingStage(teleopStarted), matchStartTime, currentTime) blueScore.AutoCellsBottom = bluePowerPort.AutoCellsBottom @@ -786,12 +786,12 @@ func (arena *Arena) handlePlcInput() { // Handle control panel. redColor, redSegmentCount, blueColor, blueSegmentCount := arena.Plc.GetControlPanels() - redControlPanel := arena.RedRealtimeScore.ControlPanel + redControlPanel := &arena.RedRealtimeScore.ControlPanel redControlPanel.CurrentColor = redColor redControlPanel.UpdateState(redSegmentCount, redScore.StageAtCapacity(game.Stage2, teleopStarted), redScore.StageAtCapacity(game.Stage3, teleopStarted), currentTime) redScore.ControlPanelStatus = redControlPanel.ControlPanelStatus - blueControlPanel := arena.BlueRealtimeScore.ControlPanel + blueControlPanel := &arena.BlueRealtimeScore.ControlPanel blueControlPanel.CurrentColor = blueColor blueControlPanel.UpdateState(blueSegmentCount, blueScore.StageAtCapacity(game.Stage2, teleopStarted), blueScore.StageAtCapacity(game.Stage3, teleopStarted), currentTime) @@ -820,7 +820,13 @@ func (arena *Arena) handlePlcInput() { } } +// Updates the PLC's coils based on its inputs and the current scoring state. func (arena *Arena) handlePlcOutput() { + matchStartTime := arena.MatchStartTime + currentTime := time.Now() + redScore := &arena.RedRealtimeScore.CurrentScore + blueScore := &arena.BlueRealtimeScore.CurrentScore + switch arena.MatchState { case PreMatch: if arena.lastMatchState != PreMatch { @@ -849,14 +855,48 @@ func (arena *Arena) handlePlcOutput() { scoreReady := arena.RedRealtimeScore.FoulsCommitted && arena.BlueRealtimeScore.FoulsCommitted && arena.alliancePostMatchScoreReady("red") && arena.alliancePostMatchScoreReady("blue") arena.Plc.SetStackLights(false, false, !scoreReady, false) + + if arena.lastMatchState != PostMatch { + go func() { + time.Sleep(time.Second * game.PowerPortTeleopGracePeriodSec) + arena.Plc.SetPowerPortMotors(false) + }() + } + arena.Plc.SetStageActivatedLights([3]bool{false, false, false}, [3]bool{false, false, false}) + arena.Plc.SetControlPanelLights(false, false) case AutoPeriod: - arena.Plc.SetStackLights(false, false, false, true) + arena.Plc.SetPowerPortMotors(true) fallthrough case PausePeriod: + fallthrough case TeleopPeriod: - arena.Plc.SetStackLights(false, false, false, true) - if arena.lastMatchState != TeleopPeriod { + arena.Plc.SetStageActivatedLights(arena.RedScoreSummary().StagesActivated, + arena.BlueScoreSummary().StagesActivated) + + controlPanelLightState := func(state game.ControlPanelLightState) bool { + switch state { + case game.ControlPanelLightOn: + return true + case game.ControlPanelLightFlashing: + return arena.Plc.GetCycleState(2, 0, 2) + default: + return false + } } + arena.Plc.SetControlPanelLights( + controlPanelLightState(arena.RedRealtimeScore.ControlPanel.ControlPanelLightState), + controlPanelLightState(arena.BlueRealtimeScore.ControlPanel.ControlPanelLightState)) + + // If the PLC reports a ball jam, blink the orange light and the power port color. + redJam, blueJam := arena.Plc.GetPowerPortJams() + blink := arena.Plc.GetCycleState(2, 0, 2) + arena.Plc.SetStackLights(redJam && blink, blueJam && blink, (redJam || blueJam) && !blink, true) + } + + if game.ShouldAssessRung(matchStartTime, currentTime) { + arena.Plc.SetShieldGeneratorLights(redScore.RungIsLevel, blueScore.RungIsLevel) + } else { + arena.Plc.SetShieldGeneratorLights(false, false) } } diff --git a/game/match_timing.go b/game/match_timing.go index f9284ae..1d8529e 100644 --- a/game/match_timing.go +++ b/game/match_timing.go @@ -9,7 +9,7 @@ import "time" const ( powerPortAutoGracePeriodSec = 5 - powerPortTeleopGracePeriodSec = 5 + PowerPortTeleopGracePeriodSec = 5 rungAssessmentDelaySec = 5 ) diff --git a/game/power_port.go b/game/power_port.go index 9e89282..e8ce43d 100644 --- a/game/power_port.go +++ b/game/power_port.go @@ -23,7 +23,7 @@ type PowerPort struct { func (powerPort *PowerPort) UpdateState(portCells [3]int, stage Stage, matchStartTime, currentTime time.Time) { autoValidityDuration := GetDurationToAutoEnd() + powerPortAutoGracePeriodSec*time.Second autoValidityCutoff := matchStartTime.Add(autoValidityDuration) - teleopValidityDuration := GetDurationToTeleopEnd() + powerPortTeleopGracePeriodSec*time.Second + teleopValidityDuration := GetDurationToTeleopEnd() + PowerPortTeleopGracePeriodSec*time.Second teleopValidityCutoff := matchStartTime.Add(teleopValidityDuration) newBottomCells := portCells[0] - totalPortCells(powerPort.AutoCellsBottom, powerPort.TeleopCellsBottom) diff --git a/plc/plc.go b/plc/plc.go index 7ed2c0b..fa9ad71 100644 --- a/plc/plc.go +++ b/plc/plc.go @@ -224,6 +224,11 @@ func (plc *Plc) GetPowerPorts() ([3]int, [3]int) { } } +// Returns whether each of the red and blue power ports are jammed. +func (plc *Plc) GetPowerPortJams() (bool, bool) { + return plc.inputs[redPowerPortJam], plc.inputs[bluePowerPortJam] +} + // Returns the current color and number of segment transitions for each of the red and blue control panels. func (plc *Plc) GetControlPanels() (game.ControlPanelColor, int, game.ControlPanelColor, int) { return game.ControlPanelColor(plc.registers[redControlPanelColor]), int(plc.registers[redControlPanelSegments]), @@ -235,7 +240,7 @@ func (plc *Plc) GetRungs() (bool, bool) { return plc.inputs[redRungIsLevel], plc.inputs[blueRungIsLevel] } -// Set the on/off state of the stack lights on the scoring table. +// Sets the on/off state of the stack lights on the scoring table. func (plc *Plc) SetStackLights(red, blue, orange, green bool) { plc.coils[stackLightRed] = red plc.coils[stackLightBlue] = blue @@ -243,15 +248,43 @@ func (plc *Plc) SetStackLights(red, blue, orange, green bool) { plc.coils[stackLightGreen] = green } -// Set the on/off state of the stack lights on the scoring table. +// Triggers the "match ready" chime if the state is true. func (plc *Plc) SetStackBuzzer(state bool) { plc.coils[stackLightBuzzer] = state } +// Sets the on/off state of the field reset light. func (plc *Plc) SetFieldResetLight(state bool) { plc.coils[fieldResetLight] = state } +// Sets the on/off state of the agitator motors within each power port. +func (plc *Plc) SetPowerPortMotors(state bool) { + plc.coils[powerPortMotors] = state +} + +// Sets the on/off state of the lights mounted within the shield generator trussing. +func (plc *Plc) SetStageActivatedLights(red, blue [3]bool) { + plc.coils[redStage1Light] = red[0] + plc.coils[redStage2Light] = red[1] + plc.coils[redStage3Light] = red[2] + plc.coils[blueStage1Light] = blue[0] + plc.coils[blueStage2Light] = blue[1] + plc.coils[blueStage3Light] = blue[2] +} + +// Sets the on/off state of the red and blue alliance stack lights mounted to the control panel. +func (plc *Plc) SetControlPanelLights(red, blue bool) { + plc.coils[redControlPanelLight] = red + plc.coils[blueControlPanelLight] = blue +} + +// Sets the on/off state of the red and blue alliance stack lights mounted to the top of the shield generator. +func (plc *Plc) SetShieldGeneratorLights(red, blue bool) { + plc.coils[redTrussLight] = red + plc.coils[blueTrussLight] = blue +} + func (plc *Plc) GetCycleState(max, index, duration int) bool { return plc.cycleCounter/duration%max == index }