diff --git a/field/arena.go b/field/arena.go index 36084b2..d0ae2a9 100644 --- a/field/arena.go +++ b/field/arena.go @@ -796,6 +796,11 @@ func (arena *Arena) handlePlcInput() { blueControlPanel.UpdateState(blueSegmentCount, blueScore.StageAtCapacity(game.Stage2, teleopStarted), blueScore.StageAtCapacity(game.Stage3, teleopStarted), currentTime) blueScore.ControlPanelStatus = blueControlPanel.ControlPanelStatus + + // Handle shield generator rungs. + if game.ShouldAssessRung(matchStartTime, currentTime) { + redScore.RungIsLevel, blueScore.RungIsLevel = arena.Plc.GetRungs() + } } // Check if either alliance has reached Stage 3 capacity. diff --git a/game/match_timing.go b/game/match_timing.go index bb603ec..f9284ae 100644 --- a/game/match_timing.go +++ b/game/match_timing.go @@ -7,6 +7,12 @@ package game import "time" +const ( + powerPortAutoGracePeriodSec = 5 + powerPortTeleopGracePeriodSec = 5 + rungAssessmentDelaySec = 5 +) + var MatchTiming = struct { WarmupDurationSec int AutoDurationSec int @@ -25,7 +31,18 @@ func GetDurationToTeleopStart() time.Duration { time.Second } +func GetDurationToWarning() time.Duration { + return time.Duration(MatchTiming.WarmupDurationSec+MatchTiming.AutoDurationSec+MatchTiming.PauseDurationSec+ + MatchTiming.TeleopDurationSec-MatchTiming.WarningRemainingDurationSec) * time.Second +} + func GetDurationToTeleopEnd() time.Duration { return time.Duration(MatchTiming.WarmupDurationSec+MatchTiming.AutoDurationSec+MatchTiming.PauseDurationSec+ MatchTiming.TeleopDurationSec) * time.Second } + +// Returns true if the given time is within the proper range for assessing the level state of the shield generator rung. +func ShouldAssessRung(matchStartTime, currentTime time.Time) bool { + return currentTime.After(matchStartTime.Add(GetDurationToWarning())) && + currentTime.Before(matchStartTime.Add(GetDurationToTeleopEnd()+rungAssessmentDelaySec*time.Second)) +} diff --git a/game/match_timing_test.go b/game/match_timing_test.go new file mode 100644 index 0000000..aa14f0c --- /dev/null +++ b/game/match_timing_test.go @@ -0,0 +1,18 @@ +// Copyright 2020 Team 254. All Rights Reserved. +// Author: pat@patfairbank.com (Patrick Fairbank) + +package game + +import ( + "github.com/stretchr/testify/assert" + "testing" +) + +func TestShouldAssessRung(t *testing.T) { + assert.Equal(t, false, ShouldAssessRung(matchStartTime, timeAfterStart(0))) + assert.Equal(t, false, ShouldAssessRung(matchStartTime, timeAfterStart(121.9))) + assert.Equal(t, true, ShouldAssessRung(matchStartTime, timeAfterStart(122.1))) + assert.Equal(t, true, ShouldAssessRung(matchStartTime, timeAfterStart(152.1))) + assert.Equal(t, true, ShouldAssessRung(matchStartTime, timeAfterStart(156.9))) + assert.Equal(t, false, ShouldAssessRung(matchStartTime, timeAfterStart(157.1))) +} diff --git a/game/power_port.go b/game/power_port.go index a32ab09..9e89282 100644 --- a/game/power_port.go +++ b/game/power_port.go @@ -9,11 +9,6 @@ import ( "time" ) -const ( - powerPortAutoGracePeriodSec = 5 - powerPortTeleopGracePeriodSec = 5 -) - type PowerPort struct { AutoCellsBottom [2]int AutoCellsOuter [2]int diff --git a/plc/plc.go b/plc/plc.go index 399fac0..7ed2c0b 100644 --- a/plc/plc.go +++ b/plc/plc.go @@ -230,6 +230,11 @@ func (plc *Plc) GetControlPanels() (game.ControlPanelColor, int, game.ControlPan game.ControlPanelColor(plc.registers[blueControlPanelColor]), int(plc.registers[blueControlPanelSegments]) } +// Returns whether each of the red and blue rungs is level. +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. func (plc *Plc) SetStackLights(red, blue, orange, green bool) { plc.coils[stackLightRed] = red