Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/wpilibc/athena/src/Ultrasonic.cpp b/wpilibc/athena/src/Ultrasonic.cpp
new file mode 100644
index 0000000..4b3549e
--- /dev/null
+++ b/wpilibc/athena/src/Ultrasonic.cpp
@@ -0,0 +1,337 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "Ultrasonic.h"
+
+#include "Counter.h"
+#include "DigitalInput.h"
+#include "DigitalOutput.h"
+#include "HAL/HAL.h"
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+// Time (sec) for the ping trigger pulse.
+constexpr double Ultrasonic::kPingTime;
+// Priority that the ultrasonic round robin task runs.
+const int Ultrasonic::kPriority;
+// Max time (ms) between readings.
+constexpr double Ultrasonic::kMaxUltrasonicTime;
+constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
+// automatic round robin mode
+std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
+std::set<Ultrasonic*> Ultrasonic::m_sensors;
+std::thread Ultrasonic::m_thread;
+
+/**
+ * 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.
+ */
+void Ultrasonic::UltrasonicChecker() {
+ while (m_automaticEnabled) {
+ for (auto& sensor : m_sensors) {
+ if (!m_automaticEnabled) break;
+
+ if (sensor->IsEnabled()) {
+ sensor->m_pingChannel->Pulse(kPingTime); // do the ping
+ }
+
+ Wait(0.1); // wait for ping to return
+ }
+ }
+}
+
+/**
+ * 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 Ultrasonic::Initialize() {
+ bool originalMode = m_automaticEnabled;
+ SetAutomaticMode(false); // kill task when adding a new sensor
+ // link this instance on the list
+ m_sensors.insert(this);
+
+ m_counter.SetMaxPeriod(1.0);
+ m_counter.SetSemiPeriodMode(true);
+ m_counter.Reset();
+ m_enabled = true; // make it available for round robin scheduling
+ SetAutomaticMode(originalMode);
+
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
+ LiveWindow::GetInstance()->AddSensor("Ultrasonic",
+ m_echoChannel->GetChannel(), this);
+}
+
+/**
+ * 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.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+Ultrasonic::Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units)
+ : m_pingChannel(std::make_shared<DigitalOutput>(pingChannel)),
+ m_echoChannel(std::make_shared<DigitalInput>(echoChannel)),
+ m_counter(m_echoChannel) {
+ m_units = units;
+ Initialize();
+}
+
+/**
+ * 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.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
+ DistanceUnit units)
+ : m_pingChannel(pingChannel, NullDeleter<DigitalOutput>()),
+ m_echoChannel(echoChannel, NullDeleter<DigitalInput>()),
+ m_counter(m_echoChannel) {
+ if (pingChannel == nullptr || echoChannel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_units = units;
+ return;
+ }
+ m_units = units;
+ Initialize();
+}
+
+/**
+ * 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.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel,
+ DistanceUnit units)
+ : m_pingChannel(&pingChannel, NullDeleter<DigitalOutput>()),
+ m_echoChannel(&echoChannel, NullDeleter<DigitalInput>()),
+ m_counter(m_echoChannel) {
+ m_units = units;
+ Initialize();
+}
+
+/**
+ * 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.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+Ultrasonic::Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
+ std::shared_ptr<DigitalInput> echoChannel,
+ DistanceUnit units)
+ : m_pingChannel(pingChannel),
+ m_echoChannel(echoChannel),
+ m_counter(m_echoChannel) {
+ m_units = units;
+ Initialize();
+}
+
+/**
+ * Destructor for the ultrasonic sensor.
+ *
+ * Delete the instance of the ultrasonic sensor by freeing the allocated digital
+ * channels. If the system was in automatic mode (round robin), then it is
+ * stopped, then started again after this sensor is removed (provided this
+ * wasn't the last sensor).
+ */
+Ultrasonic::~Ultrasonic() {
+ bool wasAutomaticMode = m_automaticEnabled;
+ SetAutomaticMode(false);
+
+ // No synchronization needed because the background task is stopped.
+ m_sensors.erase(this);
+
+ if (!m_sensors.empty() && wasAutomaticMode) {
+ SetAutomaticMode(true);
+ }
+}
+
+/**
+ * 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
+ * prefered, it can be implemented by pinging the sensors
+ * manually and waiting for the results to come back.
+ */
+void Ultrasonic::SetAutomaticMode(bool enabling) {
+ if (enabling == m_automaticEnabled) return; // ignore the case of no change
+
+ m_automaticEnabled = enabling;
+
+ if (enabling) {
+ /* Clear all the counters so no data is valid. No synchronization is needed
+ * because the background task is stopped.
+ */
+ for (auto& sensor : m_sensors) {
+ sensor->m_counter.Reset();
+ }
+
+ m_thread = std::thread(&Ultrasonic::UltrasonicChecker);
+
+ // TODO: Currently, lvuser does not have permissions to set task priorities.
+ // Until that is the case, uncommenting this will break user code that calls
+ // Ultrasonic::SetAutomicMode().
+ // m_task.SetPriority(kPriority);
+ } else {
+ // Wait for background task to stop running
+ m_thread.join();
+
+ /* Clear all the counters (data now invalid) since automatic mode is
+ * disabled. No synchronization is needed because the background task is
+ * stopped.
+ */
+ for (auto& sensor : m_sensors) {
+ sensor->m_counter.Reset();
+ }
+ }
+}
+
+/**
+ * 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 Ultrasonic::Ping() {
+ wpi_assert(!m_automaticEnabled);
+ m_counter.Reset(); // reset the counter to zero (invalid data now)
+ m_pingChannel->Pulse(
+ kPingTime); // do the ping to start getting a single range
+}
+
+/**
+ * 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 Ultrasonic::IsRangeValid() const { return m_counter.Get() > 1; }
+
+/**
+ * Get the range in inches from the ultrasonic sensor.
+ *
+ * @return double Range in inches 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.
+ */
+double Ultrasonic::GetRangeInches() const {
+ if (IsRangeValid())
+ return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
+ else
+ return 0;
+}
+
+/**
+ * Get the range in millimeters from the ultrasonic sensor.
+ *
+ * @return double Range in millimeters of the target returned by the ultrasonic
+ * sensor. If there is no valid value yet, i.e. at least one
+ * measurement hasn't completed, then return 0.
+ */
+double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
+
+/**
+ * Get the range in the current DistanceUnit for the PIDSource base object.
+ *
+ * @return The range in DistanceUnit
+ */
+double Ultrasonic::PIDGet() {
+ switch (m_units) {
+ case Ultrasonic::kInches:
+ return GetRangeInches();
+ case Ultrasonic::kMilliMeters:
+ return GetRangeMM();
+ default:
+ return 0.0;
+ }
+}
+
+void Ultrasonic::SetPIDSourceType(PIDSourceType pidSource) {
+ if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
+ m_pidSource = pidSource;
+ }
+}
+
+/**
+ * Set the current DistanceUnit that should be used for the PIDSource base
+ * object.
+ *
+ * @param units The DistanceUnit that should be used.
+ */
+void Ultrasonic::SetDistanceUnits(DistanceUnit units) { m_units = units; }
+
+/**
+ * Get the current DistanceUnit that is used for the PIDSource base object.
+ *
+ * @return The type of DistanceUnit that is being used.
+ */
+Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const {
+ return m_units;
+}
+
+void Ultrasonic::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetRangeInches());
+ }
+}
+
+void Ultrasonic::StartLiveWindowMode() {}
+
+void Ultrasonic::StopLiveWindowMode() {}
+
+std::string Ultrasonic::GetSmartDashboardType() const { return "Ultrasonic"; }
+
+void Ultrasonic::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Ultrasonic::GetTable() const { return m_table; }