Offset rotor counts after the previous rotor has been triggered, to enforce in-order activation.

2026-03-09 21:56:50 -04:00 · 2017-09-14 23:10:05 -07:00
parent d9fd1a2e8e
commit e5270080f1
4 changed files with 97 additions and 61 deletions
--- a/field/arena.go
+++ b/field/arena.go
@@ -626,11 +626,11 @@ func (arena *Arena) handlePlcInput() {
 	blueScore.FuelHigh = arena.BlueRealtimeScore.boiler.FuelHigh

 	// Handle rotors.
-	redRotors, blueRotors := arena.Plc.GetRotors()
-	arena.RedRealtimeScore.rotorSet.UpdateState(redRotors, matchStartTime, currentTime)
+	redRotor1, redOtherRotors, blueRotor1, blueOtherRotors := arena.Plc.GetRotors()
+	arena.RedRealtimeScore.rotorSet.UpdateState(redRotor1, redOtherRotors, matchStartTime, currentTime)
 	redScore.AutoRotors = arena.RedRealtimeScore.rotorSet.AutoRotors
 	redScore.Rotors = arena.RedRealtimeScore.rotorSet.Rotors
-	arena.BlueRealtimeScore.rotorSet.UpdateState(blueRotors, matchStartTime, currentTime)
+	arena.BlueRealtimeScore.rotorSet.UpdateState(blueRotor1, blueOtherRotors, matchStartTime, currentTime)
 	blueScore.AutoRotors = arena.BlueRealtimeScore.rotorSet.AutoRotors
 	blueScore.Rotors = arena.BlueRealtimeScore.rotorSet.Rotors

--- a/field/plc.go
+++ b/field/plc.go
@@ -25,11 +25,10 @@ type Plc struct {
 }

 const (
-	modbusPort          = 502
-	rotorGearToothCount = 15
-	plcLoopPeriodMs     = 100
-	plcRetryIntevalSec  = 3
-	cycleCounterMax     = 96
+	modbusPort         = 502
+	plcLoopPeriodMs    = 100
+	plcRetryIntevalSec = 3
+	cycleCounterMax    = 96
 )

 // Discrete inputs
@@ -161,19 +160,17 @@ func (plc *Plc) GetBalls() (int, int, int, int) {
 }

 // Returns the state of red and blue activated rotors.
-func (plc *Plc) GetRotors() ([4]bool, [4]bool) {
-	var redRotors, blueRotors [4]bool
+func (plc *Plc) GetRotors() (bool, [3]int, bool, [3]int) {
+	var redOtherRotors, blueOtherRotors [3]int

-	redRotors[0] = plc.Inputs[redRotor1]
-	redRotors[1] = int(plc.Counters[redRotor2Count]) >= rotorGearToothCount
-	redRotors[2] = int(plc.Counters[redRotor3Count]) >= rotorGearToothCount
-	redRotors[3] = int(plc.Counters[redRotor4Count]) >= rotorGearToothCount
-	blueRotors[0] = plc.Inputs[blueRotor1]
-	blueRotors[1] = int(plc.Counters[blueRotor2Count]) >= rotorGearToothCount
-	blueRotors[2] = int(plc.Counters[blueRotor3Count]) >= rotorGearToothCount
-	blueRotors[3] = int(plc.Counters[blueRotor4Count]) >= rotorGearToothCount
+	redOtherRotors[0] = int(plc.Counters[redRotor2Count])
+	redOtherRotors[1] = int(plc.Counters[redRotor3Count])
+	redOtherRotors[2] = int(plc.Counters[redRotor4Count])
+	blueOtherRotors[0] = int(plc.Counters[blueRotor2Count])
+	blueOtherRotors[1] = int(plc.Counters[blueRotor3Count])
+	blueOtherRotors[2] = int(plc.Counters[blueRotor4Count])

-	return redRotors, blueRotors
+	return plc.Inputs[redRotor1], redOtherRotors, plc.Inputs[blueRotor1], blueOtherRotors
 }

 func (plc *Plc) GetTouchpads() ([3]bool, [3]bool) {
--- a/game/rotor_set.go
+++ b/game/rotor_set.go
@@ -9,36 +9,44 @@ import (
 	"time"
 )

+const rotorGearToothCount = 15
+
 type RotorSet struct {
-	AutoRotors int
-	Rotors     int
+	AutoRotors     int
+	Rotors         int
+	counterOffsets [3]int
 }

 // Updates the internal counting state of the rotors given the current state of the sensors.
-func (rotorSet *RotorSet) UpdateState(rotors [4]bool, matchStartTime, currentTime time.Time) {
+func (rotorSet *RotorSet) UpdateState(rotor1 bool, otherRotors [3]int, matchStartTime, currentTime time.Time) {
 	autoValidityCutoff := matchStartTime.Add(time.Duration(MatchTiming.AutoDurationSec) * time.Second)
 	teleopValidityCutoff := autoValidityCutoff.Add(time.Duration(MatchTiming.PauseDurationSec+
 		MatchTiming.TeleopDurationSec) * time.Second)

 	if currentTime.After(matchStartTime) {
 		if currentTime.Before(autoValidityCutoff) {
-			if rotorSet.AutoRotors == 0 && rotors[0] {
+			if rotorSet.AutoRotors == 0 && rotor1 {
 				rotorSet.AutoRotors++
+				rotorSet.counterOffsets[0] = otherRotors[0]
 			}
-			if rotorSet.AutoRotors == 1 && rotors[1] {
+			if rotorSet.AutoRotors == 1 && otherRotors[0]-rotorSet.counterOffsets[0] >= rotorGearToothCount {
 				rotorSet.AutoRotors++
+				rotorSet.counterOffsets[1] = otherRotors[1]
 			}
 		} else if currentTime.Before(teleopValidityCutoff) {
-			if rotorSet.totalRotors() == 0 && rotors[0] {
+			if rotorSet.totalRotors() == 0 && rotor1 {
 				rotorSet.Rotors++
+				rotorSet.counterOffsets[0] = otherRotors[0]
 			}
-			if rotorSet.totalRotors() == 1 && rotors[1] {
+			if rotorSet.totalRotors() == 1 && otherRotors[0]-rotorSet.counterOffsets[0] >= rotorGearToothCount {
 				rotorSet.Rotors++
+				rotorSet.counterOffsets[1] = otherRotors[1]
 			}
-			if rotorSet.totalRotors() == 2 && rotors[2] {
+			if rotorSet.totalRotors() == 2 && otherRotors[1]-rotorSet.counterOffsets[1] >= rotorGearToothCount {
 				rotorSet.Rotors++
+				rotorSet.counterOffsets[2] = otherRotors[2]
 			}
-			if rotorSet.totalRotors() == 3 && rotors[3] {
+			if rotorSet.totalRotors() == 3 && otherRotors[2]-rotorSet.counterOffsets[2] >= rotorGearToothCount {
 				rotorSet.Rotors++
 			}
 		}
--- a/game/rotor_set_test.go
+++ b/game/rotor_set_test.go
@@ -11,92 +11,123 @@ import (
 func TestRotorsBeforeMatch(t *testing.T) {
 	rotorSet := RotorSet{}

-	rotorSet.UpdateState([4]bool{true, true, true, false}, matchStartTime, timeAfterStart(-1))
+	rotorSet.UpdateState(true, [3]int{15, 15, 0}, matchStartTime, timeAfterStart(-1))
 	checkRotorCounts(t, 0, 0, &rotorSet)
 }

+func TestRotorCountThreshold(t *testing.T) {
+	rotorSet := RotorSet{}
+
+	rotorSet.UpdateState(true, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(20))
+	checkRotorCounts(t, 0, 1, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{10, 0, 0}, matchStartTime, timeAfterStart(20))
+	checkRotorCounts(t, 0, 1, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{14, 0, 0}, matchStartTime, timeAfterStart(21))
+	checkRotorCounts(t, 0, 1, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(22))
+	checkRotorCounts(t, 0, 2, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{15, 13, 0}, matchStartTime, timeAfterStart(23))
+	checkRotorCounts(t, 0, 2, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{15, 16, 0}, matchStartTime, timeAfterStart(24))
+	checkRotorCounts(t, 0, 3, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{15, 16, 50}, matchStartTime, timeAfterStart(25))
+	checkRotorCounts(t, 0, 4, &rotorSet)
+}
+
 func TestAutoRotors(t *testing.T) {
 	rotorSet := RotorSet{}

-	rotorSet.UpdateState([4]bool{false, false, false, false}, matchStartTime, timeAfterStart(1))
+	rotorSet.UpdateState(false, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(1))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, true, true, true}, matchStartTime, timeAfterStart(1))
+	rotorSet.UpdateState(false, [3]int{15, 15, 15}, matchStartTime, timeAfterStart(1))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, false, false, false}, matchStartTime, timeAfterStart(1))
+	rotorSet.UpdateState(true, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(1))
 	checkRotorCounts(t, 1, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, false, false}, matchStartTime, timeAfterStart(5))
+	rotorSet.UpdateState(true, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(5))
 	checkRotorCounts(t, 2, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, true, false}, matchStartTime, timeAfterStart(11))
+	rotorSet.UpdateState(true, [3]int{15, 15, 0}, matchStartTime, timeAfterStart(11))
 	checkRotorCounts(t, 2, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, true, false}, matchStartTime, timeAfterStart(20))
+	rotorSet.UpdateState(true, [3]int{15, 15, 0}, matchStartTime, timeAfterStart(20))
 	checkRotorCounts(t, 2, 1, &rotorSet)

-	// Check going straight to two.
-	rotorSet = RotorSet{}
-	rotorSet.UpdateState([4]bool{true, true, false, false}, matchStartTime, timeAfterStart(5))
-	checkRotorCounts(t, 2, 0, &rotorSet)
-
 	// Check timing threshold.
 	rotorSet = RotorSet{}
-	rotorSet.UpdateState([4]bool{true, false, false, false}, matchStartTime, timeAfterStart(5))
+	rotorSet.UpdateState(true, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(5))
 	checkRotorCounts(t, 1, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, false, false}, matchStartTime, timeAfterStart(15.1))
+	rotorSet.UpdateState(true, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(15.1))
 	checkRotorCounts(t, 1, 1, &rotorSet)
 }

 func TestTeleopRotors(t *testing.T) {
 	rotorSet := RotorSet{}

-	rotorSet.UpdateState([4]bool{false, false, false, false}, matchStartTime, timeAfterStart(14))
+	rotorSet.UpdateState(false, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(14))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, false, false, false}, matchStartTime, timeAfterStart(20))
+	rotorSet.UpdateState(true, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(20))
 	checkRotorCounts(t, 0, 1, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, false, false}, matchStartTime, timeAfterStart(30))
+	rotorSet.UpdateState(true, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(30))
 	checkRotorCounts(t, 0, 2, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, true, false}, matchStartTime, timeAfterStart(100))
+	rotorSet.UpdateState(true, [3]int{15, 15, 0}, matchStartTime, timeAfterStart(100))
 	checkRotorCounts(t, 0, 3, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, true, true}, matchStartTime, timeAfterStart(120))
+	rotorSet.UpdateState(true, [3]int{15, 15, 15}, matchStartTime, timeAfterStart(120))
 	checkRotorCounts(t, 0, 4, &rotorSet)
 }

 func TestRotorsAfterMatch(t *testing.T) {
 	rotorSet := RotorSet{}

-	rotorSet.UpdateState([4]bool{true, false, false, false}, matchStartTime, timeAfterEnd(1))
+	rotorSet.UpdateState(true, [3]int{0, 0, 0}, matchStartTime, timeAfterEnd(1))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, false, false}, matchStartTime, timeAfterEnd(2))
+	rotorSet.UpdateState(true, [3]int{15, 0, 0}, matchStartTime, timeAfterEnd(2))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, true, false}, matchStartTime, timeAfterEnd(3))
+	rotorSet.UpdateState(true, [3]int{15, 15, 0}, matchStartTime, timeAfterEnd(3))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, true, true}, matchStartTime, timeAfterEnd(4))
+	rotorSet.UpdateState(true, [3]int{15, 15, 15}, matchStartTime, timeAfterEnd(4))
 	checkRotorCounts(t, 0, 0, &rotorSet)
 }

 func TestRotorLatching(t *testing.T) {
 	rotorSet := RotorSet{}

-	rotorSet.UpdateState([4]bool{false, true, false, false}, matchStartTime, timeAfterStart(1))
+	rotorSet.UpdateState(false, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(1))
 	checkRotorCounts(t, 0, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, false, false, false}, matchStartTime, timeAfterStart(2))
