[wpilib] Add support for Sharp IR sensors (#6023)

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 <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#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);
+}

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 <hal/SimDevice.h>
+#include <units/length.h>
+
+#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());
+}

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 <hal/SimDevice.h>
+#include <units/length.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/AnalogInput.h"
+
+namespace frc {
+
+class SharpIR : public wpi::Sendable, public wpi::SendableHelper<SharpIR> {
+ 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

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 <hal/SimDevice.h>
+#include <units/length.h>
+
+#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

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 <gtest/gtest.h>
+
+#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());
+}

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.
+ *
+ * <p>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);
+  }
+}

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);
+  }
+}

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());
+    }
+  }
+}