diff --git a/field/arena.go b/field/arena.go index 8250a44..36084b2 100644 --- a/field/arena.go +++ b/field/arena.go @@ -761,13 +761,13 @@ func (arena *Arena) handlePlcInput() { oldBlueScore := *blueScore matchStartTime := arena.MatchStartTime currentTime := time.Now() + teleopStarted := arena.MatchState >= TeleopPeriod if arena.Plc.IsEnabled() { // Handle power ports. - redPortCells, bluePortCells := arena.Plc.GetPowerPortCells() + redPortCells, bluePortCells := arena.Plc.GetPowerPorts() redPowerPort := arena.RedRealtimeScore.powerPort - redPowerPort.UpdateState(redPortCells, redScore.CellCountingStage(arena.MatchState >= TeleopPeriod), - matchStartTime, currentTime) + redPowerPort.UpdateState(redPortCells, redScore.CellCountingStage(teleopStarted), matchStartTime, currentTime) redScore.AutoCellsBottom = redPowerPort.AutoCellsBottom redScore.AutoCellsOuter = redPowerPort.AutoCellsOuter redScore.AutoCellsInner = redPowerPort.AutoCellsInner @@ -775,26 +775,39 @@ func (arena *Arena) handlePlcInput() { redScore.TeleopCellsOuter = redPowerPort.TeleopCellsOuter redScore.TeleopCellsInner = redPowerPort.TeleopCellsInner bluePowerPort := arena.BlueRealtimeScore.powerPort - bluePowerPort.UpdateState(bluePortCells, blueScore.CellCountingStage(arena.MatchState >= TeleopPeriod), - matchStartTime, currentTime) + bluePowerPort.UpdateState(bluePortCells, blueScore.CellCountingStage(teleopStarted), 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 + + // Handle control panel. + redColor, redSegmentCount, blueColor, blueSegmentCount := arena.Plc.GetControlPanels() + redControlPanel := arena.RedRealtimeScore.ControlPanel + redControlPanel.CurrentColor = redColor + redControlPanel.UpdateState(redSegmentCount, redScore.StageAtCapacity(game.Stage2, teleopStarted), + redScore.StageAtCapacity(game.Stage3, teleopStarted), currentTime) + redScore.ControlPanelStatus = redControlPanel.ControlPanelStatus + blueControlPanel := arena.BlueRealtimeScore.ControlPanel + blueControlPanel.CurrentColor = blueColor + blueControlPanel.UpdateState(blueSegmentCount, blueScore.StageAtCapacity(game.Stage2, teleopStarted), + blueScore.StageAtCapacity(game.Stage3, teleopStarted), currentTime) + blueScore.ControlPanelStatus = blueControlPanel.ControlPanelStatus } // Check if either alliance has reached Stage 3 capacity. if redScore.StageAtCapacity(game.Stage3, arena.MatchState >= TeleopPeriod) && - redScore.Stage3TargetColor == game.ColorUnknown || + redScore.PositionControlTargetColor == game.ColorUnknown || blueScore.StageAtCapacity(game.Stage3, arena.MatchState >= TeleopPeriod) && - blueScore.Stage3TargetColor == game.ColorUnknown { + blueScore.PositionControlTargetColor == game.ColorUnknown { // Determine the position control target colors and send packets to inform the driver stations. - redScore.Stage3TargetColor = arena.RedRealtimeScore.ControlPanel.GetStage3TargetColor() - blueScore.Stage3TargetColor = arena.BlueRealtimeScore.ControlPanel.GetStage3TargetColor() - arena.sendGameDataPacket(redScore.Stage3TargetColor, "R1", "R2", "R3") - arena.sendGameDataPacket(blueScore.Stage3TargetColor, "B1", "B2", "B3") + redScore.PositionControlTargetColor = arena.RedRealtimeScore.ControlPanel.GetPositionControlTargetColor() + blueScore.PositionControlTargetColor = arena.BlueRealtimeScore.ControlPanel.GetPositionControlTargetColor() + arena.sendGameDataPacket(redScore.PositionControlTargetColor, "R1", "R2", "R3") + arena.sendGameDataPacket(blueScore.PositionControlTargetColor, "B1", "B2", "B3") } if !oldRedScore.Equals(redScore) || !oldBlueScore.Equals(blueScore) { diff --git a/game/control_panel.go b/game/control_panel.go index d1b7cdd..66c1364 100644 --- a/game/control_panel.go +++ b/game/control_panel.go @@ -5,19 +5,33 @@ package game -import "math/rand" +import ( + "math" + "math/rand" + "time" +) type ControlPanel struct { CurrentColor ControlPanelColor + ControlPanelStatus + ControlPanelLightState + rotationStarted bool + rotationStartSegmentCount int + lastSegmentCountDiff int + rotationStopTime time.Time + positionTargetColor ControlPanelColor + lastPositionCorrect bool + positionStopTime time.Time } type ControlPanelColor int +// This ordering matches the values in the official FRC PLC code: 0:UnknownError, 1:Red, 2:Blue, 3:Green, 4:Yellow const ( ColorUnknown ControlPanelColor = iota ColorRed - ColorGreen ColorBlue + ColorGreen ColorYellow ) @@ -29,17 +43,55 @@ const ( ControlPanelPosition ) -// Returns a random color that does not match the current color. -func (controlPanel *ControlPanel) GetStage3TargetColor() ControlPanelColor { - if controlPanel.CurrentColor == ColorUnknown { - // If the sensor or manual scorekeeping did not detect/set the current color, pick one of the four at random. - return ControlPanelColor(rand.Intn(4) + 1) +type ControlPanelLightState int + +const ( + ControlPanelLightOff ControlPanelLightState = iota + ControlPanelLightOn + ControlPanelLightFlashing +) + +const ( + rotationControlMinSegments = 24 + rotationControlMaxSegments = 40 + rotationControlStopDurationSec = 2 + positionControlStopMinDurationSec = 3 + positionControlStopMaxDurationSec = 5 +) + +// Updates the internal state of the control panel given the current state of the hardware counts and the rest of the +// score. +func (controlPanel *ControlPanel) UpdateState(segmentCount int, stage2AtCapacity, stage3AtCapacity bool, + currentTime time.Time) { + if !stage2AtCapacity { + controlPanel.ControlPanelStatus = ControlPanelNone + controlPanel.ControlPanelLightState = ControlPanelLightOff + } else if controlPanel.ControlPanelStatus == ControlPanelNone { + controlPanel.assessRotationControl(segmentCount, currentTime) + } else if controlPanel.ControlPanelStatus == ControlPanelRotation && stage3AtCapacity { + controlPanel.assessPositionControl(currentTime) + } else { + controlPanel.ControlPanelLightState = ControlPanelLightOff } - newColor := int(controlPanel.CurrentColor) + rand.Intn(3) + 1 - if newColor > 4 { - newColor -= 4 +} + +// Returns the target color for position control, assigning it randomly if it is not yet designated. +func (controlPanel *ControlPanel) GetPositionControlTargetColor() ControlPanelColor { + if controlPanel.positionTargetColor == ColorUnknown { + if controlPanel.CurrentColor == ColorUnknown { + // If the sensor or manual scorekeeping did not detect/set the current color, pick one of the four at + // random. + controlPanel.positionTargetColor = ControlPanelColor(rand.Intn(4) + 1) + } else { + // Randomly pick one of the non-current colors. + newColor := int(controlPanel.CurrentColor) + rand.Intn(3) + 1 + if newColor > 4 { + newColor -= 4 + } + controlPanel.positionTargetColor = ControlPanelColor(newColor) + } } - return ControlPanelColor(newColor) + return controlPanel.positionTargetColor } // Returns the string that is to be sent to the driver station for the given color. @@ -47,12 +99,68 @@ func GetGameDataForColor(color ControlPanelColor) string { switch color { case ColorRed: return "R" - case ColorGreen: - return "G" case ColorBlue: return "B" + case ColorGreen: + return "G" case ColorYellow: return "Y" } return "" } + +// Updates the state of the control panel while rotation control is in the process of being performed. +func (controlPanel *ControlPanel) assessRotationControl(segmentCount int, currentTime time.Time) { + if !controlPanel.rotationStarted { + controlPanel.rotationStarted = true + controlPanel.rotationStartSegmentCount = segmentCount + } + + segmentCountDiff := int(math.Abs(float64(segmentCount - controlPanel.rotationStartSegmentCount))) + if segmentCountDiff < rotationControlMinSegments { + // The control panel still needs to be rotated more. + controlPanel.ControlPanelLightState = ControlPanelLightOn + } else if segmentCountDiff < rotationControlMaxSegments { + // The control panel has been rotated the correct amount and needs to stop on a single color. + if segmentCountDiff != controlPanel.lastSegmentCountDiff { + // The control panel is still moving; reset the timer. + controlPanel.rotationStopTime = currentTime + controlPanel.ControlPanelLightState = ControlPanelLightFlashing + } else if currentTime.Sub(controlPanel.rotationStopTime) < rotationControlStopDurationSec*time.Second { + controlPanel.ControlPanelLightState = ControlPanelLightFlashing + } else { + // The control panel has been stopped long enough; rotation control is complete. + controlPanel.ControlPanelStatus = ControlPanelRotation + controlPanel.ControlPanelLightState = ControlPanelLightOff + } + } else { + // The control panel has been rotated too much; reset the count. + controlPanel.rotationStartSegmentCount = segmentCount + controlPanel.ControlPanelLightState = ControlPanelLightOn + } + controlPanel.lastSegmentCountDiff = segmentCountDiff +} + +// Updates the state of the control panel while position control is in the process of being performed. +func (controlPanel *ControlPanel) assessPositionControl(currentTime time.Time) { + positionCorrect := controlPanel.CurrentColor == controlPanel.GetPositionControlTargetColor() && + controlPanel.CurrentColor != ColorUnknown + if positionCorrect && !controlPanel.lastPositionCorrect { + controlPanel.positionStopTime = currentTime + } + controlPanel.lastPositionCorrect = positionCorrect + + if !positionCorrect { + controlPanel.ControlPanelLightState = ControlPanelLightOn + } else if currentTime.Sub(controlPanel.positionStopTime) < positionControlStopMinDurationSec*time.Second { + // The control panel is on the target color but may still be moving. + controlPanel.ControlPanelLightState = ControlPanelLightOn + } else if currentTime.Sub(controlPanel.positionStopTime) < positionControlStopMaxDurationSec*time.Second { + // The control panel is stopped on the target color, but not long enough to count. + controlPanel.ControlPanelLightState = ControlPanelLightFlashing + } else { + // The target color has been present for long enough; position control is complete. + controlPanel.ControlPanelStatus = ControlPanelPosition + controlPanel.ControlPanelLightState = ControlPanelLightOff + } +} diff --git a/game/control_panel_test.go b/game/control_panel_test.go index 785d69f..4c65614 100644 --- a/game/control_panel_test.go +++ b/game/control_panel_test.go @@ -7,38 +7,150 @@ import ( "github.com/stretchr/testify/assert" "math/rand" "testing" + "time" ) -func TestControlPanelGetStage3TargetColor(t *testing.T) { +func TestControlPanelGetPositionControlTargetColor(t *testing.T) { rand.Seed(0) var controlPanel ControlPanel controlPanel.CurrentColor = ColorUnknown - results := getStage3TargetColorNTimes(&controlPanel, 10000) + results := getPositionTargetColorNTimes(&controlPanel, 10000) assert.Equal(t, [5]int{0, 2543, 2527, 2510, 2420}, results) controlPanel.CurrentColor = ColorRed - results = getStage3TargetColorNTimes(&controlPanel, 10000) + results = getPositionTargetColorNTimes(&controlPanel, 10000) assert.Equal(t, [5]int{0, 0, 3351, 3311, 3338}, results) - controlPanel.CurrentColor = ColorGreen - results = getStage3TargetColorNTimes(&controlPanel, 10000) + controlPanel.CurrentColor = ColorBlue + results = getPositionTargetColorNTimes(&controlPanel, 10000) assert.Equal(t, [5]int{0, 3335, 0, 3320, 3345}, results) - controlPanel.CurrentColor = ColorBlue - results = getStage3TargetColorNTimes(&controlPanel, 10000) + controlPanel.CurrentColor = ColorGreen + results = getPositionTargetColorNTimes(&controlPanel, 10000) assert.Equal(t, [5]int{0, 3328, 3296, 0, 3376}, results) controlPanel.CurrentColor = ColorYellow - results = getStage3TargetColorNTimes(&controlPanel, 10000) + results = getPositionTargetColorNTimes(&controlPanel, 10000) assert.Equal(t, [5]int{0, 3303, 3388, 3309, 0}, results) } +func TestGetGameDataForColor(t *testing.T) { + assert.Equal(t, "", GetGameDataForColor(ColorUnknown)) + assert.Equal(t, "R", GetGameDataForColor(ColorRed)) + assert.Equal(t, "B", GetGameDataForColor(ColorBlue)) + assert.Equal(t, "G", GetGameDataForColor(ColorGreen)) + assert.Equal(t, "Y", GetGameDataForColor(ColorYellow)) + assert.Equal(t, "", GetGameDataForColor(-100)) +} + +func TestControlPanelUpdateState(t *testing.T) { + rand.Seed(0) + var controlPanel ControlPanel + controlPanel.ControlPanelStatus = ControlPanelRotation + currentTime := time.Now() + + // Check before Stage 2 capacity is reached. + controlPanel.UpdateState(0, false, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(30, false, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(50, false, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) + + // Check rotation control. + controlPanel.UpdateState(60, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(80, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(37, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(36, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(40, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(35, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(21, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(20, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(44, true, false, currentTime) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(55, true, false, currentTime.Add(1*time.Millisecond)) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(55, true, false, currentTime.Add(2000*time.Millisecond)) + assert.Equal(t, ControlPanelNone, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(55, true, false, currentTime.Add(2001*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(-1000, true, false, currentTime.Add(3000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) + + // Check position control. + assert.Equal(t, ColorUnknown, controlPanel.positionTargetColor) + controlPanel.UpdateState(1000, true, true, currentTime.Add(5000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + assert.Equal(t, ColorGreen, controlPanel.GetPositionControlTargetColor()) + controlPanel.CurrentColor = ColorBlue + controlPanel.UpdateState(1001, true, true, currentTime.Add(6000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.CurrentColor = ColorGreen + controlPanel.UpdateState(1002, true, true, currentTime.Add(7000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(1002, true, true, currentTime.Add(9999*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(1002, true, true, currentTime.Add(10000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.CurrentColor = ColorYellow + controlPanel.UpdateState(1003, true, true, currentTime.Add(11000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(1003, true, true, currentTime.Add(20000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.CurrentColor = ColorGreen + controlPanel.UpdateState(1002, true, true, currentTime.Add(21000*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOn, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(1002, true, true, currentTime.Add(25999*time.Millisecond)) + assert.Equal(t, ControlPanelRotation, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightFlashing, controlPanel.ControlPanelLightState) + controlPanel.UpdateState(1002, true, true, currentTime.Add(26000*time.Millisecond)) + assert.Equal(t, ControlPanelPosition, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) + controlPanel.CurrentColor = ColorRed + controlPanel.UpdateState(0, true, true, currentTime.Add(26001*time.Millisecond)) + assert.Equal(t, ControlPanelPosition, controlPanel.ControlPanelStatus) + assert.Equal(t, ControlPanelLightOff, controlPanel.ControlPanelLightState) +} + // Invokes the method N times and returns a map of the counts for each result, for statistical testing. -func getStage3TargetColorNTimes(controlPanel *ControlPanel, n int) [5]int { +func getPositionTargetColorNTimes(controlPanel *ControlPanel, n int) [5]int { var results [5]int for i := 0; i < n; i++ { - results[controlPanel.GetStage3TargetColor()]++ + controlPanel.positionTargetColor = ColorUnknown + results[controlPanel.GetPositionControlTargetColor()]++ } return results } diff --git a/game/score.go b/game/score.go index cb2b97c..bfed366 100644 --- a/game/score.go +++ b/game/score.go @@ -16,11 +16,11 @@ type Score struct { TeleopCellsOuter [4]int TeleopCellsInner [4]int ControlPanelStatus - EndgameStatuses [3]EndgameStatus - RungIsLevel bool - Fouls []Foul - ElimDq bool - Stage3TargetColor ControlPanelColor + EndgameStatuses [3]EndgameStatus + RungIsLevel bool + Fouls []Foul + ElimDq bool + PositionControlTargetColor ControlPanelColor } type ScoreSummary struct { diff --git a/partner/tba.go b/partner/tba.go index 6269fa3..96ffb96 100644 --- a/partner/tba.go +++ b/partner/tba.go @@ -545,7 +545,7 @@ func createTbaScoringBreakdown(match *model.Match, matchResult *model.MatchResul breakdown.Stage1Activated = scoreSummary.StagesActivated[0] breakdown.Stage2Activated = scoreSummary.StagesActivated[1] breakdown.Stage3Activated = scoreSummary.StagesActivated[2] - breakdown.Stage3TargetColor = controlPanelColorMapping[score.Stage3TargetColor] + breakdown.Stage3TargetColor = controlPanelColorMapping[score.PositionControlTargetColor] breakdown.EndgameRobot1 = endgameMapping[score.EndgameStatuses[0]] breakdown.EndgameRobot2 = endgameMapping[score.EndgameStatuses[1]] breakdown.EndgameRobot3 = endgameMapping[score.EndgameStatuses[2]] diff --git a/plc/plc.go b/plc/plc.go index 580f7f3..399fac0 100644 --- a/plc/plc.go +++ b/plc/plc.go @@ -7,6 +7,7 @@ package plc import ( "fmt" + "github.com/Team254/cheesy-arena/game" "github.com/Team254/cheesy-arena/websocket" "github.com/goburrow/modbus" "log" @@ -210,7 +211,7 @@ 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) { +func (plc *Plc) GetPowerPorts() ([3]int, [3]int) { return [3]int{ int(plc.registers[redPowerPortBottom]), int(plc.registers[redPowerPortOuter]), @@ -223,6 +224,12 @@ func (plc *Plc) GetPowerPortCells() ([3]int, [3]int) { } } +// Returns the current color and number of segment transitions for each of the red and blue control panels. +func (plc *Plc) GetControlPanels() (game.ControlPanelColor, int, game.ControlPanelColor, int) { + return game.ControlPanelColor(plc.registers[redControlPanelColor]), int(plc.registers[redControlPanelSegments]), + game.ControlPanelColor(plc.registers[blueControlPanelColor]), int(plc.registers[blueControlPanelSegments]) +} + // 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