Skip to content

Commit 2933cca

Browse files
ThadHouseThadHouse
committed
Add the rest of the robot bases
1 parent 8bc9c58 commit 2933cca

File tree

5 files changed

+537
-1
lines changed

5 files changed

+537
-1
lines changed

‎src/wpilibsharp/DSControlWord.cs

+98
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
using WPIHal;
2+
3+
namespace WPILib;
4+
5+
public struct DSControlWord
6+
{
7+
private ControlWord m_controlWord;
8+
9+
public DSControlWord()
10+
{
11+
Refresh();
12+
}
13+
14+
public void Refresh()
15+
{
16+
m_controlWord = DriverStation.GetControlWordFromCache();
17+
}
18+
19+
/**
20+
* Gets a value indicating whether the Driver Station requires the robot to be enabled.
21+
*
22+
* @return True if the robot is enabled, false otherwise.
23+
*/
24+
public readonly bool IsEnabled => m_controlWord.Enabled && m_controlWord.DsAttached;
25+
26+
/**
27+
* Gets a value indicating whether the Driver Station requires the robot to be disabled.
28+
*
29+
* @return True if the robot should be disabled, false otherwise.
30+
*/
31+
public readonly bool IsDisabled => !IsEnabled;
32+
33+
/**
34+
* Gets a value indicating whether the Robot is e-stopped.
35+
*
36+
* @return True if the robot is e-stopped, false otherwise.
37+
*/
38+
public readonly bool IsEStopped => m_controlWord.EStop;
39+
40+
/**
41+
* Gets a value indicating whether the Driver Station requires the robot to be running in
42+
* autonomous mode.
43+
*
44+
* @return True if autonomous mode should be enabled, false otherwise.
45+
*/
46+
public readonly bool IsAutonomous => m_controlWord.Autonomous;
47+
48+
/**
49+
* Gets a value indicating whether the Driver Station requires the robot to be running in
50+
* autonomous mode and enabled.
51+
*
52+
* @return True if autonomous should be set and the robot should be enabled.
53+
*/
54+
public readonly bool IsAutonomousEnabled => m_controlWord.Autonomous
55+
&& m_controlWord.Enabled
56+
&& m_controlWord.DsAttached;
57+
58+
/**
59+
* Gets a value indicating whether the Driver Station requires the robot to be running in
60+
* operator-controlled mode.
61+
*
62+
* @return True if operator-controlled mode should be enabled, false otherwise.
63+
*/
64+
public readonly bool IsTeleop => !(IsAutonomous || IsTest);
65+
66+
/**
67+
* Gets a value indicating whether the Driver Station requires the robot to be running in
68+
* operator-controller mode and enabled.
69+
*
70+
* @return True if operator-controlled mode should be set and the robot should be enabled.
71+
*/
72+
public readonly bool IsTeleopEnabled => !m_controlWord.Autonomous
73+
&& !m_controlWord.Test
74+
&& m_controlWord.Enabled
75+
&& m_controlWord.DsAttached;
76+
77+
/**
78+
* Gets a value indicating whether the Driver Station requires the robot to be running in test
79+
* mode.
80+
*
81+
* @return True if test mode should be enabled, false otherwise.
82+
*/
83+
public readonly bool IsTest => m_controlWord.Test;
84+
85+
/**
86+
* Gets a value indicating whether the Driver Station is attached.
87+
*
88+
* @return True if Driver Station is attached, false otherwise.
89+
*/
90+
public readonly bool IsDSAttached => m_controlWord.DsAttached;
91+
92+
/**
93+
* Gets if the driver station attached to a Field Management System.
94+
*
95+
* @return true if the robot is competing on a field being controlled by a Field Management System
96+
*/
97+
public readonly bool IsFMSAttached => m_controlWord.FmsAttached;
98+
}

‎src/wpilibsharp/IterativeRobotBase.cs

