mirror of
https://github.com/Team254/cheesy-arena-lite.git
synced 2026-03-09 21:56:50 -04:00
Implement PLC integration for the control panel.
This commit is contained in:
Patrick Fairbank
@@ -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
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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 {
|
||||
|
||||
Reference in New Issue
Block a user