+	rotorSet.UpdateState(true, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(2))
 	checkRotorCounts(t, 1, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, false, false, false}, matchStartTime, timeAfterStart(5))
+	rotorSet.UpdateState(false, [3]int{0, 0, 0}, matchStartTime, timeAfterStart(5))
 	checkRotorCounts(t, 1, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, true, false, false}, matchStartTime, timeAfterStart(10))
+	rotorSet.UpdateState(false, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(10))
 	checkRotorCounts(t, 2, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{true, true, false, false}, matchStartTime, timeAfterStart(10))
+	rotorSet.UpdateState(true, [3]int{15, 0, 0}, matchStartTime, timeAfterStart(10))
 	checkRotorCounts(t, 2, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, false, false, true}, matchStartTime, timeAfterStart(20))
+	rotorSet.UpdateState(false, [3]int{0, 0, 15}, matchStartTime, timeAfterStart(20))
 	checkRotorCounts(t, 2, 0, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, false, true, false}, matchStartTime, timeAfterStart(30))
+	rotorSet.UpdateState(false, [3]int{0, 15, 0}, matchStartTime, timeAfterStart(30))
 	checkRotorCounts(t, 2, 1, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, false, false, true}, matchStartTime, timeAfterStart(50))
+	rotorSet.UpdateState(false, [3]int{0, 0, 15}, matchStartTime, timeAfterStart(50))
 	checkRotorCounts(t, 2, 2, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, false, false, false}, matchStartTime, timeAfterEnd(-1))
