1+ // Copyright (c) 2025-2026 Littleton Robotics
2+ // http://github.com/Mechanical-Advantage
3+ //
4+
5+ // MIT License
6+
7+ // Copyright (c) 2025-2026 Littleton Robotics
8+
9+ // Permission is hereby granted, free of charge, to any person obtaining a copy
10+ // of this software and associated documentation files (the "Software"), to deal
11+ // in the Software without restriction, including without limitation the rights
12+ // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
13+ // copies of the Software, and to permit persons to whom the Software is
14+ // furnished to do so, subject to the following conditions:
15+
16+ // The above copyright notice and this permission notice shall be included in all
17+ // copies or substantial portions of the Software.
18+
19+ // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
20+ // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
21+ // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
22+ // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
23+ // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
24+ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
25+ // SOFTWARE.
26+
27+
28+ package frc .robot .util ;
29+
30+ import java .util .Optional ;
31+ import java .util .function .Supplier ;
32+
33+ import org .littletonrobotics .junction .AutoLogOutput ;
34+
35+ import edu .wpi .first .wpilibj .DriverStation ;
36+ import edu .wpi .first .wpilibj .DriverStation .Alliance ;
37+ import edu .wpi .first .wpilibj .Timer ;
38+
39+ public class HubShiftUtil {
40+ public enum ShiftEnum {
41+ TRANSITION ,
42+ SHIFT1 ,
43+ SHIFT2 ,
44+ SHIFT3 ,
45+ SHIFT4 ,
46+ ENDGAME ,
47+ AUTO ,
48+ DISABLED ;
49+ }
50+
51+ public record ShiftInfo (
52+ ShiftEnum currentShift , double elapsedTime , double remainingTime , boolean active ) {}
53+
54+ private static Timer shiftTimer = new Timer ();
55+ private static final ShiftEnum [] shiftsEnums = ShiftEnum .values ();
56+
57+ private static final double [] shiftStartTimes = {0.0 , 10.0 , 35.0 , 60.0 , 85.0 , 110.0 };
58+ private static final double [] shiftEndTimes = {10.0 , 35.0 , 60.0 , 85.0 , 110.0 , 140.0 };
59+
60+ // private static final double minFuelCountDelay = 1.0;
61+ // private static final double maxFuelCountDelay = 2.0;
62+ // private static final double shiftEndFuelCountExtension = 3.0;
63+
64+ public static final double autoEndTime = 20.0 ;
65+ public static final double teleopDuration = 140.0 ;
66+ private static final boolean [] activeSchedule = {true , true , false , true , false , true };
67+ private static final boolean [] inactiveSchedule = {true , false , true , false , true , true };
68+ private static final double timeResetThreshold = 1.0 ;
69+ private static double shiftTimerOffset = 0.0 ;
70+ private static Supplier <Optional <Boolean >> allianceWinOverride = () -> Optional .empty ();
71+
72+ public static Optional <Boolean > getAllianceWinOverride () {
73+ return allianceWinOverride .get ();
74+ }
75+
76+ public static void setAllianceWinOverride (Supplier <Optional <Boolean >> win ) {
77+ allianceWinOverride = win ;
78+ }
79+
80+ public static Alliance getFirstActiveAlliance () {
81+ var alliance = DriverStation .getAlliance ().orElse (Alliance .Blue );
82+
83+ // Return override value
84+ var winOverride = getAllianceWinOverride ();
85+ if (!winOverride .isEmpty ()) {
86+ return winOverride .get ()
87+ ? (alliance == Alliance .Blue ? Alliance .Red : Alliance .Blue )
88+ : (alliance == Alliance .Blue ? Alliance .Blue : Alliance .Red );
89+ }
90+
91+ // Return FMS value
92+ String message = DriverStation .getGameSpecificMessage ();
93+ if (message .length () > 0 ) {
94+ char character = message .charAt (0 );
95+ if (character == 'R' ) {
96+ return Alliance .Blue ;
97+ } else if (character == 'B' ) {
98+ return Alliance .Red ;
99+ }
100+ }
101+
102+ // Return default value
103+ return alliance == Alliance .Blue ? Alliance .Red : Alliance .Blue ;
104+ }
105+
106+ /** Starts the timer at the begining of teleop. */
107+ public static void initialize () {
108+ shiftTimerOffset = 0 ;
109+ shiftTimer .restart ();
110+ }
111+
112+ private static boolean [] getSchedule () {
113+ boolean [] currentSchedule ;
114+ Alliance startAlliance = getFirstActiveAlliance ();
115+ currentSchedule =
116+ startAlliance == DriverStation .getAlliance ().orElse (Alliance .Blue )
117+ ? activeSchedule
118+ : inactiveSchedule ;
119+ return currentSchedule ;
120+ }
121+
122+ @ AutoLogOutput
123+ private static ShiftInfo getShiftInfo (
124+ boolean [] currentSchedule , double [] shiftStartTimes , double [] shiftEndTimes ) {
125+ double timerValue = shiftTimer .get ();
126+ double currentTime = timerValue - shiftTimerOffset ;
127+ double stateTimeElapsed = currentTime ;
128+ double stateTimeRemaining = 0.0 ;
129+ boolean active = false ;
130+ ShiftEnum currentShift = ShiftEnum .DISABLED ;
131+ double fieldTeleopTime = 140.0 - DriverStation .getMatchTime ();
132+
133+ if (DriverStation .isAutonomousEnabled ()) {
134+ stateTimeElapsed = currentTime ;
135+ stateTimeRemaining = autoEndTime - currentTime ;
136+ active = true ;
137+ currentShift = ShiftEnum .AUTO ;
138+ } else if (DriverStation .isEnabled ()) {
139+ // Adjust the current offset if the time difference above the theshold
140+ if (Math .abs (fieldTeleopTime - currentTime ) >= timeResetThreshold
141+ && fieldTeleopTime <= 135
142+ && DriverStation .isFMSAttached ()) {
143+ // shiftTimerOffset += currentTime - fieldTeleopTime;
144+ // currentTime = timerValue + shiftTimerOffset;
145+
146+ shiftTimerOffset = timerValue - fieldTeleopTime ;
147+ currentTime = fieldTeleopTime ;
148+ }
149+ int currentShiftIndex = -1 ;
150+ for (int i = 0 ; i < shiftStartTimes .length ; i ++) {
151+ if (currentTime >= shiftStartTimes [i ] && currentTime < shiftEndTimes [i ]) {
152+ currentShiftIndex = i ;
153+ break ;
154+ }
155+ }
156+ if (currentShiftIndex < 0 ) {
157+ // After last shift, so assume endgame
158+ currentShiftIndex = shiftStartTimes .length - 1 ;
159+ }
160+
161+ // Calculate elapsed and remaining time in the current shift, ignoring combined shifts
162+ stateTimeElapsed = currentTime - shiftStartTimes [currentShiftIndex ];
163+ stateTimeRemaining = shiftEndTimes [currentShiftIndex ] - currentTime ;
164+
165+ // If the state is the same as the last shift, combine the elapsed time
166+ if (currentShiftIndex > 0 ) {
167+ if (currentSchedule [currentShiftIndex ] == currentSchedule [currentShiftIndex - 1 ]) {
168+ stateTimeElapsed = currentTime - shiftStartTimes [currentShiftIndex - 1 ];
169+ }
170+ }
171+
172+ // If the state is the same as the next shift, combine the remaining time
173+ if (currentShiftIndex < shiftEndTimes .length - 1 ) {
174+ if (currentSchedule [currentShiftIndex ] == currentSchedule [currentShiftIndex + 1 ]) {
175+ stateTimeRemaining = shiftEndTimes [currentShiftIndex + 1 ] - currentTime ;
176+ }
177+ }
178+
179+ active = currentSchedule [currentShiftIndex ];
180+ currentShift = shiftsEnums [currentShiftIndex ];
181+ }
182+ ShiftInfo shiftInfo = new ShiftInfo (currentShift , stateTimeElapsed , stateTimeRemaining , active );
183+ return shiftInfo ;
184+ }
185+
186+ public static ShiftInfo getOfficialShiftInfo () {
187+ return getShiftInfo (getSchedule (), shiftStartTimes , shiftEndTimes );
188+ }
189+
190+ // public static ShiftInfo getShiftedShiftInfo() {
191+ // boolean[] shiftSchedule = getSchedule();
192+ // // Starting active
193+ // if (shiftSchedule[1] == true) {
194+ // double[] shiftedShiftStartTimes = {
195+ // 0.0,
196+ // 10.0,
197+ // 35.0 + endingActiveFudge,
198+ // 60.0 + approachingActiveFudge,
199+ // 85.0 + endingActiveFudge,
200+ // 110.0 + approachingActiveFudge
201+ // };
202+ // double[] shiftedShiftEndTimes = {
203+ // 10.0,
204+ // 35.0 + endingActiveFudge,
205+ // 60.0 + approachingActiveFudge,
206+ // 85.0 + endingActiveFudge,
207+ // 110.0 + approachingActiveFudge,
208+ // 140.0
209+ // };
210+ // return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes);
211+ // }
212+ // double[] shiftedShiftStartTimes = {
213+ // 0.0,
214+ // 10.0 + endingActiveFudge,
215+ // 35.0 + approachingActiveFudge,
216+ // 60.0 + endingActiveFudge,
217+ // 85.0 + approachingActiveFudge,
218+ // 110.0
219+ // };
220+ // double[] shiftedShiftEndTimes = {
221+ // 10.0 + endingActiveFudge,
222+ // 35.0 + approachingActiveFudge,
223+ // 60.0 + endingActiveFudge,
224+ // 85.0 + approachingActiveFudge,
225+ // 110.0,
226+ // 140.0
227+ // };
228+ // return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes);
229+ // // }
230+ // }
231+ }
0 commit comments