diff --git a/field/arena.go b/field/arena.go index 410f0f6..6ea371a 100644 --- a/field/arena.go +++ b/field/arena.go @@ -194,6 +194,7 @@ func (arena *Arena) LoadMatch(match *model.Match) error { // Reset the realtime scores. arena.RedRealtimeScore = NewRealtimeScore() arena.BlueRealtimeScore = NewRealtimeScore() + arena.Plc.ResetCounts() arena.FieldReset = false // Notify any listeners about the new match. @@ -636,11 +637,11 @@ func (arena *Arena) handlePlcInput() { // Handle touchpads. redTouchpads, blueTouchpads := arena.Plc.GetTouchpads() for i := 0; i < 3; i++ { - arena.RedRealtimeScore.touchpads[i].UpdateState(redTouchpads[i], currentTime) - arena.BlueRealtimeScore.touchpads[i].UpdateState(blueTouchpads[i], currentTime) + arena.RedRealtimeScore.touchpads[i].UpdateState(redTouchpads[i], matchStartTime, currentTime) + arena.BlueRealtimeScore.touchpads[i].UpdateState(blueTouchpads[i], matchStartTime, currentTime) } - redScore.Takeoffs = game.CountTouchpads(&arena.RedRealtimeScore.touchpads, matchStartTime, currentTime) - blueScore.Takeoffs = game.CountTouchpads(&arena.BlueRealtimeScore.touchpads, matchStartTime, currentTime) + redScore.Takeoffs = game.CountTouchpads(&arena.RedRealtimeScore.touchpads, currentTime) + blueScore.Takeoffs = game.CountTouchpads(&arena.BlueRealtimeScore.touchpads, currentTime) if !oldRedScore.Equals(redScore) || !oldBlueScore.Equals(blueScore) { arena.RealtimeScoreNotifier.Notify(nil) @@ -680,7 +681,6 @@ func (arena *Arena) handlePlcOutput() { // Handle touchpads. var redTouchpads, blueTouchpads [3]bool currentTime := time.Now() - matchStartTime := arena.MatchStartTime blinkStopTime := matchEndTime.Add(-time.Duration(game.MatchTiming.EndgameTimeLeftSec-2) * time.Second) if arena.MatchState == EndgamePeriod && currentTime.Before(blinkStopTime) { // Blink the touchpads at the endgame start point. @@ -689,15 +689,11 @@ func (arena *Arena) handlePlcOutput() { blueTouchpads[i] = arena.Plc.BlinkState } } else { - if arena.MatchState == PreMatch { - // Allow touchpads to be triggered before a match. - matchStartTime = currentTime - } for i := 0; i < 3; i++ { - redState := arena.RedRealtimeScore.touchpads[i].GetState(matchStartTime, currentTime) - redTouchpads[i] = redState == 2 || redState == 1 && arena.Plc.BlinkState - blueState := arena.BlueRealtimeScore.touchpads[i].GetState(matchStartTime, currentTime) - blueTouchpads[i] = blueState == 2 || blueState == 1 && arena.Plc.BlinkState + redState := arena.RedRealtimeScore.touchpads[i].GetState(currentTime) + redTouchpads[i] = redState == game.Held || redState == game.Triggered && arena.Plc.BlinkState + blueState := arena.BlueRealtimeScore.touchpads[i].GetState(currentTime) + blueTouchpads[i] = blueState == game.Held || blueState == game.Triggered && arena.Plc.BlinkState } } arena.Plc.SetTouchpadLights(redTouchpads, blueTouchpads) diff --git a/field/plc.go b/field/plc.go index fea77d5..a7dc06a 100644 --- a/field/plc.go +++ b/field/plc.go @@ -13,14 +13,15 @@ import ( ) type Plc struct { - IsHealthy bool - BlinkState bool - address string - handler *modbus.TCPClientHandler - client modbus.Client - Inputs [15]bool - Counters [10]uint16 - Coils [24]bool + IsHealthy bool + BlinkState bool + address string + handler *modbus.TCPClientHandler + client modbus.Client + Inputs [15]bool + Counters [10]uint16 + Coils [24]bool + resetCountCycles int } const ( @@ -117,9 +118,9 @@ func (plc *Plc) Run() { startTime := time.Now() isHealthy := true + isHealthy = isHealthy && plc.writeCoils() isHealthy = isHealthy && plc.readInputs() isHealthy = isHealthy && plc.readCounters() - isHealthy = isHealthy && plc.writeCoils() if !isHealthy { plc.resetConnection() } @@ -185,6 +186,7 @@ func (plc *Plc) GetTouchpads() ([3]bool, [3]bool) { // Resets the ball and rotor gear tooth counts to zero. func (plc *Plc) ResetCounts() { plc.Coils[resetCounts] = true + plc.resetCountCycles = 0 } func (plc *Plc) SetBoilerMotors(on bool) { @@ -289,7 +291,11 @@ func (plc *Plc) writeCoils() bool { return false } - plc.Coils[resetCounts] = false // Only need to send a single pulse to reset the counters. + if plc.resetCountCycles > 5 { + plc.Coils[resetCounts] = false // Need to send a short pulse to reset the counters. + } else { + plc.resetCountCycles++ + } return true } diff --git a/game/boiler.go b/game/boiler.go index 0a973da..f5a8c83 100644 --- a/game/boiler.go +++ b/game/boiler.go @@ -36,7 +36,7 @@ func (boiler *Boiler) UpdateState(lowCount, highCount int, matchStartTime, curre boiler.FuelLow = 0 boiler.FuelHigh = 0 } else if currentTime.Before(teleopValidityCutoff) { - boiler.FuelLow = lowCount - boiler.FuelHigh = highCount + boiler.FuelLow = lowCount - boiler.AutoFuelLow + boiler.FuelHigh = highCount - boiler.AutoFuelHigh } } diff --git a/game/boiler_test.go b/game/boiler_test.go index 92deedf..fe0dd81 100644 --- a/game/boiler_test.go +++ b/game/boiler_test.go @@ -27,24 +27,24 @@ func TestAutoFuel(t *testing.T) { checkBoilerCounts(t, 5, 6, 0, 0, &boiler) boiler.UpdateState(7, 8, matchStartTime, timeAfterStart(19.9)) checkBoilerCounts(t, 7, 8, 0, 0, &boiler) - boiler.UpdateState(9, 10, matchStartTime, timeAfterStart(20.1)) - checkBoilerCounts(t, 7, 8, 9, 10, &boiler) + boiler.UpdateState(9, 11, matchStartTime, timeAfterStart(20.1)) + checkBoilerCounts(t, 7, 8, 2, 3, &boiler) } func TestTeleopFuel(t *testing.T) { boiler := Boiler{} boiler.UpdateState(1, 2, matchStartTime, timeAfterStart(1)) - boiler.UpdateState(3, 4, matchStartTime, timeAfterStart(21)) - checkBoilerCounts(t, 1, 2, 3, 4, &boiler) - boiler.UpdateState(5, 6, matchStartTime, timeAfterStart(120)) - checkBoilerCounts(t, 1, 2, 5, 6, &boiler) - boiler.UpdateState(7, 8, matchStartTime, timeAfterEnd(-1)) - checkBoilerCounts(t, 1, 2, 7, 8, &boiler) - boiler.UpdateState(9, 10, matchStartTime, timeAfterEnd(4.9)) - checkBoilerCounts(t, 1, 2, 9, 10, &boiler) - boiler.UpdateState(11, 12, matchStartTime, timeAfterEnd(5.1)) - checkBoilerCounts(t, 1, 2, 9, 10, &boiler) + boiler.UpdateState(3, 5, matchStartTime, timeAfterStart(21)) + checkBoilerCounts(t, 1, 2, 2, 3, &boiler) + boiler.UpdateState(5, 7, matchStartTime, timeAfterStart(120)) + checkBoilerCounts(t, 1, 2, 4, 5, &boiler) + boiler.UpdateState(7, 9, matchStartTime, timeAfterEnd(-1)) + checkBoilerCounts(t, 1, 2, 6, 7, &boiler) + boiler.UpdateState(9, 11, matchStartTime, timeAfterEnd(4.9)) + checkBoilerCounts(t, 1, 2, 8, 9, &boiler) + boiler.UpdateState(11, 13, matchStartTime, timeAfterEnd(5.1)) + checkBoilerCounts(t, 1, 2, 8, 9, &boiler) } func checkBoilerCounts(t *testing.T, autoLow, autoHigh, low, high int, boiler *Boiler) { diff --git a/game/touchpad.go b/game/touchpad.go index a13a458..a56ceaf 100644 --- a/game/touchpad.go +++ b/game/touchpad.go @@ -22,11 +22,16 @@ type Touchpad struct { } // Updates the internal timing state of the touchpad given the current state of the sensor. -func (touchpad *Touchpad) UpdateState(triggered bool, currentTime time.Time) { - if triggered && !touchpad.lastTriggered { +func (touchpad *Touchpad) UpdateState(triggered bool, matchStartTime, currentTime time.Time) { + matchEndTime := GetMatchEndTime(matchStartTime) + + if triggered && !touchpad.lastTriggered && currentTime.Before(matchEndTime) { touchpad.triggeredTime = ¤tTime touchpad.untriggeredTime = nil } else if !triggered && touchpad.lastTriggered { + if currentTime.Before(matchEndTime) || touchpad.GetState(currentTime) == Triggered { + touchpad.triggeredTime = nil + } touchpad.untriggeredTime = ¤tTime } touchpad.lastTriggered = triggered @@ -34,31 +39,29 @@ func (touchpad *Touchpad) UpdateState(triggered bool, currentTime time.Time) { // Determines the scoring status of the touchpad. Returns 0 if not triggered, 1 if triggered but not yet for a full // second, and 2 if triggered and counting for points. -func (touchpad *Touchpad) GetState(matchStartTime, currentTime time.Time) int { - matchEndTime := GetMatchEndTime(matchStartTime) - - if touchpad.triggeredTime != nil && touchpad.triggeredTime.Before(matchEndTime) { - if touchpad.untriggeredTime == nil { - if currentTime.Sub(*touchpad.triggeredTime) >= time.Second { +func (touchpad *Touchpad) GetState(currentTime time.Time) int { + if touchpad.triggeredTime != nil { + if touchpad.untriggeredTime != nil { + if touchpad.untriggeredTime.Sub(*touchpad.triggeredTime) >= time.Second { return Held } else { - return Triggered + return NotTriggered } - } else if touchpad.untriggeredTime.Sub(*touchpad.triggeredTime) >= time.Second && - touchpad.untriggeredTime.After(matchEndTime) { + } + if currentTime.Sub(*touchpad.triggeredTime) >= time.Second { return Held + } else { + return Triggered } } return NotTriggered } -func CountTouchpads(touchpads *[3]Touchpad, matchStartTime, currentTime time.Time) int { - matchEndTime := GetMatchEndTime(matchStartTime) - +func CountTouchpads(touchpads *[3]Touchpad, currentTime time.Time) int { count := 0 for _, touchpad := range touchpads { - if touchpad.GetState(matchEndTime, currentTime) == 2 { + if touchpad.GetState(currentTime) == Held { count++ } } diff --git a/game/touchpad_test.go b/game/touchpad_test.go index fd8ab8e..6cda201 100644 --- a/game/touchpad_test.go +++ b/game/touchpad_test.go @@ -10,91 +10,96 @@ import ( func TestNotTriggered(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(false, timeAfterEnd(-1)) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(-1))) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(2))) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-1)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(-1))) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(2))) } func TestTriggeredReleasedEarly(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(true, timeAfterEnd(-5)) - assert.Equal(t, Triggered, touchpad.GetState(matchStartTime, timeAfterEnd(-4.9))) - assert.Equal(t, Held, touchpad.GetState(matchStartTime, timeAfterEnd(-3))) - touchpad.UpdateState(false, timeAfterEnd(-1)) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(-1.1))) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(2))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-5)) + assert.Equal(t, Triggered, touchpad.GetState(timeAfterEnd(-4.9))) + assert.Equal(t, Held, touchpad.GetState(timeAfterEnd(-3))) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-1)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(-1.1))) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(2))) } func TestTriggeredTooShort(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(true, timeAfterEnd(-0.5)) - touchpad.UpdateState(true, timeAfterEnd(0)) - assert.Equal(t, Triggered, touchpad.GetState(matchStartTime, timeAfterEnd(0.2))) - touchpad.UpdateState(false, timeAfterEnd(0.4)) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(0.5))) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(2))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-0.5)) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(0)) + assert.Equal(t, Triggered, touchpad.GetState(timeAfterEnd(0.2))) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(0.4)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(0.5))) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(2))) + + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(3)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(5))) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(6)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(8))) } func TestTriggeredHeld(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(true, timeAfterEnd(-5)) - touchpad.UpdateState(true, timeAfterEnd(-3)) - touchpad.UpdateState(true, timeAfterEnd(1)) - assert.Equal(t, Held, touchpad.GetState(matchStartTime, timeAfterEnd(2))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-5)) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-3)) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(1)) + assert.Equal(t, Held, touchpad.GetState(timeAfterEnd(2))) } func TestTriggeredReleased(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(true, timeAfterEnd(-5)) - touchpad.UpdateState(true, timeAfterEnd(-3)) - touchpad.UpdateState(true, timeAfterEnd(1)) - assert.Equal(t, Held, touchpad.GetState(matchStartTime, timeAfterEnd(2))) - touchpad.UpdateState(false, timeAfterEnd(3)) - assert.Equal(t, Held, touchpad.GetState(matchStartTime, timeAfterEnd(4))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-5)) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-3)) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(1)) + assert.Equal(t, Held, touchpad.GetState(timeAfterEnd(2))) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(3)) + assert.Equal(t, Held, touchpad.GetState(timeAfterEnd(4))) } func TestReTriggered(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(true, timeAfterEnd(-5)) - assert.Equal(t, Held, touchpad.GetState(matchStartTime, timeAfterEnd(-3))) - touchpad.UpdateState(false, timeAfterEnd(-1)) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(-1.1))) - touchpad.UpdateState(true, timeAfterEnd(-0.1)) - assert.Equal(t, Triggered, touchpad.GetState(matchStartTime, timeAfterEnd(0.1))) - assert.Equal(t, Held, touchpad.GetState(matchStartTime, timeAfterEnd(2))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-5)) + assert.Equal(t, Held, touchpad.GetState(timeAfterEnd(-3))) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-1)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(-1.1))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(-0.1)) + assert.Equal(t, Triggered, touchpad.GetState(timeAfterEnd(0.1))) + assert.Equal(t, Held, touchpad.GetState(timeAfterEnd(2))) } func TestTriggeredLate(t *testing.T) { touchpad := Touchpad{} - touchpad.UpdateState(false, timeAfterEnd(-10)) + touchpad.UpdateState(false, matchStartTime, timeAfterEnd(-10)) - touchpad.UpdateState(true, timeAfterEnd(0.1)) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(0.2))) - assert.Equal(t, NotTriggered, touchpad.GetState(matchStartTime, timeAfterEnd(2))) + touchpad.UpdateState(true, matchStartTime, timeAfterEnd(0.1)) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(0.2))) + assert.Equal(t, NotTriggered, touchpad.GetState(timeAfterEnd(2))) } func TestCountTouchpads(t *testing.T) { var touchpads [3]Touchpad - touchpads[0].UpdateState(true, timeAfterEnd(-5)) - touchpads[1].UpdateState(true, timeAfterEnd(-2)) - touchpads[2].UpdateState(true, timeAfterEnd(-0.1)) + touchpads[0].UpdateState(true, matchStartTime, timeAfterEnd(-5)) + touchpads[1].UpdateState(true, matchStartTime, timeAfterEnd(-2)) + touchpads[2].UpdateState(true, matchStartTime, timeAfterEnd(-0.1)) - assert.Equal(t, 0, CountTouchpads(&touchpads, matchStartTime, timeAfterEnd(-6))) - assert.Equal(t, 0, CountTouchpads(&touchpads, matchStartTime, timeAfterEnd(-5.5))) - assert.Equal(t, 1, CountTouchpads(&touchpads, matchStartTime, timeAfterEnd(-3))) - assert.Equal(t, 1, CountTouchpads(&touchpads, matchStartTime, timeAfterEnd(-1.5))) - assert.Equal(t, 2, CountTouchpads(&touchpads, matchStartTime, timeAfterEnd(0))) - assert.Equal(t, 3, CountTouchpads(&touchpads, matchStartTime, timeAfterEnd(1))) + assert.Equal(t, 0, CountTouchpads(&touchpads, timeAfterEnd(-6))) + assert.Equal(t, 0, CountTouchpads(&touchpads, timeAfterEnd(-5.5))) + assert.Equal(t, 1, CountTouchpads(&touchpads, timeAfterEnd(-3))) + assert.Equal(t, 1, CountTouchpads(&touchpads, timeAfterEnd(-1.5))) + assert.Equal(t, 2, CountTouchpads(&touchpads, timeAfterEnd(0))) + assert.Equal(t, 3, CountTouchpads(&touchpads, timeAfterEnd(1))) }