This is the latest WPILib src, VisionSample2013, cRIO image, ... pulled down from firstforge.wpi.edu.
There might be risks in using the top of tree rather than an official release, but the commit messages do mention fixes for some deadlocks and race conditions.
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4066 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Ultrasonic.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Ultrasonic.cpp
new file mode 100644
index 0000000..a8b3991
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Ultrasonic.cpp
@@ -0,0 +1,360 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "NetworkCommunication/UsageReporting.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+const double Ultrasonic::kPingTime; ///< Time (sec) for the ping trigger pulse.
+const UINT32 Ultrasonic::kPriority; ///< Priority that the ultrasonic round robin task runs.
+const double Ultrasonic::kMaxUltrasonicTime; ///< Max time (ms) between readings.
+const double Ultrasonic::kSpeedOfSoundInchesPerSec;
+Task Ultrasonic::m_task("UltrasonicChecker", (FUNCPTR)UltrasonicChecker); // task doing the round-robin automatic sensing
+Ultrasonic *Ultrasonic::m_firstSensor = NULL; // head of the ultrasonic sensor list
+bool Ultrasonic::m_automaticEnabled = false; // automatic round robin mode
+SEM_ID Ultrasonic::m_semaphore = 0;
+
+/**
+ * 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 = NULL;
+ while (m_automaticEnabled)
+ {
+ if (u == NULL) u = m_firstSensor;
+ if (u == NULL) 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;
+ if (m_semaphore == 0) m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+ SetAutomaticMode(false); // kill task when adding a new sensor
+ semTake(m_semaphore, WAIT_FOREVER); // link this instance on the list
+ {
+ m_nextSensor = m_firstSensor;
+ m_firstSensor = this;
+ }
+ semGive(m_semaphore);
+
+ m_counter = new Counter(m_echoChannel); // set up counter for this sensor
+ m_counter->SetMaxPeriod(1.0);
+ m_counter->SetSemiPeriodMode(true);
+ m_counter->Reset();
+ m_counter->Start();
+ m_enabled = true; // make it available for round robin scheduling
+ SetAutomaticMode(originalMode);
+
+ static int instances = 0;
+ instances++;
+ nUsageReporting::report(nUsageReporting::kResourceType_Ultrasonic, instances);
+ LiveWindow::GetInstance()->AddSensor("Ultrasonic", m_echoChannel->GetModuleForRouting(), m_echoChannel->GetChannel(), this);
+}
+
+/**
+ * Create an instance of the Ultrasonic Sensor using the default module.
+ * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
+ * constructor assumes that both digital I/O channels are in the default digital module.
+ * @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 pingChannel, UINT32 echoChannel, DistanceUnit units)
+{
+ m_pingChannel = new DigitalOutput(pingChannel);
+ m_echoChannel = new DigitalInput(echoChannel);
+ m_allocatedChannels = true;
+ 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)
+{
+ if (pingChannel == NULL || echoChannel == NULL)
+ {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ m_allocatedChannels = false;
+ m_pingChannel = pingChannel;
+ m_echoChannel = 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_allocatedChannels = false;
+ m_pingChannel = &pingChannel;
+ m_echoChannel = &echoChannel;
+ m_units = units;
+ Initialize();
+}
+
+/**
+ * Create an instance of the Ultrasonic sensor using specified modules.
+ * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic sensors. This
+ * constructors takes the channel and module slot for each of the required digital I/O channels.
+ * @param pingModuleNumber The digital module that the pingChannel is on.
+ * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
+ * sending the ping.
+ * @param echoModuleNumber The digital module that the echoChannel is on.
+ * @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(UINT8 pingModuleNumber, UINT32 pingChannel,
+ UINT8 echoModuleNumber, UINT32 echoChannel, DistanceUnit units)
+{
+ m_pingChannel = new DigitalOutput(pingModuleNumber, pingChannel);
+ m_echoChannel = new DigitalInput(echoModuleNumber, echoChannel);
+ m_allocatedChannels = true;
+ 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);
+ if (m_allocatedChannels)
+ {
+ delete m_pingChannel;
+ delete m_echoChannel;
+ }
+ wpi_assert(m_firstSensor != NULL);
+
+ semTake(m_semaphore, WAIT_FOREVER);
+ {
+ if (this == m_firstSensor)
+ {
+ m_firstSensor = m_nextSensor;
+ if (m_firstSensor == NULL)
+ {
+ SetAutomaticMode(false);
+ }
+ }
+ else
+ {
+ wpi_assert(m_firstSensor->m_nextSensor != NULL);
+ for (Ultrasonic *s = m_firstSensor; s != NULL; s = s->m_nextSensor)
+ {
+ if (this == s->m_nextSensor)
+ {
+ s->m_nextSensor = s->m_nextSensor->m_nextSensor;
+ break;
+ }
+ }
+ }
+ }
+ semGive(m_semaphore);
+ if (m_firstSensor != NULL && 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 preffered, 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 != NULL; 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.Start();
+ }
+ 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 != NULL; u = u->m_nextSensor)
+ {
+ u->m_counter->Reset();
+ }
+ m_task.Stop();
+ }
+}
+
+/**
+ * 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()
+{
+ // TODO: Either assert or disable, not both.
+ wpi_assert(!m_automaticEnabled);
+ SetAutomaticMode(false); // turn off automatic round robin if pinging single sensor
+ 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()
+{
+ 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()
+{
+ 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()
+{
+ 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;
+ }
+}
+
+/**
+ * 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()
+{
+ return m_units;
+}
+
+void Ultrasonic::UpdateTable() {
+ if (m_table != NULL) {
+ m_table->PutNumber("Value", GetRangeInches());
+ }
+}
+
+void Ultrasonic::StartLiveWindowMode() {
+
+}
+
+void Ultrasonic::StopLiveWindowMode() {
+
+}
+
+std::string Ultrasonic::GetSmartDashboardType() {
+ return "Ultrasonic";
+}
+
+void Ultrasonic::InitTable(ITable *subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+ITable * Ultrasonic::GetTable() {
+ return m_table;
+}
+