Send out game data when Stage 3 capacity is reached.

field/arena.go

@@ -70,6 +70,8 @@ type Arena struct {
 	MuteMatchSounds            bool
 	matchAborted               bool
 	soundsPlayed               map[*game.MatchSound]struct{}
+	RedControlPanel            *game.ControlPanel
+	BlueControlPanel           *game.ControlPanel
 }
 
 type AllianceStation struct {
@@ -206,10 +208,13 @@ func (arena *Arena) LoadMatch(match *model.Match) error {
 	arena.FieldVolunteers = false
 	arena.FieldReset = false
 	arena.ScoringPanelRegistry.resetScoreCommitted()
+	arena.RedControlPanel = new(game.ControlPanel)
+	arena.BlueControlPanel = new(game.ControlPanel)
 
 	// Notify any listeners about the new match.
 	arena.MatchLoadNotifier.Notify()
 	arena.RealtimeScoreNotifier.Notify()
+	arena.ControlPanelColorNotifier.Notify()
 	arena.AllianceStationDisplayMode = "match"
 	arena.AllianceStationDisplayModeNotifier.Notify()
 
@@ -686,6 +691,23 @@ func (arena *Arena) sendDsPacket(auto bool, enabled bool) {
 	arena.lastDsPacketTime = time.Now()
 }
 
+// Sends a game data packet encoded with the given Control Panel target color to the given stations.
+func (arena *Arena) sendGameDataPacket(color game.ControlPanelColor, stations ...string) {
+	gameData := game.GetGameDataForColor(color)
+	log.Printf("Sending game data packet '%s' to stations %v", gameData, stations)
+	for _, station := range stations {
+		if allianceStation, ok := arena.AllianceStations[station]; ok {
+			dsConn := allianceStation.DsConn
+			if dsConn != nil {
+				err := dsConn.sendGameDataPacket(gameData)
+				if err != nil {
+					log.Printf("Error sending game data packet to Team %d: %v", dsConn.TeamId, err)
+				}
+			}
+		}
+	}
+}
+
 // Returns the alliance station identifier for the given team, or the empty string if the team is not present
 // in the current match.
 func (arena *Arena) getAssignedAllianceStation(teamId int) string {
@@ -717,6 +739,26 @@ func (arena *Arena) handlePlcInput() {
 		// Don't do anything if we're outside the match, otherwise we may overwrite manual edits.
 		return
 	}
+
+	redScore := &arena.RedRealtimeScore.CurrentScore
+	oldRedScore := *redScore
+	blueScore := &arena.BlueRealtimeScore.CurrentScore
+	oldBlueScore := *blueScore
+
+	if redScore.StageAtCapacity(game.Stage3, arena.MatchState >= TeleopPeriod) &&
+		redScore.Stage3TargetColor == game.ColorUnknown ||
+		blueScore.StageAtCapacity(game.Stage3, arena.MatchState >= TeleopPeriod) &&
+			blueScore.Stage3TargetColor == game.ColorUnknown {
+		// Determine the position control target colors and send packets to inform the driver stations.
+		redScore.Stage3TargetColor = arena.RedControlPanel.GetStage3TargetColor()
+		blueScore.Stage3TargetColor = arena.BlueControlPanel.GetStage3TargetColor()
+		arena.sendGameDataPacket(redScore.Stage3TargetColor, "R1", "R2", "R3")
+		arena.sendGameDataPacket(blueScore.Stage3TargetColor, "B1", "B2", "B3")
+	}
+
+	if !oldRedScore.Equals(redScore) || !oldBlueScore.Equals(blueScore) {
+		arena.RealtimeScoreNotifier.Notify()
+	}
 }
 
 func (arena *Arena) handlePlcOutput() {

field/arena_notifiers.go

@@ -29,6 +29,7 @@ type ArenaNotifiers struct {
 	ReloadDisplaysNotifier             *websocket.Notifier
 	ScorePostedNotifier                *websocket.Notifier
 	ScoringStatusNotifier              *websocket.Notifier
+	ControlPanelColorNotifier          *websocket.Notifier
 }
 
 type DisplayConfigurationMessage struct {
@@ -65,6 +66,7 @@ func (arena *Arena) configureNotifiers() {
 	arena.ReloadDisplaysNotifier = websocket.NewNotifier("reload", nil)
 	arena.ScorePostedNotifier = websocket.NewNotifier("scorePosted", arena.generateScorePostedMessage)
 	arena.ScoringStatusNotifier = websocket.NewNotifier("scoringStatus", arena.generateScoringStatusMessage)
+	arena.ControlPanelColorNotifier = websocket.NewNotifier("controlPanelColor", arena.generateControlPanelColorMessage)
 }
 
 func (arena *Arena) generateAllianceSelectionMessage() interface{} {
@@ -226,6 +228,13 @@ func (arena *Arena) generateScoringStatusMessage() interface{} {
 		arena.ScoringPanelRegistry.GetNumPanels("blue"), arena.ScoringPanelRegistry.GetNumScoreCommitted("blue")}
 }
 
+func (arena *Arena) generateControlPanelColorMessage() interface{} {
+	return &struct {
+		RedControlPanelColor  game.ControlPanelColor
+		BlueControlPanelColor game.ControlPanelColor
+	}{arena.RedControlPanel.CurrentColor, arena.BlueControlPanel.CurrentColor}
+}
+
 // Constructs the data object for one alliance sent to the audience display for the realtime scoring overlay.
 func getAudienceAllianceScoreFields(allianceScore *RealtimeScore,
 	allianceScoreSummary *game.ScoreSummary) *audienceAllianceScoreFields {

field/driver_station_connection.go

@@ -385,3 +385,26 @@ func (dsConn *DriverStationConnection) handleTcpConnection(arena *Arena) {
 		}
 	}
 }
+
+// Sends a TCP packet containing the given game data to the driver station.
+func (dsConn *DriverStationConnection) sendGameDataPacket(gameData string) error {
+	byteData := []byte(gameData)
+	size := len(byteData)
+	packet := make([]byte, size+4)
+
+	packet[0] = 0              // Packet size
+	packet[1] = byte(size + 2) // Packet size
+	packet[2] = 28             // Packet type
+	packet[3] = byte(size)     // Data size
+
+	// Fill the rest of the packet with the data.
+	for i, character := range byteData {
+		packet[i+4] = character
+	}
+
+	if dsConn.tcpConn != nil {
+		_, err := dsConn.tcpConn.Write(packet)
+		return err
+	}
+	return nil
+}