1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <atomic>
#include <memory>
#include <thread>
#include <vector>
#include <hal/SimDevice.h>
#include <units/length.h>
#include <units/time.h>
#include <units/velocity.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/Counter.h"
namespace frc {
class DigitalInput;
class DigitalOutput;
/**
* Ultrasonic rangefinder class.
*
* The Ultrasonic rangefinder measures absolute distance based on the round-trip
* time of a ping generated by the controller. These sensors use two
* transducers, a speaker and a microphone both tuned to the ultrasonic range. A
* common ultrasonic sensor, the Daventech SRF04 requires a short pulse to be
* generated on a digital channel. This causes the chirp to be emitted. A second
* line becomes high as the ping is transmitted and goes low when the echo is
* received. The time that the line is high determines the round trip distance
* (time of flight).
*/
class Ultrasonic : public wpi::Sendable,
public wpi::SendableHelper<Ultrasonic> {
public:
/**
* Create an instance of the Ultrasonic Sensor.
*
* This is designed to support the Daventech SRF04 and Vex ultrasonic sensors.
*
* @param pingChannel The digital output channel that sends the pulse to
* initiate the sensor sending the ping.
* @param echoChannel The digital input channel that receives the echo. The
* length of time that the echo is high represents the
* round trip time of the ping, and the distance.
*/
Ultrasonic(int pingChannel, int echoChannel);
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
* channel and a DigitalOutput for the ping channel.
*
* @param pingChannel The digital output object that starts the sensor doing a
* ping. Requires a 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to
* determine the range.
*/
Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel);
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
* channel and a DigitalOutput for the ping channel.
*
* @param pingChannel The digital output object that starts the sensor doing a
* ping. Requires a 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to
* determine the range.
*/
Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel);
/**
* Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
* channel and a DigitalOutput for the ping channel.
*
* @param pingChannel The digital output object that starts the sensor doing a
* ping. Requires a 10uS pulse to start.
* @param echoChannel The digital input object that times the return pulse to
* determine the range.
*/
Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
std::shared_ptr<DigitalInput> echoChannel);
~Ultrasonic() override;
Ultrasonic(Ultrasonic&&) = default;
Ultrasonic& operator=(Ultrasonic&&) = default;
/**
* Returns the echo channel.
*
* @return The echo channel.
*/
int GetEchoChannel() const;
/**
* Single ping to ultrasonic sensor.
*
* Send out a single ping to the ultrasonic sensor. This only works if
* automatic (round robin) mode is disabled. A single ping is sent out, and
* the counter should count the semi-period when it comes in. The counter is
* reset to make the current value invalid.
*/
void Ping();
/**
* Check if there is a valid range measurement.
*
* The ranges are accumulated in a counter that will increment on each edge of
* the echo (return) signal. If the count is not at least 2, then the range
* has not yet been measured, and is invalid.
*/
bool IsRangeValid() const;
/**
* Turn Automatic mode on/off.
*
* When in Automatic mode, all sensors will fire in round robin, waiting a set
* time between each sensor.
*
* @param enabling Set to true if round robin scheduling should start for all
* the ultrasonic sensors. This scheduling method assures that
* the sensors are non-interfering because no two sensors fire
* at the same time. If another scheduling algorithm is
* preferred, it can be implemented by pinging the sensors
* manually and waiting for the results to come back.
*/
static void SetAutomaticMode(bool enabling);
/**
* Get the range from the ultrasonic sensor.
*
* @return Range of the target returned from the ultrasonic sensor. If there
* is no valid value yet, i.e. at least one measurement hasn't
* completed, then return 0.
*/
units::meter_t GetRange() const;
bool IsEnabled() const;
void SetEnabled(bool enable);
void InitSendable(wpi::SendableBuilder& builder) override;
private:
/**
* Initialize the Ultrasonic Sensor.
*
* This is the common code that initializes the ultrasonic sensor given that
* there are two digital I/O channels allocated. If the system was running in
* automatic mode (round robin) when the new sensor is added, it is stopped,
* the sensor is added, then automatic mode is restored.
*/
void Initialize();
/**
* Background task that goes through the list of ultrasonic sensors and pings
* each one in turn. The counter is configured to read the timing of the
* returned echo pulse.
*
* DANGER WILL ROBINSON, DANGER WILL ROBINSON:
* This code runs as a task and assumes that none of the ultrasonic sensors
* will change while it's running. Make sure to disable automatic mode before
* touching the list.
*/
static void UltrasonicChecker();
// Time (usec) for the ping trigger pulse.
static constexpr auto kPingTime = 10_us;
// Max time (ms) between readings.
static constexpr auto kMaxUltrasonicTime = 0.1_s;
static constexpr auto kSpeedOfSound = 1130_fps;
// Thread doing the round-robin automatic sensing
static std::thread m_thread;
// Ultrasonic sensors
static std::vector<Ultrasonic*> m_sensors;
// Automatic round-robin mode
static std::atomic<bool> m_automaticEnabled;
std::shared_ptr<DigitalOutput> m_pingChannel;
std::shared_ptr<DigitalInput> m_echoChannel;
bool m_enabled = false;
Counter m_counter;
hal::SimDevice m_simDevice;
hal::SimBoolean m_simRangeValid;
hal::SimDouble m_simRange;
};
} // namespace frc