diff --git a/field/arena.go b/field/arena.go index c1db41d..8250a44 100644 --- a/field/arena.go +++ b/field/arena.go @@ -416,7 +416,7 @@ func (arena *Arena) Update() { case AutoPeriod: auto = true enabled = true - if matchTimeSec >= float64(game.MatchTiming.WarmupDurationSec+game.MatchTiming.AutoDurationSec) { + if matchTimeSec >= game.GetDurationToAutoEnd().Seconds() { auto = false sendDsPacket = true if game.MatchTiming.PauseDurationSec > 0 { @@ -430,8 +430,7 @@ func (arena *Arena) Update() { case PausePeriod: auto = false enabled = false - if matchTimeSec >= float64(game.MatchTiming.WarmupDurationSec+game.MatchTiming.AutoDurationSec+ - game.MatchTiming.PauseDurationSec) { + if matchTimeSec >= game.GetDurationToTeleopStart().Seconds() { arena.MatchState = TeleopPeriod auto = false enabled = true @@ -443,8 +442,7 @@ func (arena *Arena) Update() { case TeleopPeriod: auto = false enabled = true - if matchTimeSec >= float64(game.MatchTiming.WarmupDurationSec+game.MatchTiming.AutoDurationSec+ - game.MatchTiming.PauseDurationSec+game.MatchTiming.TeleopDurationSec) { + if matchTimeSec >= game.GetDurationToTeleopEnd().Seconds() { arena.MatchState = PostMatch auto = false enabled = false @@ -761,7 +759,33 @@ func (arena *Arena) handlePlcInput() { oldRedScore := *redScore blueScore := &arena.BlueRealtimeScore.CurrentScore oldBlueScore := *blueScore + matchStartTime := arena.MatchStartTime + currentTime := time.Now() + if arena.Plc.IsEnabled() { + // Handle power ports. + redPortCells, bluePortCells := arena.Plc.GetPowerPortCells() + redPowerPort := arena.RedRealtimeScore.powerPort + redPowerPort.UpdateState(redPortCells, redScore.CellCountingStage(arena.MatchState >= TeleopPeriod), + matchStartTime, currentTime) + redScore.AutoCellsBottom = redPowerPort.AutoCellsBottom + redScore.AutoCellsOuter = redPowerPort.AutoCellsOuter + redScore.AutoCellsInner = redPowerPort.AutoCellsInner + redScore.TeleopCellsBottom = redPowerPort.TeleopCellsBottom + redScore.TeleopCellsOuter = redPowerPort.TeleopCellsOuter + redScore.TeleopCellsInner = redPowerPort.TeleopCellsInner + bluePowerPort := arena.BlueRealtimeScore.powerPort + bluePowerPort.UpdateState(bluePortCells, blueScore.CellCountingStage(arena.MatchState >= TeleopPeriod), + matchStartTime, currentTime) + blueScore.AutoCellsBottom = bluePowerPort.AutoCellsBottom + blueScore.AutoCellsOuter = bluePowerPort.AutoCellsOuter + blueScore.AutoCellsInner = bluePowerPort.AutoCellsInner + blueScore.TeleopCellsBottom = bluePowerPort.TeleopCellsBottom + blueScore.TeleopCellsOuter = bluePowerPort.TeleopCellsOuter + blueScore.TeleopCellsInner = bluePowerPort.TeleopCellsInner + } + + // Check if either alliance has reached Stage 3 capacity. if redScore.StageAtCapacity(game.Stage3, arena.MatchState >= TeleopPeriod) && redScore.Stage3TargetColor == game.ColorUnknown || blueScore.StageAtCapacity(game.Stage3, arena.MatchState >= TeleopPeriod) && diff --git a/field/realtime_score.go b/field/realtime_score.go index 9cf7df1..6c294df 100644 --- a/field/realtime_score.go +++ b/field/realtime_score.go @@ -11,6 +11,7 @@ type RealtimeScore struct { CurrentScore game.Score Cards map[string]string FoulsCommitted bool + powerPort game.PowerPort ControlPanel game.ControlPanel } diff --git a/game/match_timing.go b/game/match_timing.go index 370adce..bb603ec 100644 --- a/game/match_timing.go +++ b/game/match_timing.go @@ -5,6 +5,8 @@ package game +import "time" + var MatchTiming = struct { WarmupDurationSec int AutoDurationSec int @@ -13,3 +15,17 @@ var MatchTiming = struct { WarningRemainingDurationSec int TimeoutDurationSec int }{0, 15, 2, 135, 30, 0} + +func GetDurationToAutoEnd() time.Duration { + return time.Duration(MatchTiming.WarmupDurationSec+MatchTiming.AutoDurationSec) * time.Second +} + +func GetDurationToTeleopStart() time.Duration { + return time.Duration(MatchTiming.WarmupDurationSec+MatchTiming.AutoDurationSec+MatchTiming.PauseDurationSec) * + time.Second +} + +func GetDurationToTeleopEnd() time.Duration { + return time.Duration(MatchTiming.WarmupDurationSec+MatchTiming.AutoDurationSec+MatchTiming.PauseDurationSec+ + MatchTiming.TeleopDurationSec) * time.Second +} diff --git a/game/power_port.go b/game/power_port.go new file mode 100644 index 0000000..a32ab09 --- /dev/null +++ b/game/power_port.go @@ -0,0 +1,59 @@ +// Copyright 2020 Team 254. All Rights Reserved. +// Author: pat@patfairbank.com (Patrick Fairbank) +// +// Scoring logic for the 2020 Power Port element. + +package game + +import ( + "time" +) + +const ( + powerPortAutoGracePeriodSec = 5 + powerPortTeleopGracePeriodSec = 5 +) + +type PowerPort struct { + AutoCellsBottom [2]int + AutoCellsOuter [2]int + AutoCellsInner [2]int + TeleopCellsBottom [4]int + TeleopCellsOuter [4]int + TeleopCellsInner [4]int +} + +// Updates the internal counting state of the power port given the current state of the hardware counts. Allows the +// score to accumulate before the match, since the counters will be reset in hardware. +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 + teleopValidityCutoff := matchStartTime.Add(teleopValidityDuration) + + newBottomCells := portCells[0] - totalPortCells(powerPort.AutoCellsBottom, powerPort.TeleopCellsBottom) + newOuterCells := portCells[1] - totalPortCells(powerPort.AutoCellsOuter, powerPort.TeleopCellsOuter) + newInnerCells := portCells[2] - totalPortCells(powerPort.AutoCellsInner, powerPort.TeleopCellsInner) + + if currentTime.Before(autoValidityCutoff) && stage <= Stage2 { + powerPort.AutoCellsBottom[stage] += newBottomCells + powerPort.AutoCellsOuter[stage] += newOuterCells + powerPort.AutoCellsInner[stage] += newInnerCells + } else if currentTime.Before(teleopValidityCutoff) { + powerPort.TeleopCellsBottom[stage] += newBottomCells + powerPort.TeleopCellsOuter[stage] += newOuterCells + powerPort.TeleopCellsInner[stage] += newInnerCells + } +} + +// Returns the total number of cells scored across all stages in a port level. +func totalPortCells(autoCells [2]int, teleopCells [4]int) int { + var total int + for _, stageCount := range autoCells { + total += stageCount + } + for _, stageCount := range teleopCells { + total += stageCount + } + return total +} diff --git a/game/power_port_test.go b/game/power_port_test.go new file mode 100644 index 0000000..414f8ee --- /dev/null +++ b/game/power_port_test.go @@ -0,0 +1,70 @@ +// Copyright 2020 Team 254. All Rights Reserved. +// Author: pat@patfairbank.com (Patrick Fairbank) + +package game + +import ( + "github.com/stretchr/testify/assert" + "testing" + "time" +) + +var matchStartTime = time.Unix(10, 0) + +func TestPowerPort(t *testing.T) { + var powerPort PowerPort + assertPowerPort(t, [3][2]int{}, [3][4]int{}, &powerPort) + + // Check before match start and during the autonomous period. + powerPort.UpdateState([3]int{0, 1, 2}, Stage1, matchStartTime, timeAfterStart(-1)) + assertPowerPort(t, [3][2]int{{0, 0}, {1, 0}, {2, 0}}, [3][4]int{}, &powerPort) + powerPort.UpdateState([3]int{0, 0, 0}, Stage1, matchStartTime, timeAfterStart(1)) + assertPowerPort(t, [3][2]int{{0, 0}, {0, 0}, {0, 0}}, [3][4]int{}, &powerPort) + powerPort.UpdateState([3]int{0, 1, 2}, Stage1, matchStartTime, timeAfterStart(2)) + assertPowerPort(t, [3][2]int{{0, 0}, {1, 0}, {2, 0}}, [3][4]int{}, &powerPort) + powerPort.UpdateState([3]int{3, 5, 2}, Stage1, matchStartTime, timeAfterStart(5)) + assertPowerPort(t, [3][2]int{{3, 0}, {5, 0}, {2, 0}}, [3][4]int{}, &powerPort) + + // Check boundary conditions around the auto end grace period. + powerPort.UpdateState([3]int{4, 6, 3}, Stage1, matchStartTime, timeAfterStart(16.9)) + assertPowerPort(t, [3][2]int{{4, 0}, {6, 0}, {3, 0}}, [3][4]int{}, &powerPort) + powerPort.UpdateState([3]int{5, 8, 6}, Stage2, matchStartTime, timeAfterStart(17.1)) + assertPowerPort(t, [3][2]int{{4, 1}, {6, 2}, {3, 3}}, [3][4]int{}, &powerPort) + powerPort.UpdateState([3]int{8, 10, 7}, Stage2, matchStartTime, timeAfterStart(19.9)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{}, &powerPort) + powerPort.UpdateState([3]int{8, 10, 8}, Stage2, matchStartTime, timeAfterStart(20.1)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 1, 0, 0}}, + &powerPort) + + // Check during the teleoperated period. + powerPort.UpdateState([3]int{9, 10, 8}, Stage1, matchStartTime, timeAfterStart(30)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 1, 0, 0}}, + &powerPort) + powerPort.UpdateState([3]int{10, 12, 11}, Stage3, matchStartTime, timeAfterStart(30)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{{1, 0, 1, 0}, {0, 0, 2, 0}, {0, 1, 3, 0}}, + &powerPort) + powerPort.UpdateState([3]int{40, 32, 21}, StageExtra, matchStartTime, timeAfterStart(60)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{{1, 0, 1, 30}, {0, 0, 2, 20}, {0, 1, 3, 10}}, + &powerPort) + + // Check boundary conditions around the teleop end grace period. + powerPort.UpdateState([3]int{41, 32, 21}, StageExtra, matchStartTime, timeAfterStart(156.9)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{{1, 0, 1, 31}, {0, 0, 2, 20}, {0, 1, 3, 10}}, + &powerPort) + powerPort.UpdateState([3]int{42, 33, 22}, StageExtra, matchStartTime, timeAfterStart(157.1)) + assertPowerPort(t, [3][2]int{{4, 4}, {6, 4}, {3, 4}}, [3][4]int{{1, 0, 1, 31}, {0, 0, 2, 20}, {0, 1, 3, 10}}, + &powerPort) +} + +func assertPowerPort(t *testing.T, expectedAutoCells [3][2]int, expectedTeleopCells [3][4]int, powerPort *PowerPort) { + assert.Equal(t, expectedAutoCells[0], powerPort.AutoCellsBottom) + assert.Equal(t, expectedAutoCells[1], powerPort.AutoCellsOuter) + assert.Equal(t, expectedAutoCells[2], powerPort.AutoCellsInner) + assert.Equal(t, expectedTeleopCells[0], powerPort.TeleopCellsBottom) + assert.Equal(t, expectedTeleopCells[1], powerPort.TeleopCellsOuter) + assert.Equal(t, expectedTeleopCells[2], powerPort.TeleopCellsInner) +} + +func timeAfterStart(sec float32) time.Time { + return matchStartTime.Add(time.Duration(1000*sec) * time.Millisecond) +} diff --git a/plc/plc.go b/plc/plc.go index 0dc5ff7..580f7f3 100644 --- a/plc/plc.go +++ b/plc/plc.go @@ -152,7 +152,7 @@ func (plc *Plc) Run() { isHealthy := true isHealthy = isHealthy && plc.writeCoils() isHealthy = isHealthy && plc.readInputs() - isHealthy = isHealthy && plc.readCounters() + isHealthy = isHealthy && plc.readRegisters() if !isHealthy { plc.resetConnection() } @@ -209,6 +209,20 @@ func (plc *Plc) GetEthernetConnected() ([3]bool, [3]bool) { } } +// Returns the total number of power cells scored since match start in each level of the red and blue power ports. +func (plc *Plc) GetPowerPortCells() ([3]int, [3]int) { + return [3]int{ + int(plc.registers[redPowerPortBottom]), + int(plc.registers[redPowerPortOuter]), + int(plc.registers[redPowerPortInner]), + }, + [3]int{ + int(plc.registers[bluePowerPortBottom]), + int(plc.registers[bluePowerPortOuter]), + int(plc.registers[bluePowerPortInner]), + } +} + // 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 @@ -297,7 +311,7 @@ func (plc *Plc) readInputs() bool { return true } -func (plc *Plc) readCounters() bool { +func (plc *Plc) readRegisters() bool { if len(plc.registers) == 0 { return true } @@ -308,7 +322,7 @@ func (plc *Plc) readCounters() bool { return false } if len(registers)/2 < len(plc.registers) { - log.Printf("Insufficient length of PLC counters: got %d bytes, expected %d words.", len(registers), + log.Printf("Insufficient length of PLC registers: got %d bytes, expected %d words.", len(registers), len(plc.registers)) return false }