mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
a9f0e46680
This makes the halsim_adx_gyro_accelerometer simulation plugin and the accelerometer part of lowfi_simulation obsolete.
242 lines
8.5 KiB
C++
242 lines
8.5 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2008-2019 FIRST. 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. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#pragma once
|
|
|
|
#include <atomic>
|
|
#include <memory>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include <hal/SimDevice.h>
|
|
|
|
#include "frc/Counter.h"
|
|
#include "frc/ErrorBase.h"
|
|
#include "frc/PIDSource.h"
|
|
#include "frc/smartdashboard/Sendable.h"
|
|
#include "frc/smartdashboard/SendableHelper.h"
|
|
|
|
namespace frc {
|
|
|
|
class DigitalInput;
|
|
class DigitalOutput;
|
|
|
|
/**
|
|
* Ultrasonic rangefinder class.
|
|
*
|
|
* The Ultrasonic rangefinder measures absolute distance based on the round-trip
|
|
* time of a ping generated by the controller. These sensors use two
|
|
* transducers, a speaker and a microphone both tuned to the ultrasonic range. A
|
|
* common ultrasonic sensor, the Daventech SRF04 requires a short pulse to be
|
|
* generated on a digital channel. This causes the chirp to be emitted. A second
|
|
* line becomes high as the ping is transmitted and goes low when the echo is
|
|
* received. The time that the line is high determines the round trip distance
|
|
* (time of flight).
|
|
*/
|
|
class Ultrasonic : public ErrorBase,
|
|
public Sendable,
|
|
public PIDSource,
|
|
public SendableHelper<Ultrasonic> {
|
|
public:
|
|
enum DistanceUnit { kInches = 0, kMilliMeters = 1 };
|
|
|
|
/**
|
|
* 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(int pingChannel, int echoChannel, DistanceUnit units = kInches);
|
|
|
|
/**
|
|
* 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(DigitalOutput* pingChannel, DigitalInput* echoChannel,
|
|
DistanceUnit units = kInches);
|
|
|
|
/**
|
|
* 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(DigitalOutput& pingChannel, DigitalInput& echoChannel,
|
|
DistanceUnit units = kInches);
|
|
|
|
/**
|
|
* 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(std::shared_ptr<DigitalOutput> pingChannel,
|
|
std::shared_ptr<DigitalInput> echoChannel,
|
|
DistanceUnit units = kInches);
|
|
|
|
~Ultrasonic() override;
|
|
|
|
Ultrasonic(Ultrasonic&&) = default;
|
|
Ultrasonic& operator=(Ultrasonic&&) = default;
|
|
|
|
/**
|
|
* 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 Ping();
|
|
|
|
/**
|
|
* 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 IsRangeValid() const;
|
|
|
|
/**
|
|
* 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.
|
|
*/
|
|
static void SetAutomaticMode(bool enabling);
|
|
|
|
/**
|
|
* Get the range in inches from the ultrasonic sensor.
|
|
*
|
|
* @return 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 GetRangeInches() const;
|
|
|
|
/**
|
|
* Get the range in millimeters from the ultrasonic sensor.
|
|
*
|
|
* @return 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 GetRangeMM() const;
|
|
|
|
bool IsEnabled() const;
|
|
|
|
void SetEnabled(bool enable);
|
|
|
|
/**
|
|
* Set the current DistanceUnit that should be used for the PIDSource base
|
|
* object.
|
|
*
|
|
* @param units The DistanceUnit that should be used.
|
|
*/
|
|
void SetDistanceUnits(DistanceUnit units);
|
|
|
|
/**
|
|
* Get the current DistanceUnit that is used for the PIDSource base object.
|
|
*
|
|
* @return The type of DistanceUnit that is being used.
|
|
*/
|
|
DistanceUnit GetDistanceUnits() const;
|
|
|
|
/**
|
|
* Get the range in the current DistanceUnit for the PIDSource base object.
|
|
*
|
|
* @return The range in DistanceUnit
|
|
*/
|
|
double PIDGet() override;
|
|
|
|
void SetPIDSourceType(PIDSourceType pidSource) override;
|
|
|
|
void InitSendable(SendableBuilder& builder) override;
|
|
|
|
private:
|
|
/**
|
|
* 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 Initialize();
|
|
|
|
/**
|
|
* 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.
|
|
*/
|
|
static void UltrasonicChecker();
|
|
|
|
// Time (sec) for the ping trigger pulse.
|
|
static constexpr double kPingTime = 10 * 1e-6;
|
|
|
|
// Priority that the ultrasonic round robin task runs.
|
|
static constexpr int kPriority = 64;
|
|
|
|
// Max time (ms) between readings.
|
|
static constexpr double kMaxUltrasonicTime = 0.1;
|
|
static constexpr double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
|
|
|
|
// Thread doing the round-robin automatic sensing
|
|
static std::thread m_thread;
|
|
|
|
// Ultrasonic sensors
|
|
static std::vector<Ultrasonic*> m_sensors;
|
|
|
|
// Automatic round-robin mode
|
|
static std::atomic<bool> m_automaticEnabled;
|
|
|
|
std::shared_ptr<DigitalOutput> m_pingChannel;
|
|
std::shared_ptr<DigitalInput> m_echoChannel;
|
|
bool m_enabled = false;
|
|
Counter m_counter;
|
|
DistanceUnit m_units;
|
|
|
|
hal::SimDevice m_simDevice;
|
|
hal::SimBoolean m_simRangeValid;
|
|
hal::SimDouble m_simRange;
|
|
};
|
|
|
|
} // namespace frc
|