Implement PLC integration for the power port.

2026-03-09 21:56:50 -04:00 · 2020-04-04 16:48:58 -07:00
parent 0ed6f771d5
commit 783791d42c
6 changed files with 192 additions and 8 deletions
--- 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) &&
--- 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
 }

--- 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
+}
--- a/game/power_port.go
+++ 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
+}
--- a/game/power_port_test.go
+++ 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)
+}
--- 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
 	}