Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Major formatting changes (breaks diffs). No code changes.

Browse Source 
The changes made in this commit do not affect any actual code,
    they are purely aesthetic. I ran clang-format with google style
    over all .h/.cpp files in wpilibc that weren't in wpilibC++Sim
    or gtest, and the eclipse formatter over all of the Java files
    using the Google eclipse formatting configuration.

Change-Id: I9627bca0bc103c398ecc1c5ba17467193291ae63
This commit is contained in:
James Kuszmaul
2015-06-25 15:07:55 -04:00
parent bd64d9a7ef
commit 7eb8550bdb
470 changed files with 89798 additions and 77287 deletions
Download Patch File Download Diff File Expand all files Collapse all files

428
wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
View File

@@ -1,5 +1,6 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* 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. */
/*----------------------------------------------------------------------------*/
@@ -15,320 +16,317 @@
#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::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
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
SEMAPHORE_ID Ultrasonic::m_semaphore = 0;
/**
* Background task that goes through the list of ultrasonic sensors and pings each one in turn. The counter
* 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
* 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
}
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
* 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 = initializeSemaphore(SEMAPHORE_FULL);
SetAutomaticMode(false); // kill task when adding a new sensor
takeSemaphore(m_semaphore); // link this instance on the list
{
m_nextSensor = m_firstSensor;
m_firstSensor = this;
}
giveSemaphore(m_semaphore);
void Ultrasonic::Initialize() {
m_table = NULL;
bool originalMode = m_automaticEnabled;
if (m_semaphore == 0) m_semaphore = initializeSemaphore(SEMAPHORE_FULL);
SetAutomaticMode(false); // kill task when adding a new sensor
takeSemaphore(m_semaphore); // link this instance on the list
{
m_nextSensor = m_firstSensor;
m_firstSensor = this;
}
giveSemaphore(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_enabled = true; // make it available for round robin scheduling
SetAutomaticMode(originalMode);
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_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);
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
* 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
* @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();
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
* 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 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();
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
* 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 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();
Ultrasonic::Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel,
DistanceUnit units) {
m_allocatedChannels = false;
m_pingChannel = &pingChannel;
m_echoChannel = &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
* 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);
Ultrasonic::~Ultrasonic() {
bool wasAutomaticMode = m_automaticEnabled;
SetAutomaticMode(false);
if (m_allocatedChannels) {
delete m_pingChannel;
delete m_echoChannel;
}
wpi_assert(m_firstSensor != NULL);
takeSemaphore(m_semaphore);
{
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;
}
}
}
}
giveSemaphore(m_semaphore);
if (m_firstSensor != NULL && wasAutomaticMode)
SetAutomaticMode(true);
takeSemaphore(m_semaphore);
{
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;
}
}
}
}
giveSemaphore(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
* @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
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
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();
}
// 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
* 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
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.
* 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;
}
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.
* @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;
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.
* @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;
}
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() const
{
switch(m_units)
{
case Ultrasonic::kInches:
return GetRangeInches();
case Ultrasonic::kMilliMeters:
return GetRangeMM();
default:
return 0.0;
}
double Ultrasonic::PIDGet() const {
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.
* 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;
}
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;
Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const {
return m_units;
}
void Ultrasonic::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetRangeInches());
}
if (m_table != NULL) {
m_table->PutNumber("Value", GetRangeInches());
}
}
void Ultrasonic::StartLiveWindowMode() {
void Ultrasonic::StartLiveWindowMode() {}
}
void Ultrasonic::StopLiveWindowMode() {}
void Ultrasonic::StopLiveWindowMode() {
}
std::string Ultrasonic::GetSmartDashboardType() const {
return "Ultrasonic";
}
std::string Ultrasonic::GetSmartDashboardType() const { return "Ultrasonic"; }
void Ultrasonic::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
m_table = subTable;
UpdateTable();
}
ITable * Ultrasonic::GetTable() const {
return m_table;
}
ITable *Ultrasonic::GetTable() const { return m_table; }