blob: 8ef19227172d591470e41c5956783b05303a6ba2 [file] [log] [blame]
Austin Schuh812d0d12021-11-04 20:16:48 -07001// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#include <cstdlib>
6#include <thread>
7
8#include <fmt/core.h>
James Kuszmaulb13e13f2023-11-22 20:44:04 -08009#include <gtest/gtest.h>
Austin Schuh812d0d12021-11-04 20:16:48 -070010#include <hal/HAL.h>
11
Austin Schuh812d0d12021-11-04 20:16:48 -070012#include "mockds/MockDS.h"
13
14using namespace std::chrono_literals;
15
16class TestEnvironment : public testing::Environment {
17 bool m_alreadySetUp = false;
18 MockDS m_mockDS;
19
20 public:
21 TestEnvironment() {
22 // Only set up once. This allows gtest_repeat to be used to automatically
23 // repeat tests.
24 if (m_alreadySetUp) {
25 return;
26 }
27 m_alreadySetUp = true;
28
29 if (!HAL_Initialize(500, 0)) {
30 fmt::print(stderr, "FATAL ERROR: HAL could not be initialized\n");
31 std::exit(-1);
32 }
33
34 m_mockDS.Start();
35
36 // This sets up the network communications library to enable the driver
37 // station. After starting network coms, it will loop until the driver
38 // station returns that the robot is enabled, to ensure that tests will be
39 // able to run on the hardware.
40 HAL_ObserveUserProgramStarting();
41
42 fmt::print("Started coms\n");
43
44 int enableCounter = 0;
45
46 auto checkEnabled = []() {
47 HAL_ControlWord controlWord;
48 std::memset(&controlWord, 0, sizeof(controlWord));
49 HAL_GetControlWord(&controlWord);
50 return controlWord.enabled && controlWord.dsAttached;
51 };
James Kuszmaulcf324122023-01-14 14:07:17 -080052 HAL_RefreshDSData();
Austin Schuh812d0d12021-11-04 20:16:48 -070053 while (!checkEnabled()) {
54 if (enableCounter > 50) {
55 // Robot did not enable properly after 5 seconds.
56 // Force exit
57 fmt::print(stderr, " Failed to enable. Aborting\n");
58 std::terminate();
59 }
60
61 std::this_thread::sleep_for(100ms);
62
63 fmt::print("Waiting for enable: {}\n", enableCounter++);
James Kuszmaulcf324122023-01-14 14:07:17 -080064 HAL_RefreshDSData();
Austin Schuh812d0d12021-11-04 20:16:48 -070065 }
66 std::this_thread::sleep_for(500ms);
67 }
68
69 ~TestEnvironment() override { m_mockDS.Stop(); }
70};
71
72testing::Environment* const environment =
73 testing::AddGlobalTestEnvironment(new TestEnvironment);