copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/externals/WPILib/WPILib/Ultrasonic.cpp b/aos/externals/WPILib/WPILib/Ultrasonic.cpp
new file mode 100644
index 0000000..24cfb74
--- /dev/null
+++ b/aos/externals/WPILib/WPILib/Ultrasonic.cpp
@@ -0,0 +1,330 @@
+/*----------------------------------------------------------------------------*/
+/* 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"
+
+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);
+}
+
+/**
+ * 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;
+}