WPILibC++  unspecified
Ultrasonic.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2008-2018 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 #pragma once
9 
10 #include <atomic>
11 #include <memory>
12 #include <thread>
13 #include <vector>
14 
15 #include "Counter.h"
16 #include "PIDSource.h"
17 #include "SensorBase.h"
18 
19 namespace frc {
20 
21 class DigitalInput;
22 class DigitalOutput;
23 
36 class Ultrasonic : public SensorBase, public PIDSource {
37  public:
38  enum DistanceUnit { kInches = 0, kMilliMeters = 1 };
39 
40  Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
41  DistanceUnit units = kInches);
42  Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel,
43  DistanceUnit units = kInches);
44  Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
45  std::shared_ptr<DigitalInput> echoChannel,
46  DistanceUnit units = kInches);
47  Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units = kInches);
48  ~Ultrasonic() override;
49 
50  void Ping();
51  bool IsRangeValid() const;
52  static void SetAutomaticMode(bool enabling);
53  double GetRangeInches() const;
54  double GetRangeMM() const;
55  bool IsEnabled() const { return m_enabled; }
56  void SetEnabled(bool enable) { m_enabled = enable; }
57 
58  double PIDGet() override;
59  void SetPIDSourceType(PIDSourceType pidSource) override;
60  void SetDistanceUnits(DistanceUnit units);
61  DistanceUnit GetDistanceUnits() const;
62 
63  void InitSendable(SendableBuilder& builder) override;
64 
65  private:
66  void Initialize();
67 
68  static void UltrasonicChecker();
69 
70  // Time (sec) for the ping trigger pulse.
71  static constexpr double kPingTime = 10 * 1e-6;
72 
73  // Priority that the ultrasonic round robin task runs.
74  static constexpr int kPriority = 64;
75 
76  // Max time (ms) between readings.
77  static constexpr double kMaxUltrasonicTime = 0.1;
78  static constexpr double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
79 
80  // Thread doing the round-robin automatic sensing
81  static std::thread m_thread;
82 
83  // Ultrasonic sensors
84  static std::vector<Ultrasonic*> m_sensors;
85 
86  // Automatic round-robin mode
87  static std::atomic<bool> m_automaticEnabled;
88 
89  std::shared_ptr<DigitalOutput> m_pingChannel;
90  std::shared_ptr<DigitalInput> m_echoChannel;
91  bool m_enabled = false;
92  Counter m_counter;
93  DistanceUnit m_units;
94 };
95 
96 } // namespace frc
Definition: RobotController.cpp:14
Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, DistanceUnit units=kInches)
Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a DigitalOutp...
Definition: Ultrasonic.cpp:110
double PIDGet() override
Get the range in the current DistanceUnit for the PIDSource base object.
Definition: Ultrasonic.cpp:284
Base class for all sensors.
Definition: SensorBase.h:25
~Ultrasonic() override
Destructor for the ultrasonic sensor.
Definition: Ultrasonic.cpp:171
PIDSource interface is a generic sensor source for the PID class.
Definition: PIDSource.h:20
bool IsRangeValid() const
Check if there is a valid range measurement.
Definition: Ultrasonic.cpp:254
void InitSendable(SendableBuilder &builder) override
Initializes this Sendable object.
Definition: Ultrasonic.cpp:318
Class for counting the number of ticks on a digital input channel.
Definition: Counter.h:33
DistanceUnit GetDistanceUnits() const
Get the current DistanceUnit that is used for the PIDSource base object.
Definition: Ultrasonic.cpp:314
void SetDistanceUnits(DistanceUnit units)
Set the current DistanceUnit that should be used for the PIDSource base object.
Definition: Ultrasonic.cpp:307
double GetRangeInches() const
Get the range in inches from the ultrasonic sensor.
Definition: Ultrasonic.cpp:263
void Ping()
Single ping to ultrasonic sensor.
Definition: Ultrasonic.cpp:237
static void SetAutomaticMode(bool enabling)
Turn Automatic mode on/off.
Definition: Ultrasonic.cpp:197
Class to write to digital outputs.
Definition: DigitalOutput.h:24
Definition: SendableBuilder.h:23
double GetRangeMM() const
Get the range in millimeters from the ultrasonic sensor.
Definition: Ultrasonic.cpp:277
Class to read a digital input.
Definition: DigitalInput.h:25
void SetPIDSourceType(PIDSourceType pidSource) override
Set which parameter you are using as a process control variable.
Definition: Ultrasonic.cpp:295
Ultrasonic rangefinder class.
Definition: Ultrasonic.h:36