mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Update to 2018_v4 image and new build system. (#598)
* Revert "Force OpenCV to 3.1.0 (#602)"
This reverts commit 50ed55e8e2.
* Removes Simulation
* Removes old build system
* Removes old gtest
* Adds new gmock and gtest
* Updates to new ni-libraries
* removes MyRobot (to be replaced)
* moves files to new location
* Adds new sim backend and new test executables
* updates .styleguide and .gitignore
* Changes cpp WPILibVersion to a function
MSVC throws an AV with the old version.
* Disables USBCamera on all systems except for linux
* 2018 NI Libraries
* New build system
This commit is contained in:
Thad House
committed by
Peter Johnson
Peter Johnson
parent
50ed55e8e2
commit
e1195e8b9d
337
wpilibc/src/main/native/cpp/Ultrasonic.cpp
Normal file
337
wpilibc/src/main/native/cpp/Ultrasonic.cpp
Normal file
@@ -0,0 +1,337 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2008-2017. 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 the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "Ultrasonic.h"
|
||||
|
||||
#include "Counter.h"
|
||||
#include "DigitalInput.h"
|
||||
#include "DigitalOutput.h"
|
||||
#include "HAL/HAL.h"
|
||||
#include "LiveWindow/LiveWindow.h"
|
||||
#include "Timer.h"
|
||||
#include "Utility.h"
|
||||
#include "WPIErrors.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
// Time (sec) for the ping trigger pulse.
|
||||
constexpr double Ultrasonic::kPingTime;
|
||||
// Priority that the ultrasonic round robin task runs.
|
||||
const int Ultrasonic::kPriority;
|
||||
// Max time (ms) between readings.
|
||||
constexpr double Ultrasonic::kMaxUltrasonicTime;
|
||||
constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
|
||||
// automatic round robin mode
|
||||
std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
|
||||
std::set<Ultrasonic*> Ultrasonic::m_sensors;
|
||||
std::thread Ultrasonic::m_thread;
|
||||
|
||||
/**
|
||||
* 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. Make sure to disable automatic mode before
|
||||
* touching the list.
|
||||
*/
|
||||
void Ultrasonic::UltrasonicChecker() {
|
||||
while (m_automaticEnabled) {
|
||||
for (auto& sensor : m_sensors) {
|
||||
if (!m_automaticEnabled) break;
|
||||
|
||||
if (sensor->IsEnabled()) {
|
||||
sensor->m_pingChannel->Pulse(kPingTime); // do the ping
|
||||
}
|
||||
|
||||
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
|
||||
m_sensors.insert(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++;
|
||||
HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
|
||||
LiveWindow::GetInstance()->AddSensor("Ultrasonic",
|
||||
m_echoChannel->GetChannel(), this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create an instance of the Ultrasonic Sensor.
|
||||
*
|
||||
* This is designed to support 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(int pingChannel, int 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_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);
|
||||
|
||||
// No synchronization needed because the background task is stopped.
|
||||
m_sensors.erase(this);
|
||||
|
||||
if (!m_sensors.empty() && 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) {
|
||||
/* Clear all the counters so no data is valid. No synchronization is needed
|
||||
* because the background task is stopped.
|
||||
*/
|
||||
for (auto& sensor : m_sensors) {
|
||||
sensor->m_counter.Reset();
|
||||
}
|
||||
|
||||
m_thread = std::thread(&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 {
|
||||
// Wait for background task to stop running
|
||||
m_thread.join();
|
||||
|
||||
/* Clear all the counters (data now invalid) since automatic mode is
|
||||
* disabled. No synchronization is needed because the background task is
|
||||
* stopped.
|
||||
*/
|
||||
for (auto& sensor : m_sensors) {
|
||||
sensor->m_counter.Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 completed, 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; }
|
||||
Reference in New Issue
Block a user