/Ultrasonic.cpp
C++ | 361 lines | 215 code | 32 blank | 114 comment | 38 complexity | e5dd98f8d1b0a3d4ae09338c05d3425c MD5 | raw file
- /*----------------------------------------------------------------------------*/
- /* 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"
-
- 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;
- 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()
- {
- m_table = NULL;
- 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_t pingChannel, uint32_t 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_t pingModuleNumber, uint32_t pingChannel,
- uint8_t echoModuleNumber, uint32_t 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;
- }
-