diff --git a/include/hardware/Distance/DistanceSensor.hpp b/include/hardware/Distance/DistanceSensor.hpp new file mode 100644 index 0000000..e06f4bf --- /dev/null +++ b/include/hardware/Distance/DistanceSensor.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "hardware/Device.hpp" +#include "units/units.hpp" + +namespace lemlib { +/** + * @brief abstract class for distance sensors + * + * An distance sensor is a device that measures the distance to an external object. This class is abstract, and must be + * implemented by a subclass. + */ +class DistanceSensor : public Device { + public: + /** + * @brief Get the distance measured by the distance sensor + * + * The distance measured by the distance sensor is the distance from the sensor to the nearest object. + * + * @return Length the distance measured by the encoder + * @return INFINITY if there is an error, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * DistanceSensor* distanceSensor; + * const Length distance = distanceSensor->getDistance(); + * if (distance == INFINITY) { + * std::cout << "Error getting distance!" << std::endl; + * } else { + * std::cout << "Distance: " << distance.convert(in) << std::endl; + * } + * } + * @endcode + */ + virtual Length getDistance() const = 0; + /** + * @brief Set the offset of the distance sensor + * + * This function sets the offset of the distance sensor. A positive offset increases distance readings while a + * negative offset decreases them. This function is non-blocking. + * + * @param distance the length to set the offset to + * @return 0 on success + * @return INT_MAX on failure, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * DistanceSensor* distanceSensor; + * if (distanceSensor->setOffset(1_in) == 0) { + * std::cout << "Offset set!" << std::endl; + * // With an object touching the surface of the distance sensor + * std::cout << "Offset distance: " << distanceSensor->getDistance().convert(in) << std::endl; // + * outputs 1 inch } else { std::cout << "Error setting offset!" << std::endl; + * } + * } + * @endcode + */ + virtual int32_t setOffset(Length distance) = 0; + virtual ~DistanceSensor() = default; +}; +} // namespace lemlib \ No newline at end of file diff --git a/include/hardware/Distance/V5DistanceSensor.hpp b/include/hardware/Distance/V5DistanceSensor.hpp new file mode 100644 index 0000000..32cf6cf --- /dev/null +++ b/include/hardware/Distance/V5DistanceSensor.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include "hardware/Distance/DistanceSensor.hpp" +#include "hardware/Port.hpp" +#include "pros/distance.hpp" + +namespace lemlib { +/** + * @brief Distance sensor implementation for the V5 Distance sensor + * + */ +class V5DistanceSensor : public DistanceSensor { + public: + /** + * @brief Construct a new V5 Distance Sensor + * + * @param port the unsigned port of the distance sensor. + * + * @b Example: + * @code {.cpp} + * void initialize() { + * // distance sensor on port 1 + * lemlib::V5DistanceSensor distance(1); + * } + * @endcode + */ + V5DistanceSensor(SmartPort port); + /** + * @brief V5DistanceSensor copy constructor + * + * Because pros::Mutex does not have a copy constructor, an explicit + * copy constructor for the V5DistanceSensor is necessary + * + * @param other the V5DistanceSensor to copy + */ + V5DistanceSensor(const V5DistanceSensor& other); + /** + * @brief Create a new V5 Distance Sensor + * + * @param encoder the pros::Distance object to use + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::V5DistanceSensor distance = lemlib::V5DistanceSensor::from_pros_dist(pros::Distance(1)); + * } + * @endcode + */ + static V5DistanceSensor from_pros_dist(pros::Distance distanceSensor); + /** + * @brief whether the V5 Distance Sensor is connected + * + * @return 0 if its not connected + * @return 1 if it is connected + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::V5DistanceSensor distance = lemlib::V5DistanceSensor(1); + * const int result = distance.isConnected(); + * if (result == 1) { + * std::cout << "Distance sensor is connected!" << std::endl; + * } else if (result == 0) { + * std::cout << "Distance sensor is not connected!" << std::endl; + * } else { + * std::cout << "Error checking if distance sensor is connected!" << std::endl; + * } + * } + * @endcode + */ + int32_t isConnected() const override; + /** + * @brief Get the distance measured by the V5 Distance Sensor + * + * This function uses the following values of errno when an error state is reached: + * + * ENXIO: the port is not within the range of valid ports (1-21) + * ENODEV: the port cannot be configured as an V5 Distance sensor + * + * @return Length the relative distance measured by the distance sensor + * @return INFINITY if there is an error, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::V5DistanceSensor distanceSensor = lemlib::V5DistanceSensor(1); + * const Length distance = distanceSensor.getDistance(); + * if (angle == INFINITY) { + * std::cout << "Error getting distance!" << std::endl; + * } else { + * std::cout << "Distance: " << distance.convert(in) << std::endl; + * } + * } + * @endcode + */ + Length getDistance() const override; + /** + * @brief Set the offset of the distance sensor + * + * This function sets the offset of the distance sensor. A positive offset increases distance readings while a + * negative offset decreases them. This function is non-blocking. + * + * @param distance the length to set the offset to + * @return 0 on success + * @return INT_MAX on failure, setting errno + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::V5DistanceSensor distanceSensor(1); + * if (distanceSensor->setOffset(1_in) == 0) { + * std::cout << "Offset set!" << std::endl; + * // With an object touching the surface of the distance sensor + * std::cout << "Offset distance: " << distanceSensor->getDistance().convert(in) << std::endl; // + * outputs 1 inch } else { std::cout << "Error setting offset!" << std::endl; + * } + * } + * @endcode + */ + int32_t setOffset(Length distance) override; + private: + mutable pros::Mutex m_mutex; + Length m_offset = 0_in; + int m_port; +}; +} // namespace lemlib \ No newline at end of file diff --git a/src/hardware/Distance/V5DistanceSensor.cpp b/src/hardware/Distance/V5DistanceSensor.cpp new file mode 100644 index 0000000..20c6b39 --- /dev/null +++ b/src/hardware/Distance/V5DistanceSensor.cpp @@ -0,0 +1,39 @@ +#include "hardware/Distance/V5DistanceSensor.hpp" +#include "hardware/util.hpp" +#include +#include +#include + +namespace lemlib { +V5DistanceSensor::V5DistanceSensor(SmartPort port) + : m_port(port) {} + +V5DistanceSensor::V5DistanceSensor(const V5DistanceSensor& other) + : m_port(other.m_port), + m_offset(other.m_offset) {} + +V5DistanceSensor V5DistanceSensor::from_pros_dist(pros::Distance distanceSensor) { + return V5DistanceSensor {{distanceSensor.get_port(), runtime_check_port}}; +} + +int32_t V5DistanceSensor::isConnected() const { + if (pros::c::distance_get(m_port) == INT_MAX) return 0; + else return 1; +} + +Length V5DistanceSensor::getDistance() const { + std::lock_guard lock(m_mutex); + const int32_t raw = pros::c::distance_get(m_port); + if (raw == INT_MAX) return from_in(INFINITY); + // the distance sensor returns mm + const Length distance = from_mm(raw); + return distance + m_offset; +} + +int32_t V5DistanceSensor::setOffset(Length offset) { + const int32_t raw = pros::c::distance_get(m_port); + if (raw == INT_MAX) return INT_MAX; + m_offset = offset; + return 0; +} +} // namespace lemlib \ No newline at end of file