001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj; 006 007import edu.wpi.first.hal.HAL; 008import edu.wpi.first.hal.SimDevice; 009import edu.wpi.first.hal.SimDevice.Direction; 010import edu.wpi.first.hal.SimDouble; 011import edu.wpi.first.math.MathUtil; 012import edu.wpi.first.util.sendable.Sendable; 013import edu.wpi.first.util.sendable.SendableBuilder; 014import edu.wpi.first.util.sendable.SendableRegistry; 015 016/** 017 * SharpIR analog distance sensor class. These distance measuring sensors output an analog voltage 018 * corresponding to the detection distance. 019 * 020 * <p>Teams are advised that the case of these sensors are conductive and grounded, so they should 021 * not be mounted on a metallic surface on an FRC robot. 022 */ 023@SuppressWarnings("MethodName") 024public class SharpIR implements Sendable, AutoCloseable { 025 private AnalogInput m_sensor; 026 027 private SimDevice m_simDevice; 028 private SimDouble m_simRange; 029 030 private final double m_A; 031 private final double m_B; 032 private final double m_min; // m 033 private final double m_max; // m 034 035 /** 036 * Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20 cm to 150 cm. 037 * 038 * @param channel Analog input channel the sensor is connected to 039 * @return sensor object 040 */ 041 public static SharpIR GP2Y0A02YK0F(int channel) { 042 return new SharpIR(channel, 62.28, -1.092, 0.2, 1.5); 043 } 044 045 /** 046 * Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10 cm to 80 cm. 047 * 048 * @param channel Analog input channel the sensor is connected to 049 * @return sensor object 050 */ 051 public static SharpIR GP2Y0A21YK0F(int channel) { 052 return new SharpIR(channel, 26.449, -1.226, 0.1, 0.8); 053 } 054 055 /** 056 * Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4 cm to 30 cm. 057 * 058 * @param channel Analog input channel the sensor is connected to 059 * @return sensor object 060 */ 061 public static SharpIR GP2Y0A41SK0F(int channel) { 062 return new SharpIR(channel, 12.354, -1.07, 0.04, 0.3); 063 } 064 065 /** 066 * Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2 cm to 15 cm. 067 * 068 * @param channel Analog input channel the sensor is connected to 069 * @return sensor object 070 */ 071 public static SharpIR GP2Y0A51SK0F(int channel) { 072 return new SharpIR(channel, 5.2819, -1.161, 0.02, 0.15); 073 } 074 075 /** 076 * Manually construct a SharpIR object. The distance is computed using this formula: A*v ^ B. 077 * Prefer to use one of the static factories to create this device instead. 078 * 079 * @param channel AnalogInput channel 080 * @param a Constant A 081 * @param b Constant B 082 * @param min Minimum distance to report in meters 083 * @param max Maximum distance to report in meters 084 */ 085 @SuppressWarnings("this-escape") 086 public SharpIR(int channel, double a, double b, double min, double max) { 087 m_sensor = new AnalogInput(channel); 088 089 m_A = a; 090 m_B = b; 091 m_min = min; 092 m_max = max; 093 094 HAL.reportUsage("IO", channel, "SharpIR"); 095 SendableRegistry.add(this, "SharpIR", channel); 096 097 m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel()); 098 if (m_simDevice != null) { 099 m_simRange = m_simDevice.createDouble("Range (m)", Direction.kInput, 0.0); 100 m_sensor.setSimDevice(m_simDevice); 101 } 102 } 103 104 @Override 105 public void close() { 106 SendableRegistry.remove(this); 107 m_sensor.close(); 108 m_sensor = null; 109 110 if (m_simDevice != null) { 111 m_simDevice.close(); 112 m_simDevice = null; 113 m_simRange = null; 114 } 115 } 116 117 /** 118 * Get the analog input channel number. 119 * 120 * @return analog input channel 121 */ 122 public int getChannel() { 123 return m_sensor.getChannel(); 124 } 125 126 /** 127 * Get the range in meters from the distance sensor. 128 * 129 * @return range in meters of the target returned by the sensor 130 */ 131 public double getRange() { 132 if (m_simRange != null) { 133 return MathUtil.clamp(m_simRange.get(), m_min, m_max); 134 } else { 135 // Don't allow zero/negative values 136 var v = Math.max(m_sensor.getVoltage(), 0.00001); 137 138 return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max); 139 } 140 } 141 142 @Override 143 public void initSendable(SendableBuilder builder) { 144 builder.setSmartDashboardType("Ultrasonic"); 145 builder.addDoubleProperty("Value", this::getRange, null); 146 } 147}