+	rotorSet.UpdateState(false, [3]int{0, 0, 0}, matchStartTime, timeAfterEnd(-1))
 	checkRotorCounts(t, 2, 2, &rotorSet)
-	rotorSet.UpdateState([4]bool{false, false, false, false}, matchStartTime, timeAfterEnd(1))
+	rotorSet.UpdateState(false, [3]int{0, 0, 0}, matchStartTime, timeAfterEnd(1))
 	checkRotorCounts(t, 2, 2, &rotorSet)
 }

+func TestRotorActivationOrder(t *testing.T) {
+	rotorSet := RotorSet{}
+
+	rotorSet.UpdateState(true, [3]int{0, 25, 50}, matchStartTime, timeAfterStart(20))
+	checkRotorCounts(t, 0, 1, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{20, 25, 50}, matchStartTime, timeAfterStart(21))
+	checkRotorCounts(t, 0, 2, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{20, 39, 50}, matchStartTime, timeAfterStart(22))
+	checkRotorCounts(t, 0, 2, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{20, 40, 70}, matchStartTime, timeAfterStart(23))
+	checkRotorCounts(t, 0, 3, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{20, 40, 84}, matchStartTime, timeAfterStart(24))
+	checkRotorCounts(t, 0, 3, &rotorSet)
+	rotorSet.UpdateState(true, [3]int{20, 40, 85}, matchStartTime, timeAfterStart(25))
+	checkRotorCounts(t, 0, 4, &rotorSet)
+}
+
 func checkRotorCounts(t *testing.T, autoRotors, rotors int, rotorSet *RotorSet) {
 	assert.Equal(t, autoRotors, rotorSet.AutoRotors)
 	assert.Equal(t, rotors, rotorSet.Rotors)