+300
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,300 @@
1+
using CommunityToolkit.Diagnostics;
2+
using NetworkTables;
3+
using UnitsNet;
4+
using WPIHal;
5+
using WPIHal.Natives;
6+
7+
namespace WPILib;
8+
9+
public abstract class IterativeRobotBase : RobotBase
10+
{
11+
private enum Mode
12+
{
13+
None,
14+
Disabled,
15+
Autonomous,
16+
Teleop,
17+
Test
18+
}
19+
20+
private DSControlWord m_word = new();
21+
private Mode m_lastMode = Mode.None;
22+
private readonly Watchdog m_watchdog;
23+
private bool m_calledDsConnected;
24+
25+
protected IterativeRobotBase(Duration period)
26+
{
27+
Period = period;
28+
m_watchdog = new(period, PrintLoopOverrunMessage);
29+
}
30+
31+
public virtual void RobotInit() { }
32+
33+
public virtual void DriverStationConnected() { }
34+
35+
public virtual void SimulationInit() { }
36+
37+
public virtual void DisabledInit() { }
38+
39+
public virtual void AutonomousInit() { }
40+
41+
public virtual void TeleopInit() { }
42+
43+
public virtual void TestInit() { }
44+
45+
private bool m_rpFirstRun = true;
46+
47+
/** Periodic code for all robot modes should go here. */
48+
public virtual void RobotPeriodic()
49+
{
50+
if (m_rpFirstRun)
51+
{
52+
Console.WriteLine("Default robotPeriodic() method... Override me!");
53+
m_rpFirstRun = false;
54+
}
55+
}
56+
57+
private bool m_spFirstRun = true;
58+
59+
public virtual void SimulationPeriodic()
60+
{
61+
if (m_spFirstRun)
62+
{
63+
Console.WriteLine("Default simulationPeriodic() method... Override me!");
64+
m_spFirstRun = false;
65+
}
66+
}
67+
68+
private bool m_dpFirstRun = true;
69+
70+
/** Periodic code for disabled mode should go here. */
71+
public virtual void DisabledPeriodic()
72+
{
73+
if (m_dpFirstRun)
74+
{
75+
Console.WriteLine("Default disabledPeriodic() method... Override me!");
76+
m_dpFirstRun = false;
77+
}
78+
}
79+
80+
private bool m_apFirstRun = true;
81+
82+
/** Periodic code for autonomous mode should go here. */
83+
public virtual void AutonomousPeriodic()
84+
{
85+
if (m_apFirstRun)
86+
{
87+
Console.WriteLine("Default autonomousPeriodic() method... Override me!");
88+
m_apFirstRun = false;
89+
}
90+
}
91+
92+
private bool m_tpFirstRun = true;
93+
94+
/** Periodic code for teleop mode should go here. */
95+
public virtual void TeleopPeriodic()
96+
{
97+
if (m_tpFirstRun)
98+
{
99+
Console.WriteLine("Default teleopPeriodic() method... Override me!");
100+
m_tpFirstRun = false;
101+
}
102+
}
103+
104+
private bool m_tmpFirstRun = true;
105+
106+
/** Periodic code for test mode should go here. */
107+
public virtual void TestPeriodic()
108+
{
109+
if (m_tmpFirstRun)
110+
{
111+
Console.WriteLine("Default testPeriodic() method... Override me!");
112+
m_tmpFirstRun = false;
113+
}
114+
}
115+
116+
public virtual void DisabledExit() { }
117+
118+
public virtual void AutonomousExit() { }
119+
120+
public virtual void TeleopExit() { }
121+
122+
public virtual void TestExit() { }
123+
124+
public bool NetworkTablesFlushEnabled { get; set; }
125+
126+
private bool m_reportedLw;
127+
private bool m_lwEnabledInTest;
128+
129+
public bool EnableLiveWindowInTest
130+
{
131+
get => m_lwEnabledInTest;
132+
set
133+
{
134+
if (IsTestEnabled)
135+
{
136+
ThrowHelper.ThrowInvalidOperationException("Can't configure test mode while in test mode!");
137+
}
138+
if (!m_reportedLw && value)
139+
{
140+
// TODO HAL Report
141+
m_reportedLw = true;
142+
}
143+
m_lwEnabledInTest = value;
144+
}
145+
}
146+
147+
public Duration Period { get; }
148+
149+
protected void LoopFunc()
150+
{
151+
DriverStation.RefreshData();
152+
m_watchdog.Reset();
153+
154+
m_word.Refresh();
155+
156+
// Get current mode
157+
Mode mode = Mode.None;
158+
if (!m_word.IsDisabled)
159+
{
160+
mode = Mode.Disabled;
161+
}
162+
else if (m_word.IsAutonomous)
163+
{
164+
mode = Mode.Autonomous;
165+
}
166+
else if (m_word.IsTeleop)
167+
{
168+
mode = Mode.Teleop;
169+
}
170+
else if (m_word.IsTest)
171+
{
172+
mode = Mode.Test;
173+
}
174+
175+
if (!m_calledDsConnected && m_word.IsDSAttached)
176+
{
177+
m_calledDsConnected = true;
178+
DriverStationConnected();
179+
}
180+
181+
// If mode changed, call mode exit and entry functions
182+
if (m_lastMode != mode)
183+
{
184+
// Call last mode's exit function
185+
if (m_lastMode == Mode.Disabled)
186+
{
187+
DisabledExit();
188+
}
189+
else if (m_lastMode == Mode.Autonomous)
190+
{
191+
AutonomousExit();
192+
}
193+
else if (m_lastMode == Mode.Teleop)
194+
{
195+
TeleopExit();
196+
}
197+
else if (m_lastMode == Mode.Test)
198+
{
199+
if (m_lwEnabledInTest)
200+
{
201+
//LiveWindow.setEnabled(false);
202+
//Shuffleboard.disableActuatorWidgets();
203+
}
204+
TestExit();
205+
}
206+
207+
// Call current mode's entry function
208+
if (mode == Mode.Disabled)
209+
{
210+
DisabledInit();
211+
m_watchdog.AddEpoch("disabledInit()");
212+
}
213+
else if (mode == Mode.Autonomous)
214+
{
215+
AutonomousInit();
216+
m_watchdog.AddEpoch("autonomousInit()");
217+
}
218+
else if (mode == Mode.Teleop)
219+
{
220+
TeleopInit();
221+
m_watchdog.AddEpoch("teleopInit()");
222+
}
223+
else if (mode == Mode.Test)
224+
{
225+
if (m_lwEnabledInTest)
226+
{
227+
// LiveWindow.setEnabled(true);
228+
// Shuffleboard.enableActuatorWidgets();
229+
}
230+
TestInit();
231+
m_watchdog.AddEpoch("testInit()");
232+
}
233+
234+
m_lastMode = mode;
235+
}
236+
237+
// Call the appropriate function depending upon the current robot mode
238+
if (mode == Mode.Disabled)
239+
{
240+
HalDriverStation.ObserveUserProgramDisabled();
241+
DisabledPeriodic();
242+
m_watchdog.AddEpoch("disabledPeriodic()");
243+
}
244+
else if (mode == Mode.Autonomous)
245+
{
246+
HalDriverStation.ObserveUserProgramAutonomous();
247+
AutonomousPeriodic();
248+
m_watchdog.AddEpoch("autonomousPeriodic()");
249+
}
250+
else if (mode == Mode.Teleop)
251+
{
252+
HalDriverStation.ObserveUserProgramTeleop();
253+
TeleopPeriodic();
254+
m_watchdog.AddEpoch("teleopPeriodic()");
255+
}
256+
else
257+
{
258+
HalDriverStation.ObserveUserProgramTest();
259+
TestPeriodic();
260+
m_watchdog.AddEpoch("testPeriodic()");
261+
}
262+
263+
RobotPeriodic();
264+
m_watchdog.AddEpoch("robotPeriodic()");
265+
266+
//SmartDashboard.updateValues();
267+
m_watchdog.AddEpoch("SmartDashboard.updateValues()");
268+
//LiveWindow.updateValues();
269+
m_watchdog.AddEpoch("LiveWindow.updateValues()");
270+
//Shuffleboard.update();
271+
m_watchdog.AddEpoch("Shuffleboard.update()");
272+
273+
if (IsSimulation)
274+
{
275+
HalBase.SimPeriodicBefore();
276+
SimulationPeriodic();
277+
HalBase.SimPeriodicAfter();
278+
m_watchdog.AddEpoch("simulationPeriodic()");
279+
}
280+
281+
m_watchdog.Disable();
282+
283+
// Flush NetworkTables
284+
if (NetworkTablesFlushEnabled)
285+
{
286+
NetworkTableInstance.Default.FlushLocal();
287+
}
288+
289+
// Warn on loop time overruns
290+
if (m_watchdog.IsExpired)
291+
{
292+
m_watchdog.PrintEpochs();
293+
}
294+
}
295+
296+
private void PrintLoopOverrunMessage()
297+
{
298+
DriverStation.ReportWarning($"Loop time of {Period}s overrun\n", false);
299+
}
300+
}

0 commit comments

Comments
 (0)