From dc0e9712e6c16a533a2b7281accc2a00f359c94b Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Fri, 24 May 2024 15:55:30 -0400 Subject: [PATCH] [wpilib] Add support for Sharp IR sensors (#6023) --- wpilibc/src/main/native/cpp/SharpIR.cpp | 64 +++++++ .../main/native/cpp/simulation/SharpIRSim.cpp | 24 +++ wpilibc/src/main/native/include/frc/SharpIR.h | 99 +++++++++++ .../include/frc/simulation/SharpIRSim.h | 25 +++ wpilibc/src/test/native/cpp/SharpIRTest.cpp | 23 +++ .../java/edu/wpi/first/wpilibj/SharpIR.java | 156 ++++++++++++++++++ .../first/wpilibj/simulation/SharpIRSim.java | 40 +++++ .../edu/wpi/first/wpilibj/SharpIRTest.java | 27 +++ 8 files changed, 458 insertions(+) create mode 100644 wpilibc/src/main/native/cpp/SharpIR.cpp create mode 100644 wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp create mode 100644 wpilibc/src/main/native/include/frc/SharpIR.h create mode 100644 wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h create mode 100644 wpilibc/src/test/native/cpp/SharpIRTest.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java diff --git a/wpilibc/src/main/native/cpp/SharpIR.cpp b/wpilibc/src/main/native/cpp/SharpIR.cpp new file mode 100644 index 0000000000..db9b9b9a89 --- /dev/null +++ b/wpilibc/src/main/native/cpp/SharpIR.cpp @@ -0,0 +1,64 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/SharpIR.h" + +#include +#include + +#include "frc/AnalogInput.h" + +using namespace frc; + +SharpIR SharpIR::GP2Y0A02YK0F(int channel) { + return SharpIR(channel, 62.28, -1.092, 20, 150.0); +} + +SharpIR SharpIR::GP2Y0A21YK0F(int channel) { + return SharpIR(channel, 26.449, -1.226, 10.0, 80.0); +} + +SharpIR SharpIR::GP2Y0A41SK0F(int channel) { + return SharpIR(channel, 12.354, -1.07, 4.0, 30.0); +} + +SharpIR SharpIR::GP2Y0A51SK0F(int channel) { + return SharpIR(channel, 5.2819, -1.161, 2.0, 15.0); +} + +SharpIR::SharpIR(int channel, double a, double b, double minCM, double maxCM) + : m_sensor(channel), m_A(a), m_B(b), m_minCM(minCM), m_maxCM(maxCM) { + wpi::SendableRegistry::AddLW(this, "SharpIR", channel); + + m_simDevice = hal::SimDevice("SharpIR", m_sensor.GetChannel()); + if (m_simDevice) { + m_simRange = m_simDevice.CreateDouble("Range (cm)", false, 0.0); + m_sensor.SetSimDevice(m_simDevice); + } +} + +int SharpIR::GetChannel() const { + return m_sensor.GetChannel(); +} + +units::centimeter_t SharpIR::GetRange() const { + double distance; + + if (m_simRange) { + distance = m_simRange.Get(); + } else { + // Don't allow zero/negative values + auto v = std::max(m_sensor.GetVoltage(), 0.00001); + distance = m_A * std::pow(v, m_B); + } + + // Always constrain output + return units::centimeter_t{std::max(std::min(distance, m_maxCM), m_minCM)}; +} + +void SharpIR::InitSendable(wpi::SendableBuilder& builder) { + builder.SetSmartDashboardType("Ultrasonic"); + builder.AddDoubleProperty( + "Value", [=, this] { return GetRange().value(); }, nullptr); +} diff --git a/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp b/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp new file mode 100644 index 0000000000..46922aa6e4 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/simulation/SharpIRSim.h" + +#include +#include + +#include "frc/simulation/SimDeviceSim.h" + +using namespace frc; + +SharpIRSim::SharpIRSim(const SharpIR& sharpIR) + : SharpIRSim(sharpIR.GetChannel()) {} + +SharpIRSim::SharpIRSim(int channel) { + frc::sim::SimDeviceSim deviceSim{"SharpIR", channel}; + m_simRange = deviceSim.GetDouble("Range (cm)"); +} + +void SharpIRSim::SetRange(units::centimeter_t rng) { + m_simRange.Set(rng.value()); +} diff --git a/wpilibc/src/main/native/include/frc/SharpIR.h b/wpilibc/src/main/native/include/frc/SharpIR.h new file mode 100644 index 0000000000..4dd7e43319 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/SharpIR.h @@ -0,0 +1,99 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include + +#include "frc/AnalogInput.h" + +namespace frc { + +class SharpIR : public wpi::Sendable, public wpi::SendableHelper { + public: + /** + * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring + * distances from 20cm to 150cm. + * + * @param channel Analog input channel the sensor is connected to + * + * @return sensor object + */ + static SharpIR GP2Y0A02YK0F(int channel); + + /** + * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring + * distances from 10cm to 80cm. + * + * @param channel Analog input channel the sensor is connected to + * + * @return sensor object + */ + static SharpIR GP2Y0A21YK0F(int channel); + + /** + * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring + * distances from 4cm to 30cm. + * + * @param channel Analog input channel the sensor is connected to + * + * @return sensor object + */ + static SharpIR GP2Y0A41SK0F(int channel); + + /** + * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring + * distances from 2cm to 15cm. + * + * @param channel Analog input channel the sensor is connected to + * + * @return sensor object + */ + static SharpIR GP2Y0A51SK0F(int channel); + + /** + * Manually construct a SharpIR object. The distance is computed using this + * formula: A*v ^ B. Prefer to use one of the static factories to create this + * device instead. + * + * @param channel Analog input channel the sensor is connected to + * @param a Constant A + * @param b Constant B + * @param minCM Minimum distance to report in centimeters + * @param maxCM Maximum distance to report in centimeters + */ + SharpIR(int channel, double a, double b, double minCM, double maxCM); + + /** + * Get the analog input channel number. + * + * @return analog input channel + */ + int GetChannel() const; + + /** + * Get the range from the distance sensor. + * + * @return range of the target returned by the sensor + */ + units::centimeter_t GetRange() const; + + void InitSendable(wpi::SendableBuilder& builder) override; + + private: + AnalogInput m_sensor; + + hal::SimDevice m_simDevice; + hal::SimDouble m_simRange; + + double m_A; + double m_B; + double m_minCM; + double m_maxCM; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h b/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h new file mode 100644 index 0000000000..8ae43daf23 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/SharpIR.h" + +namespace frc { + +class SharpIRSim { + public: + explicit SharpIRSim(const SharpIR& sharpIR); + explicit SharpIRSim(int channel); + + void SetRange(units::centimeter_t rng); + + private: + hal::SimDouble m_simRange; +}; + +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/SharpIRTest.cpp b/wpilibc/src/test/native/cpp/SharpIRTest.cpp new file mode 100644 index 0000000000..e5445eb709 --- /dev/null +++ b/wpilibc/src/test/native/cpp/SharpIRTest.cpp @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/SharpIR.h" +#include "frc/simulation/SharpIRSim.h" + +using namespace frc; + +TEST(SharpIRTest, SimDevices) { + SharpIR s = SharpIR::GP2Y0A02YK0F(1); + SharpIRSim sim(s); + + EXPECT_EQ(20, s.GetRange().value()); + + sim.SetRange(units::centimeter_t{30}); + EXPECT_EQ(30, s.GetRange().value()); + + sim.SetRange(units::centimeter_t{300}); + EXPECT_EQ(150, s.GetRange().value()); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java new file mode 100644 index 0000000000..da89a28c02 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java @@ -0,0 +1,156 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; + +/** + * SharpIR analog distance sensor class. These distance measuring sensors output an analog voltage + * corresponding to the detection distance. + * + *

Teams are advised that the case of these sensors are conductive and grounded, so they should + * not be mounted on a metallic surface on an FRC robot. + */ +@SuppressWarnings("MethodName") +public class SharpIR implements Sendable, AutoCloseable { + private AnalogInput m_sensor; + + private SimDevice m_simDevice; + private SimDouble m_simRange; + + private final double m_A; + private final double m_B; + private final double m_minCM; + private final double m_maxCM; + + /** + * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20cm to 150cm. + * + * @param channel Analog input channel the sensor is connected to + * @return sensor object + */ + public static SharpIR GP2Y0A02YK0F(int channel) { + return new SharpIR(channel, 62.28, -1.092, 20, 150.0); + } + + /** + * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10cm to 80cm. + * + * @param channel Analog input channel the sensor is connected to + * @return sensor object + */ + public static SharpIR GP2Y0A21YK0F(int channel) { + return new SharpIR(channel, 26.449, -1.226, 10.0, 80.0); + } + + /** + * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4cm to 30cm. + * + * @param channel Analog input channel the sensor is connected to + * @return sensor object + */ + public static SharpIR GP2Y0A41SK0F(int channel) { + return new SharpIR(channel, 12.354, -1.07, 4.0, 30.0); + } + + /** + * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2cm to 15cm. + * + * @param channel Analog input channel the sensor is connected to + * @return sensor object + */ + public static SharpIR GP2Y0A51SK0F(int channel) { + return new SharpIR(channel, 5.2819, -1.161, 2.0, 15.0); + } + + /** + * Manually construct a SharpIR object. The distance is computed using this formula: A*v ^ B. + * Prefer to use one of the static factories to create this device instead. + * + * @param channel AnalogInput channel + * @param a Constant A + * @param b Constant B + * @param minCM Minimum distance to report in centimeters + * @param maxCM Maximum distance to report in centimeters + */ + public SharpIR(int channel, double a, double b, double minCM, double maxCM) { + m_sensor = new AnalogInput(channel); + + m_A = a; + m_B = b; + m_minCM = minCM; + m_maxCM = maxCM; + + SendableRegistry.addLW(this, "SharpIR", channel); + + m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel()); + if (m_simDevice != null) { + m_simRange = m_simDevice.createDouble("Range (cm)", Direction.kInput, 0.0); + m_sensor.setSimDevice(m_simDevice); + } + } + + @Override + public void close() { + SendableRegistry.remove(this); + m_sensor.close(); + m_sensor = null; + + if (m_simDevice != null) { + m_simDevice.close(); + m_simDevice = null; + m_simRange = null; + } + } + + /** + * Get the analog input channel number. + * + * @return analog input channel + */ + public int getChannel() { + return m_sensor.getChannel(); + } + + /** + * Get the range in inches from the distance sensor. + * + * @return range in inches of the target returned by the sensor + */ + public double getRangeInches() { + return getRangeCM() / 2.54; + } + + /** + * Get the range in centimeters from the distance sensor. + * + * @return range in centimeters of the target returned by the sensor + */ + public double getRangeCM() { + double distance; + + if (m_simRange != null) { + distance = m_simRange.get(); + } else { + // Don't allow zero/negative values + var v = Math.max(m_sensor.getVoltage(), 0.00001); + distance = m_A * Math.pow(v, m_B); + } + + // Always constrain output + return Math.max(Math.min(distance, m_maxCM), m_minCM); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Ultrasonic"); + builder.addDoubleProperty("Value", this::getRangeInches, null); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java new file mode 100644 index 0000000000..db4785a351 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.SharpIR; + +public class SharpIRSim { + private final SimDouble m_simRange; + + /** + * Constructor. + * + * @param sharpIR The real sensor to simulate + */ + public SharpIRSim(SharpIR sharpIR) { + this(sharpIR.getChannel()); + } + + /** + * Constructor. + * + * @param channel Analog channel for this sensor + */ + public SharpIRSim(int channel) { + SimDeviceSim simDevice = new SimDeviceSim("SharpIR", channel); + m_simRange = simDevice.getDouble("Range (cm)"); + } + + public void setRangeInches(double inches) { + m_simRange.set(Units.inchesToMeters(inches) * 100.0); + } + + public void setRangeCm(double cm) { + m_simRange.set(cm); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java new file mode 100644 index 0000000000..1ae35ae781 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.wpilibj.simulation.SharpIRSim; +import org.junit.jupiter.api.Test; + +class SharpIRTest { + @Test + void testSharpIR() { + try (SharpIR s = SharpIR.GP2Y0A02YK0F(1)) { + SharpIRSim sim = new SharpIRSim(s); + + assertEquals(20, s.getRangeCM()); + + sim.setRangeCm(30); + assertEquals(30, s.getRangeCM()); + + sim.setRangeCm(300); + assertEquals(150, s.getRangeCM()); + } + } +}