Update PLC input/output logic for 2024.

Team254 · May 19, 2024 · 497585e · 497585e
1 parent 452408c
commit 497585e
Show file tree

Hide file tree

Showing 6 changed files with 285 additions and 150 deletions.
diff --git a/field/arena.go b/field/arena.go
@@ -900,18 +900,21 @@ func (arena *Arena) handlePlcInputOutput() {
 	arena.AllianceStations["B3"].Ethernet = blueEthernets[2]
 
 	// Handle in-match PLC functions.
-	// TODO(pat): Update for 2024.
-	//matchStartTime := arena.MatchStartTime
-	//currentTime := time.Now()
-	//teleopGracePeriod := matchStartTime.Add(game.GetDurationToTeleopEnd() + game.ChargeStationTeleopGracePeriod)
-	//inGracePeriod := currentTime.Before(teleopGracePeriod)
-	//
-	//redScore := &arena.RedRealtimeScore.CurrentScore
-	//blueScore := &arena.BlueRealtimeScore.CurrentScore
-	//redChargeStationLevel, blueChargeStationLevel := arena.Plc.GetChargeStationsLevel()
+	redScore := &arena.RedRealtimeScore.CurrentScore
+	oldRedScore := *redScore
+	blueScore := &arena.BlueRealtimeScore.CurrentScore
+	oldBlueScore := *blueScore
+	matchStartTime := arena.MatchStartTime
+	currentTime := time.Now()
+	teleopGracePeriod := matchStartTime.Add(
+		game.GetDurationToTeleopEnd() + game.SpeakerTeleopGracePeriodSec*time.Second,
+	)
+	inGracePeriod := arena.MatchState == PostMatch && currentTime.Before(teleopGracePeriod)
+
 	redAllianceReady := arena.checkAllianceStationsReady("R1", "R2", "R3") == nil
 	blueAllianceReady := arena.checkAllianceStationsReady("B1", "B2", "B3") == nil
 
+	// Handle the evergreen PLC functions: stack lights, stack buzzer, and field reset light.
 	switch arena.MatchState {
 	case PreMatch:
 		if arena.lastMatchState != PreMatch {
@@ -934,52 +937,76 @@ func (arena *Arena) handlePlcInputOutput() {
 				arena.CurrentMatch.FieldReadyAt = time.Now()
 			}
 		}
-
-		// TODO(pat): Update for 2024.
-		//arena.Plc.SetChargeStationLights(false, false)
 	case PostMatch:
 		if arena.FieldReset {
 			arena.Plc.SetFieldResetLight(true)
 		}
 		scoreReady := arena.RedRealtimeScore.FoulsCommitted && arena.BlueRealtimeScore.FoulsCommitted &&
 			arena.alliancePostMatchScoreReady("red") && arena.alliancePostMatchScoreReady("blue")
 		arena.Plc.SetStackLights(false, false, !scoreReady, false)
-
-		// Game-specific PLC functions.
-		// TODO(pat): Update for 2024.
-		//if inGracePeriod {
-		//	arena.Plc.SetChargeStationLights(redChargeStationLevel, blueChargeStationLevel)
-		//} else {
-		//	arena.Plc.SetChargeStationLights(false, false)
-		//}
-		//if arena.lastMatchState != PostMatch {
-		//	go func() {
-		//		// Capture a single reading of the charge station levels after the grace period following the match.
-		//		time.Sleep(game.ChargeStationTeleopGracePeriod)
-		//		redScore.EndgameChargeStationLevel, blueScore.EndgameChargeStationLevel =
-		//			arena.Plc.GetChargeStationsLevel()
-		//		arena.RealtimeScoreNotifier.Notify()
-		//	}()
-		//}
-	case AutoPeriod:
+	case AutoPeriod, PausePeriod, TeleopPeriod:
 		arena.Plc.SetStackBuzzer(false)
 		arena.Plc.SetStackLights(!redAllianceReady, !blueAllianceReady, false, true)
-		fallthrough
-	case PausePeriod:
-		// Game-specific PLC functions.
-		// TODO(pat): Update for 2024.
-		//arena.Plc.SetChargeStationLights(redChargeStationLevel, blueChargeStationLevel)
-	case TeleopPeriod:
-		// Game-specific PLC functions.
-		// TODO(pat): Update for 2024.
-		//arena.Plc.SetChargeStationLights(redChargeStationLevel, blueChargeStationLevel)
-		arena.Plc.SetStackLights(!redAllianceReady, !blueAllianceReady, false, true)
-		//if arena.lastMatchState != TeleopPeriod {
-		//	// Capture a single reading of the charge station levels after the autonomous pause.
-		//	redScore.AutoChargeStationLevel, blueScore.AutoChargeStationLevel = arena.Plc.GetChargeStationsLevel()
-		//	arena.RealtimeScoreNotifier.Notify()
-		//}
 	}
+
+	// Get all the game-specific inputs and update the score.
+	redAmplifyButton, redCoopButton, blueAmplifyButton, blueCoopButton := arena.Plc.GetAmpButtons()
+	redAmpNoteCount, redSpeakerNoteCount, blueAmpNoteCount, blueSpeakerNoteCount := arena.Plc.GetAmpSpeakerNoteCounts()
+	redAmpSpeaker := &arena.RedRealtimeScore.CurrentScore.AmpSpeaker
+	blueAmpSpeaker := &arena.BlueRealtimeScore.CurrentScore.AmpSpeaker
+	redAmpSpeaker.UpdateState(
+		redAmpNoteCount, redSpeakerNoteCount, redAmplifyButton, redCoopButton, matchStartTime, currentTime,
+	)
+	blueAmpSpeaker.UpdateState(
+		blueAmpNoteCount, blueSpeakerNoteCount, blueAmplifyButton, blueCoopButton, matchStartTime, currentTime,
+	)
+	if !oldRedScore.Equals(redScore) || !oldBlueScore.Equals(blueScore) {
+		arena.RealtimeScoreNotifier.Notify()
+	}
+
+	// Handle the amp outputs.
+	redAmplifiedTimeRemaining := redAmpSpeaker.AmplifiedTimeRemaining(currentTime)
+	blueAmplifiedTimeRemaining := blueAmpSpeaker.AmplifiedTimeRemaining(currentTime)
+	if arena.MatchState == AutoPeriod || arena.MatchState == PausePeriod || arena.MatchState == TeleopPeriod {
+		redLowAmpLight := redAmpSpeaker.BankedAmpNotes >= 1
+		redHighAmpLight := redAmpSpeaker.BankedAmpNotes >= 2
+		redCoopAmpLight := redAmpSpeaker.CoopActivated
+		if redAmplifiedTimeRemaining > 0 {
+			redLowAmpLight = int(redAmplifiedTimeRemaining*2)%2 == 0
+			redHighAmpLight = !redLowAmpLight
+		}
+
+		blueLowAmpLight := blueAmpSpeaker.BankedAmpNotes >= 1
+		blueHighAmpLight := blueAmpSpeaker.BankedAmpNotes >= 2
+		blueCoopAmpLight := blueAmpSpeaker.CoopActivated
+		if blueAmplifiedTimeRemaining > 0 {
+			blueLowAmpLight = int(blueAmplifiedTimeRemaining*4)%2 == 0
+			blueHighAmpLight = !blueLowAmpLight
+		}
+
+		arena.Plc.SetAmpLights(
+			redLowAmpLight, redHighAmpLight, redCoopAmpLight, blueLowAmpLight, blueHighAmpLight, blueCoopAmpLight,
+		)
+	} else if arena.MatchState == PostMatch {
+		arena.Plc.SetAmpLights(false, false, false, false, false, false)
+	}
+
+	// Handle the speaker outputs.
+	arena.Plc.SetSpeakerMotors(
+		arena.MatchState == AutoPeriod || arena.MatchState == PausePeriod || arena.MatchState == TeleopPeriod ||
+			inGracePeriod,
+	)
+	arena.Plc.SetSpeakerLights(
+		redAmplifiedTimeRemaining > 0 && arena.MatchState != PostMatch,
+		blueAmplifiedTimeRemaining > 0 && arena.MatchState != PostMatch,
+	)
+
+	// Handle the subwoofer outputs.
+	arena.Plc.SetSubwooferCountdown(
+		redAmplifiedTimeRemaining > 0 && arena.MatchState != PostMatch,
+		blueAmplifiedTimeRemaining > 0 && arena.MatchState != PostMatch,
+	)
+	arena.Plc.SetPostMatchSubwooferLights(inGracePeriod)
 }
 
 func (arena *Arena) handleTeamStop(station string, eStopState, aStopState bool) {