blob: dbd0d1304ef61f185b64365569cdc1ddfa0218c5 [file] [log] [blame]
Brian Silverman8fce7482020-01-05 13:18:21 -08001/*----------------------------------------------------------------------------*/
Austin Schuh1e69f942020-11-14 15:06:14 -08002/* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */
Brian Silverman8fce7482020-01-05 13:18:21 -08003/* 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/Ultrasonic.h"
9
10#include <hal/FRCUsageReporting.h>
11
Austin Schuh1e69f942020-11-14 15:06:14 -080012#include "frc/Base.h"
Brian Silverman8fce7482020-01-05 13:18:21 -080013#include "frc/Counter.h"
14#include "frc/DigitalInput.h"
15#include "frc/DigitalOutput.h"
16#include "frc/Timer.h"
17#include "frc/Utility.h"
18#include "frc/WPIErrors.h"
19#include "frc/smartdashboard/SendableBuilder.h"
20#include "frc/smartdashboard/SendableRegistry.h"
21
22using namespace frc;
23
24// Automatic round robin mode
25std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
26
27std::vector<Ultrasonic*> Ultrasonic::m_sensors;
28std::thread Ultrasonic::m_thread;
29
30Ultrasonic::Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units)
31 : m_pingChannel(std::make_shared<DigitalOutput>(pingChannel)),
32 m_echoChannel(std::make_shared<DigitalInput>(echoChannel)),
33 m_counter(m_echoChannel) {
34 m_units = units;
35 Initialize();
36 auto& registry = SendableRegistry::GetInstance();
37 registry.AddChild(this, m_pingChannel.get());
38 registry.AddChild(this, m_echoChannel.get());
39}
40
41Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
42 DistanceUnit units)
43 : m_pingChannel(pingChannel, NullDeleter<DigitalOutput>()),
44 m_echoChannel(echoChannel, NullDeleter<DigitalInput>()),
45 m_counter(m_echoChannel) {
46 if (pingChannel == nullptr || echoChannel == nullptr) {
47 wpi_setWPIError(NullParameter);
48 m_units = units;
49 return;
50 }
51 m_units = units;
52 Initialize();
53}
54
55Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel,
56 DistanceUnit units)
57 : m_pingChannel(&pingChannel, NullDeleter<DigitalOutput>()),
58 m_echoChannel(&echoChannel, NullDeleter<DigitalInput>()),
59 m_counter(m_echoChannel) {
60 m_units = units;
61 Initialize();
62}
63
64Ultrasonic::Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
65 std::shared_ptr<DigitalInput> echoChannel,
66 DistanceUnit units)
67 : m_pingChannel(pingChannel),
68 m_echoChannel(echoChannel),
69 m_counter(m_echoChannel) {
70 m_units = units;
71 Initialize();
72}
73
74Ultrasonic::~Ultrasonic() {
75 // Delete the instance of the ultrasonic sensor by freeing the allocated
76 // digital channels. If the system was in automatic mode (round robin), then
77 // it is stopped, then started again after this sensor is removed (provided
78 // this wasn't the last sensor).
79
80 bool wasAutomaticMode = m_automaticEnabled;
81 SetAutomaticMode(false);
82
83 // No synchronization needed because the background task is stopped.
84 m_sensors.erase(std::remove(m_sensors.begin(), m_sensors.end(), this),
85 m_sensors.end());
86
87 if (!m_sensors.empty() && wasAutomaticMode) {
88 SetAutomaticMode(true);
89 }
90}
91
92void Ultrasonic::Ping() {
93 wpi_assert(!m_automaticEnabled);
94
95 // Reset the counter to zero (invalid data now)
96 m_counter.Reset();
97
98 // Do the ping to start getting a single range
99 m_pingChannel->Pulse(kPingTime);
100}
101
102bool Ultrasonic::IsRangeValid() const {
103 if (m_simRangeValid) return m_simRangeValid.Get();
104 return m_counter.Get() > 1;
105}
106
107void Ultrasonic::SetAutomaticMode(bool enabling) {
108 if (enabling == m_automaticEnabled) return; // ignore the case of no change
109
110 m_automaticEnabled = enabling;
111
112 if (enabling) {
113 /* Clear all the counters so no data is valid. No synchronization is needed
114 * because the background task is stopped.
115 */
116 for (auto& sensor : m_sensors) {
117 sensor->m_counter.Reset();
118 }
119
120 m_thread = std::thread(&Ultrasonic::UltrasonicChecker);
121
122 // TODO: Currently, lvuser does not have permissions to set task priorities.
123 // Until that is the case, uncommenting this will break user code that calls
124 // Ultrasonic::SetAutomicMode().
125 // m_task.SetPriority(kPriority);
126 } else {
127 // Wait for background task to stop running
128 if (m_thread.joinable()) {
129 m_thread.join();
130 }
131
132 // Clear all the counters (data now invalid) since automatic mode is
133 // disabled. No synchronization is needed because the background task is
134 // stopped.
135 for (auto& sensor : m_sensors) {
136 sensor->m_counter.Reset();
137 }
138 }
139}
140
141double Ultrasonic::GetRangeInches() const {
142 if (IsRangeValid()) {
143 if (m_simRange) return m_simRange.Get();
144 return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
145 } else {
146 return 0;
147 }
148}
149
150double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
151
152bool Ultrasonic::IsEnabled() const { return m_enabled; }
153
154void Ultrasonic::SetEnabled(bool enable) { m_enabled = enable; }
155
156void Ultrasonic::SetDistanceUnits(DistanceUnit units) { m_units = units; }
157
158Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const {
159 return m_units;
160}
161
162double Ultrasonic::PIDGet() {
163 switch (m_units) {
164 case Ultrasonic::kInches:
165 return GetRangeInches();
166 case Ultrasonic::kMilliMeters:
167 return GetRangeMM();
168 default:
169 return 0.0;
170 }
171}
172
173void Ultrasonic::SetPIDSourceType(PIDSourceType pidSource) {
174 if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
175 m_pidSource = pidSource;
176 }
177}
178
179void Ultrasonic::InitSendable(SendableBuilder& builder) {
180 builder.SetSmartDashboardType("Ultrasonic");
Austin Schuh1e69f942020-11-14 15:06:14 -0800181 builder.AddDoubleProperty(
182 "Value", [=]() { return GetRangeInches(); }, nullptr);
Brian Silverman8fce7482020-01-05 13:18:21 -0800183}
184
185void Ultrasonic::Initialize() {
186 m_simDevice = hal::SimDevice("Ultrasonic", m_echoChannel->GetChannel());
187 if (m_simDevice) {
188 m_simRangeValid = m_simDevice.CreateBoolean("Range Valid", false, true);
189 m_simRange = m_simDevice.CreateDouble("Range (in)", false, 0.0);
190 m_pingChannel->SetSimDevice(m_simDevice);
191 m_echoChannel->SetSimDevice(m_simDevice);
192 }
193
194 bool originalMode = m_automaticEnabled;
195 SetAutomaticMode(false); // Kill task when adding a new sensor
196 // Link this instance on the list
197 m_sensors.emplace_back(this);
198
199 m_counter.SetMaxPeriod(1.0);
200 m_counter.SetSemiPeriodMode(true);
201 m_counter.Reset();
202 m_enabled = true; // Make it available for round robin scheduling
203 SetAutomaticMode(originalMode);
204
205 static int instances = 0;
206 instances++;
207 HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
208 SendableRegistry::GetInstance().AddLW(this, "Ultrasonic",
209 m_echoChannel->GetChannel());
210}
211
212void Ultrasonic::UltrasonicChecker() {
213 while (m_automaticEnabled) {
214 for (auto& sensor : m_sensors) {
215 if (!m_automaticEnabled) break;
216
217 if (sensor->IsEnabled()) {
218 sensor->m_pingChannel->Pulse(kPingTime); // do the ping
219 }
220
221 Wait(0.1); // wait for ping to return
222 }
223 }
224}