Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / aos / 1e69f94f38ed7f49e02ec69438ce4325cd172c0d / . / wpilibc / src / test / native / cpp / TimedRobotTest.cpp
blob: f91efb84a4c04fcb457930e0cd45438007796382 [file] [log] [blame]
Austin Schuh1e69f942020-11-14 15:06:14 -0800[diff] [blame^]1/*----------------------------------------------------------------------------*/
2/* Copyright (c) 2020 FIRST. All Rights Reserved. */
3/* Open Source Software - may be modified and shared by FRC teams. The code */
4/* must be accompanied by the FIRST BSD license file in the root directory of */
5/* the project. */
6/*----------------------------------------------------------------------------*/
7
8#include "frc/TimedRobot.h" // NOLINT(build/include_order)
9
10#include <stdint.h>
11
12#include <atomic>
13#include <thread>
14
15#include "frc/simulation/DriverStationSim.h"
16#include "frc/simulation/SimHooks.h"
17#include "gtest/gtest.h"
18
19using namespace frc;
20
21namespace {
22class TimedRobotTest : public ::testing::Test {
23 protected:
24 void SetUp() override { frc::sim::PauseTiming(); }
25
26 void TearDown() override { frc::sim::ResumeTiming(); }
27};
28
29class MockRobot : public TimedRobot {
30 public:
31 std::atomic<uint32_t> m_robotInitCount{0};
32 std::atomic<uint32_t> m_simulationInitCount{0};
33 std::atomic<uint32_t> m_disabledInitCount{0};
34 std::atomic<uint32_t> m_autonomousInitCount{0};
35 std::atomic<uint32_t> m_teleopInitCount{0};
36 std::atomic<uint32_t> m_testInitCount{0};
37
38 std::atomic<uint32_t> m_robotPeriodicCount{0};
39 std::atomic<uint32_t> m_simulationPeriodicCount{0};
40 std::atomic<uint32_t> m_disabledPeriodicCount{0};
41 std::atomic<uint32_t> m_autonomousPeriodicCount{0};
42 std::atomic<uint32_t> m_teleopPeriodicCount{0};
43 std::atomic<uint32_t> m_testPeriodicCount{0};
44
45 void RobotInit() override { m_robotInitCount++; }
46
47 void SimulationInit() override { m_simulationInitCount++; }
48
49 void DisabledInit() override { m_disabledInitCount++; }
50
51 void AutonomousInit() override { m_autonomousInitCount++; }
52
53 void TeleopInit() override { m_teleopInitCount++; }
54
55 void TestInit() override { m_testInitCount++; }
56
57 void RobotPeriodic() override { m_robotPeriodicCount++; }
58
59 void SimulationPeriodic() override { m_simulationPeriodicCount++; }
60
61 void DisabledPeriodic() override { m_disabledPeriodicCount++; }
62
63 void AutonomousPeriodic() override { m_autonomousPeriodicCount++; }
64
65 void TeleopPeriodic() override { m_teleopPeriodicCount++; }
66
67 void TestPeriodic() override { m_testPeriodicCount++; }
68};
69} // namespace
70
71TEST_F(TimedRobotTest, Disabled) {
72 MockRobot robot;
73
74 std::thread robotThread{[&] { robot.StartCompetition(); }};
75
76 frc::sim::DriverStationSim::SetEnabled(false);
77 frc::sim::DriverStationSim::NotifyNewData();
78 frc::sim::StepTiming(0_ms); // Wait for Notifiers
79
80 EXPECT_EQ(1u, robot.m_robotInitCount);
81 EXPECT_EQ(1u, robot.m_simulationInitCount);
82 EXPECT_EQ(0u, robot.m_disabledInitCount);
83 EXPECT_EQ(0u, robot.m_autonomousInitCount);
84 EXPECT_EQ(0u, robot.m_teleopInitCount);
85 EXPECT_EQ(0u, robot.m_testInitCount);
86
87 EXPECT_EQ(0u, robot.m_robotPeriodicCount);
88 EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
89 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
90 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
91 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
92 EXPECT_EQ(0u, robot.m_testPeriodicCount);
93
94 frc::sim::StepTiming(20_ms);
95
96 EXPECT_EQ(1u, robot.m_robotInitCount);
97 EXPECT_EQ(1u, robot.m_simulationInitCount);
98 EXPECT_EQ(1u, robot.m_disabledInitCount);
99 EXPECT_EQ(0u, robot.m_autonomousInitCount);
100 EXPECT_EQ(0u, robot.m_teleopInitCount);
101 EXPECT_EQ(0u, robot.m_testInitCount);
102
103 EXPECT_EQ(1u, robot.m_robotPeriodicCount);
104 EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
105 EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
106 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
107 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
108 EXPECT_EQ(0u, robot.m_testPeriodicCount);
109
110 frc::sim::StepTiming(20_ms);
111
112 EXPECT_EQ(1u, robot.m_robotInitCount);
113 EXPECT_EQ(1u, robot.m_simulationInitCount);
114 EXPECT_EQ(1u, robot.m_disabledInitCount);
115 EXPECT_EQ(0u, robot.m_autonomousInitCount);
116 EXPECT_EQ(0u, robot.m_teleopInitCount);
117 EXPECT_EQ(0u, robot.m_testInitCount);
118
119 EXPECT_EQ(2u, robot.m_robotPeriodicCount);
120 EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
121 EXPECT_EQ(2u, robot.m_disabledPeriodicCount);
122 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
123 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
124 EXPECT_EQ(0u, robot.m_testPeriodicCount);
125
126 robot.EndCompetition();
127 robotThread.join();
128}
129
130TEST_F(TimedRobotTest, Autonomous) {
131 MockRobot robot;
132
133 std::thread robotThread{[&] { robot.StartCompetition(); }};
134
135 frc::sim::DriverStationSim::SetEnabled(true);
136 frc::sim::DriverStationSim::SetAutonomous(true);
137 frc::sim::DriverStationSim::SetTest(false);
138 frc::sim::DriverStationSim::NotifyNewData();
139 frc::sim::StepTiming(0_ms); // Wait for Notifiers
140
141 EXPECT_EQ(1u, robot.m_robotInitCount);
142 EXPECT_EQ(1u, robot.m_simulationInitCount);
143 EXPECT_EQ(0u, robot.m_disabledInitCount);
144 EXPECT_EQ(0u, robot.m_autonomousInitCount);
145 EXPECT_EQ(0u, robot.m_teleopInitCount);
146 EXPECT_EQ(0u, robot.m_testInitCount);
147
148 EXPECT_EQ(0u, robot.m_robotPeriodicCount);
149 EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
150 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
151 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
152 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
153 EXPECT_EQ(0u, robot.m_testPeriodicCount);
154
155 frc::sim::StepTiming(20_ms);
156
157 EXPECT_EQ(1u, robot.m_robotInitCount);
158 EXPECT_EQ(1u, robot.m_simulationInitCount);
159 EXPECT_EQ(0u, robot.m_disabledInitCount);
160 EXPECT_EQ(1u, robot.m_autonomousInitCount);
161 EXPECT_EQ(0u, robot.m_teleopInitCount);
162 EXPECT_EQ(0u, robot.m_testInitCount);
163
164 EXPECT_EQ(1u, robot.m_robotPeriodicCount);
165 EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
166 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
167 EXPECT_EQ(1u, robot.m_autonomousPeriodicCount);
168 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
169 EXPECT_EQ(0u, robot.m_testPeriodicCount);
170
171 frc::sim::StepTiming(20_ms);
172
173 EXPECT_EQ(1u, robot.m_robotInitCount);
174 EXPECT_EQ(1u, robot.m_simulationInitCount);
175 EXPECT_EQ(0u, robot.m_disabledInitCount);
176 EXPECT_EQ(1u, robot.m_autonomousInitCount);
177 EXPECT_EQ(0u, robot.m_teleopInitCount);
178 EXPECT_EQ(0u, robot.m_testInitCount);
179
180 EXPECT_EQ(2u, robot.m_robotPeriodicCount);
181 EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
182 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
183 EXPECT_EQ(2u, robot.m_autonomousPeriodicCount);
184 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
185 EXPECT_EQ(0u, robot.m_testPeriodicCount);
186
187 robot.EndCompetition();
188 robotThread.join();
189}
190
191TEST_F(TimedRobotTest, Teleop) {
192 MockRobot robot;
193
194 std::thread robotThread{[&] { robot.StartCompetition(); }};
195
196 frc::sim::DriverStationSim::SetEnabled(true);
197 frc::sim::DriverStationSim::SetAutonomous(false);
198 frc::sim::DriverStationSim::SetTest(false);
199 frc::sim::DriverStationSim::NotifyNewData();
200 frc::sim::StepTiming(0_ms); // Wait for Notifiers
201
202 EXPECT_EQ(1u, robot.m_robotInitCount);
203 EXPECT_EQ(1u, robot.m_simulationInitCount);
204 EXPECT_EQ(0u, robot.m_disabledInitCount);
205 EXPECT_EQ(0u, robot.m_autonomousInitCount);
206 EXPECT_EQ(0u, robot.m_teleopInitCount);
207 EXPECT_EQ(0u, robot.m_testInitCount);
208
209 EXPECT_EQ(0u, robot.m_robotPeriodicCount);
210 EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
211 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
212 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
213 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
214 EXPECT_EQ(0u, robot.m_testPeriodicCount);
215
216 frc::sim::StepTiming(20_ms);
217
218 EXPECT_EQ(1u, robot.m_robotInitCount);
219 EXPECT_EQ(1u, robot.m_simulationInitCount);
220 EXPECT_EQ(0u, robot.m_disabledInitCount);
221 EXPECT_EQ(0u, robot.m_autonomousInitCount);
222 EXPECT_EQ(1u, robot.m_teleopInitCount);
223 EXPECT_EQ(0u, robot.m_testInitCount);
224
225 EXPECT_EQ(1u, robot.m_robotPeriodicCount);
226 EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
227 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
228 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
229 EXPECT_EQ(1u, robot.m_teleopPeriodicCount);
230 EXPECT_EQ(0u, robot.m_testPeriodicCount);
231
232 frc::sim::StepTiming(20_ms);
233
234 EXPECT_EQ(1u, robot.m_robotInitCount);
235 EXPECT_EQ(1u, robot.m_simulationInitCount);
236 EXPECT_EQ(0u, robot.m_disabledInitCount);
237 EXPECT_EQ(0u, robot.m_autonomousInitCount);
238 EXPECT_EQ(1u, robot.m_teleopInitCount);
239 EXPECT_EQ(0u, robot.m_testInitCount);
240
241 EXPECT_EQ(2u, robot.m_robotPeriodicCount);
242 EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
243 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
244 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
245 EXPECT_EQ(2u, robot.m_teleopPeriodicCount);
246 EXPECT_EQ(0u, robot.m_testPeriodicCount);
247
248 robot.EndCompetition();
249 robotThread.join();
250}
251
252TEST_F(TimedRobotTest, Test) {
253 MockRobot robot;
254
255 std::thread robotThread{[&] { robot.StartCompetition(); }};
256
257 frc::sim::DriverStationSim::SetEnabled(true);
258 frc::sim::DriverStationSim::SetAutonomous(false);
259 frc::sim::DriverStationSim::SetTest(true);
260 frc::sim::DriverStationSim::NotifyNewData();
261 frc::sim::StepTiming(0_ms); // Wait for Notifiers
262
263 EXPECT_EQ(1u, robot.m_robotInitCount);
264 EXPECT_EQ(1u, robot.m_simulationInitCount);
265 EXPECT_EQ(0u, robot.m_disabledInitCount);
266 EXPECT_EQ(0u, robot.m_autonomousInitCount);
267 EXPECT_EQ(0u, robot.m_teleopInitCount);
268 EXPECT_EQ(0u, robot.m_testInitCount);
269
270 EXPECT_EQ(0u, robot.m_robotPeriodicCount);
271 EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
272 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
273 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
274 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
275 EXPECT_EQ(0u, robot.m_testPeriodicCount);
276
277 frc::sim::StepTiming(20_ms);
278
279 EXPECT_EQ(1u, robot.m_robotInitCount);
280 EXPECT_EQ(1u, robot.m_simulationInitCount);
281 EXPECT_EQ(0u, robot.m_disabledInitCount);
282 EXPECT_EQ(0u, robot.m_autonomousInitCount);
283 EXPECT_EQ(0u, robot.m_teleopInitCount);
284 EXPECT_EQ(1u, robot.m_testInitCount);
285
286 EXPECT_EQ(1u, robot.m_robotPeriodicCount);
287 EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
288 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
289 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
290 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
291 EXPECT_EQ(1u, robot.m_testPeriodicCount);
292
293 frc::sim::StepTiming(20_ms);
294
295 EXPECT_EQ(1u, robot.m_robotInitCount);
296 EXPECT_EQ(1u, robot.m_simulationInitCount);
297 EXPECT_EQ(0u, robot.m_disabledInitCount);
298 EXPECT_EQ(0u, robot.m_autonomousInitCount);
299 EXPECT_EQ(0u, robot.m_teleopInitCount);
300 EXPECT_EQ(1u, robot.m_testInitCount);
301
302 EXPECT_EQ(2u, robot.m_robotPeriodicCount);
303 EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
304 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
305 EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
306 EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
307 EXPECT_EQ(2u, robot.m_testPeriodicCount);
308
309 robot.EndCompetition();
310 robotThread.join();
311}
312
313TEST_F(TimedRobotTest, AddPeriodic) {
314 MockRobot robot;
315
316 std::atomic<uint32_t> callbackCount{0};
317 robot.AddPeriodic([&] { callbackCount++; }, 10_ms);
318
319 std::thread robotThread{[&] { robot.StartCompetition(); }};
320
321 frc::sim::DriverStationSim::SetEnabled(false);
322 frc::sim::DriverStationSim::NotifyNewData();
323 frc::sim::StepTiming(0_ms); // Wait for Notifiers
324
325 EXPECT_EQ(0u, robot.m_disabledInitCount);
326 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
327 EXPECT_EQ(0u, callbackCount);
328
329 frc::sim::StepTiming(10_ms);
330
331 EXPECT_EQ(0u, robot.m_disabledInitCount);
332 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
333 EXPECT_EQ(1u, callbackCount);
334
335 frc::sim::StepTiming(10_ms);
336
337 EXPECT_EQ(1u, robot.m_disabledInitCount);
338 EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
339 EXPECT_EQ(2u, callbackCount);
340
341 robot.EndCompetition();
342 robotThread.join();
343}
344
345TEST_F(TimedRobotTest, AddPeriodicWithOffset) {
346 MockRobot robot;
347
348 std::atomic<uint32_t> callbackCount{0};
349 robot.AddPeriodic([&] { callbackCount++; }, 10_ms, 5_ms);
350
351 // Expirations in this test (ms)
352 //
353 // Robot | Callback
354 // ================
355 // 20 | 15
356 // 40 | 25
357
358 std::thread robotThread{[&] { robot.StartCompetition(); }};
359
360 frc::sim::DriverStationSim::SetEnabled(false);
361 frc::sim::DriverStationSim::NotifyNewData();
362 frc::sim::StepTiming(0_ms); // Wait for Notifiers
363
364 EXPECT_EQ(0u, robot.m_disabledInitCount);
365 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
366 EXPECT_EQ(0u, callbackCount);
367
368 frc::sim::StepTiming(7.5_ms);
369
370 EXPECT_EQ(0u, robot.m_disabledInitCount);
371 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
372 EXPECT_EQ(0u, callbackCount);
373
374 frc::sim::StepTiming(7.5_ms);
375
376 EXPECT_EQ(0u, robot.m_disabledInitCount);
377 EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
378 EXPECT_EQ(1u, callbackCount);
379
380 frc::sim::StepTiming(5_ms);
381
382 EXPECT_EQ(1u, robot.m_disabledInitCount);
383 EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
384 EXPECT_EQ(1u, callbackCount);
385
386 frc::sim::StepTiming(5_ms);
387
388 EXPECT_EQ(1u, robot.m_disabledInitCount);
389 EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
390 EXPECT_EQ(2u, callbackCount);
391
392 robot.EndCompetition();
393 robotThread.join();
394}