Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/Athena/src/Ultrasonic.cpp b/wpilibc/Athena/src/Ultrasonic.cpp
new file mode 100644
index 0000000..ba25a04
--- /dev/null
+++ b/wpilibc/Athena/src/Ultrasonic.cpp
@@ -0,0 +1,354 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. 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 $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Ultrasonic.h"
+
+#include "Counter.h"
+#include "DigitalInput.h"
+#include "DigitalOutput.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+constexpr double
+ Ultrasonic::kPingTime; ///< Time (sec) for the ping trigger pulse.
+const uint32_t Ultrasonic::kPriority; ///< Priority that the ultrasonic round
+ ///robin task runs.
+constexpr double
+ Ultrasonic::kMaxUltrasonicTime; ///< Max time (ms) between readings.
+constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
+Ultrasonic *Ultrasonic::m_firstSensor =
+ nullptr; // head of the ultrasonic sensor list
+Task Ultrasonic::m_task;
+std::atomic<bool> Ultrasonic::m_automaticEnabled{false}; // automatic round robin mode
+priority_mutex Ultrasonic::m_mutex;
+
+/**
+ * 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. If one does, then this will certainly break. Make sure to disable
+ * automatic mode before changing
+ * anything with the sensors!!
+ */
+void Ultrasonic::UltrasonicChecker() {
+ Ultrasonic *u = nullptr;
+ while (m_automaticEnabled) {
+ if (u == nullptr) u = m_firstSensor;
+ if (u == nullptr) return;
+ if (u->IsEnabled()) u->m_pingChannel->Pulse(kPingTime); // do the ping
+ u = u->m_nextSensor;
+ 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
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_nextSensor = m_firstSensor;
+ m_firstSensor = 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++;
+ HALReport(HALUsageReporting::kResourceType_Ultrasonic, instances);
+ LiveWindow::GetInstance()->AddSensor("Ultrasonic",
+ m_echoChannel->GetChannel(), this);
+}
+
+/**
+ * Create an instance of the Ultrasonic Sensor
+ * This is designed to supchannel 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(uint32_t pingChannel, uint32_t 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_nextSensor = nullptr;
+ 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);
+ wpi_assert(m_firstSensor != nullptr);
+
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ if (this == m_firstSensor) {
+ m_firstSensor = m_nextSensor;
+ if (m_firstSensor == nullptr) {
+ SetAutomaticMode(false);
+ }
+ } else {
+ wpi_assert(m_firstSensor->m_nextSensor != nullptr);
+ for (Ultrasonic *s = m_firstSensor; s != nullptr; s = s->m_nextSensor) {
+ if (this == s->m_nextSensor) {
+ s->m_nextSensor = s->m_nextSensor->m_nextSensor;
+ break;
+ }
+ }
+ }
+ }
+ if (m_firstSensor != nullptr && 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) {
+ // enabling automatic mode.
+ // Clear all the counters so no data is valid
+ for (Ultrasonic *u = m_firstSensor; u != nullptr; u = u->m_nextSensor) {
+ u->m_counter.Reset();
+ }
+ // Start round robin task
+ wpi_assert(m_task.Verify() ==
+ false); // should be false since was previously disabled
+ m_task = Task("UltrasonicChecker", &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 {
+ // disabling automatic mode. Wait for background task to stop running.
+ while (m_task.Verify())
+ Wait(0.15); // just a little longer than the ping time for round-robin to
+ // stop
+
+ // clear all the counters (data now invalid) since automatic mode is stopped
+ for (Ultrasonic *u = m_firstSensor; u != nullptr; u = u->m_nextSensor) {
+ u->m_counter.Reset();
+ }
+ m_automaticEnabled = false;
+ m_task.join();
+ }
+}
+
+/**
+ * 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
+ * complted, 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; }