Color and distance sensors

hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt

@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.hardware.sensors
+
+import android.graphics.Color
+import com.qualcomm.robotcore.hardware.DistanceSensor
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor
+import dev.nextftc.hardware.LazyHardware
+import dev.nextftc.hardware.RobotController
+import dev.nextftc.hardware.sensors.colors.ColorProfile
+import dev.nextftc.hardware.sensors.colors.NextColor
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
+
+/**
+ * Combines a color sensor and an optional distance sensor into one class.
+ * Call [update] each loop to read the hardware. Use [isColor] to check
+ * against a [dev.nextftc.hardware.sensors.colors.ColorProfile].
+ *
+ * Example:
+ * ```
+ * val green = ColorProfile(
+ *     space = ColorSpace.HSV,
+ *     color = NextColor.HSV(160f, 0.8f, 0.7f),
+ *     tolerance = NextColor.HSV(15f, 0.3f, 1f),
+ * )
+ *
+ * override fun periodic() {
+ *     sensor.update()
+ *     if (sensor.isWithinDistance(4.0) && sensor.isColor(green)) { ... }
+ * }
+ * ```
+ *
+ * Use [debug] in telemetry to calibrate [dev.nextftc.hardware.sensors.colors.ColorProfile]s.
+ *
+ * @param colorInitializer Lazily resolves the backing [NormalizedColorSensor].
+ * @param distanceInitializer Optional lazy distance sensor.
+ *
+ * @author 28shettr
+ */
+class NextColorDistanceSensor(
+  colorInitializer: () -> NormalizedColorSensor,
+  distanceInitializer: (() -> DistanceSensor)? = null,
+) {
+  @JvmOverloads
+  constructor(sensorName: String, hasDistance: Boolean = false) : this(
+    { RobotController.hardwareMap[sensorName] as NormalizedColorSensor },
+    if (hasDistance) {
+      { RobotController.hardwareMap[sensorName] as DistanceSensor }
+    } else {
+      null
+    },
+  )
+
+  private val colorSensor by LazyHardware(colorInitializer)
+  private val distanceSensor: DistanceSensor? by lazy { distanceInitializer?.invoke() }
+
+  private var cachedDistanceCm: Double = Double.NaN
+
+  private var cachedColor: NextColor = NextColor.rgb(0f, 0f, 0f)
+  private var cachedHsv: FloatArray = FloatArray(3)
+
+  /** Last cached reading as a [NextColor]. Black until [update] is called. */
+  val color: NextColor
+    get() = cachedColor
+
+  /** Last cached hue in degrees (0..360). */
+  val hue: Float get() = cachedHsv[0]
+
+  /** Last cached saturation (0..1). */
+  val saturation: Float get() = cachedHsv[1]
+
+  /** Last cached value/brightness (0..1). */
+  val value: Float get() = cachedHsv[2]
+
+  /** Gain applied to the color sensor. Higher values amplify readings for better detection at distance or in low light. Typical range is 1..4. */
+  var gain: Float
+    get() = colorSensor.gain
+    set(gain) {
+      colorSensor.gain = gain
+    }
+
+  /** Reads the color sensor (and distance sensor, if present) and refreshes the cache. Call this once per loop, before reading any properties. */
+  fun update() {
+    val c = colorSensor.normalizedColors
+    cachedColor = NextColor.rgb(c.red * 255, c.green * 255, c.blue * 255)
+
+    Color.RGBToHSV(
+      cachedColor.red.toInt(),
+      cachedColor.green.toInt(),
+      cachedColor.blue.toInt(),
+      cachedHsv,
+    )
+
+    cachedDistanceCm = distanceSensor?.getDistance(DistanceUnit.CM) ?: Double.NaN
+  }
+
+  /** Returns the last cached distance converted to the requested [unit]. */
+  fun distance(unit: DistanceUnit = DistanceUnit.CM): Double =
+    unit.fromUnit(DistanceUnit.CM, cachedDistanceCm)
+
+  /** True if a distance sensor is attached and an object is within [threshold] centimeters. */
+  fun isWithinDistance(threshold: Double, unit: DistanceUnit = DistanceUnit.CM): Boolean {
+    val distance = distance(unit)
+    return !distance.isNaN() && distance <= threshold
+  }
+
+  /** True if the cached HSV reading falls inside [profile]. */
+  fun isColor(profile: ColorProfile): Boolean = profile.matches(cachedColor)
+
+  /** Single-line telemetry string showing current HSV and distance. Useful for calibrating [ColorProfile]s. */
+  fun debug(): String {
+    val r = "%.0f".format(cachedColor.red)
+    val g = "%.0f".format(cachedColor.green)
+    val b = "%.0f".format(cachedColor.blue)
+
+    val h = "%.1f".format(hue)
+    val s = "%.2f".format(saturation)
+    val v = "%.2f".format(value)
+
+    val d = if (cachedDistanceCm.isNaN()) {
+      "n/a"
+    } else {
+      "%.2f".format(cachedDistanceCm)
+    }
+
+    return "RGB=($r,$g,$b) HSV=($h,$s,$v) Dist=$d"
+  }
+}

hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt

@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.hardware.sensors
+
+import com.qualcomm.robotcore.hardware.DistanceSensor
+import dev.nextftc.hardware.LazyHardware
+import dev.nextftc.hardware.RobotController
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
+/**
+ * Lightweight wrapper for a distance sensor that caches the last reading.
+ * Call [update] in periodic to read the hardware.
+ *
+ * Use [isWithinDistance] to check
+ * if an object is close enough.
+ *
+ * Example:
+ * ```
+ * override fun periodic() {
+ *     sensor.update()
+ *     if (sensor.isWithinDistance(6.7)) { ... }
+ * }
+ * ```
+ *
+ * @param initializer Lazily resolves the backing [DistanceSensor].
+ *
+ * @author 28shettr
+ */
+class NextDistanceSensor(initializer: () -> DistanceSensor) {
+  constructor(name: String) : this(
+    { RobotController.hardwareMap[name] as DistanceSensor },
+  )
+
+  private val distanceSensor by LazyHardware(initializer)
+
+  private var cachedDistanceCm: Double = Double.NaN
+
+  /** Reads the distance sensor and refreshes the cache. Call this once per loop, before reading any properties. */
+  fun update() {
+    cachedDistanceCm = distanceSensor.getDistance(DistanceUnit.CM)
+  }
+
+  /** Returns the last cached distance converted to the requested [unit]. Defaults to centimeters. */
+  fun distance(unit: DistanceUnit = DistanceUnit.CM): Double =
+    unit.fromUnit(DistanceUnit.CM, cachedDistanceCm)
+
+  /** True if an object is within [threshold] of the sensor. Defaults to centimeters. */
+  fun isWithinDistance(threshold: Double, unit: DistanceUnit = DistanceUnit.CM): Boolean {
+    val distance = distance(unit)
+    return !distance.isNaN() && distance <= threshold
+  }
+}

hardware/src/main/kotlin/dev/nextftc/hardware/sensors/colors/ColorProfile.kt

@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.hardware.sensors.colors
+
+import dev.nextftc.hardware.sensors.NextColorDistanceSensor
+import kotlin.math.abs
+
+enum class ColorSpace { RGB, HSV }
+
+/**
+ * Describes a target color and the per-channel tolerances used to decide whether a
+ * sensor reading is a match.
+ *
+ * Use [NextColorDistanceSensor.debug] in telemetry to read live HSV values and
+ * calibrate [color] and [tolerance]. [ColorSpace.HSV] is recommended for most cases
+ * as it is more stable under changing lighting conditions.
+ *
+ * Example:
+ * ```
+ * val green = ColorProfile(
+ *     space = ColorSpace.HSV,
+ *     color = NextColor.HSV(130f, 0.7f, 0.6f),
+ *     tolerance = NextColor.HSV(20f, 0.3f, 1f),
+ * )
+ *
+ * override fun periodic() {
+ *     sensor.update()
+ *     if (sensor.isColor(green)) { ... }
+ * }
+ * ```
+ *
+ * @property space     The color space to compare in.
+ * @property color     The target color to match against.
+ * @property tolerance How far each channel can deviate from [color] and still count as a match.
+ *
+ * @author 28shettr
+ */
+data class ColorProfile(val space: ColorSpace, val color: NextColor, val tolerance: NextColor) {
+
+  private val colorHsv = color.hsv
+  private val toleranceHsv = tolerance.hsv
+  private val colorRgb = color.rgb
+  private val toleranceRgb = tolerance.rgb
+
+  /** Returns `true` if [reading] falls within [tolerance] of [color] in [space]. */
+  fun matches(reading: NextColor): Boolean = when (space) {
+    ColorSpace.RGB -> matchesRgb(reading)
+    ColorSpace.HSV -> matchesHsv(reading)
+  }
+  private fun matchesRgb(input: NextColor): Boolean {
+    val c = colorRgb
+    val t = toleranceRgb
+    val i = input.rgb
+
+    return abs(i[0] - c[0]) <= t[0] &&
+      abs(i[1] - c[1]) <= t[1] &&
+      abs(i[2] - c[2]) <= t[2]
+  }
+
+  private fun matchesHsv(input: NextColor): Boolean {
+    val c = colorHsv
+    val t = toleranceHsv
+    val i = input.hsv
+    return wraparoundCheck(i[0], c[0]) <= t[0] &&
+      abs(i[1] - c[1]) <= t[1] &&
+      abs(i[2] - c[2]) <= t[2]
+  }
+
+  private fun wraparoundCheck(a: Float, b: Float): Float {
+    val diff = abs(a - b) % 360f
+    return if (diff > 180f) 360f - diff else diff
+  }
+}

hardware/src/main/kotlin/dev/nextftc/hardware/sensors/colors/NextColor.kt

@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2026 NextFTC Team
+ *
+ *  Use of this source code is governed by an BSD-3-clause
+ *  license that can be found in the LICENSE.md file at the root of this repository or at
+ *  https://opensource.org/license/bsd-3-clause.
+ */
+
+package dev.nextftc.hardware.sensors.colors
+
+import android.graphics.Color
+import dev.nextftc.hardware.sensors.NextColorDistanceSensor
+
+/**
+ * Stores a color. Use [rgb] if you have red, green, and blue values,
+ * or [hsv] if you have hue, saturation, and brightness values.
+ *
+ * You can use the [NextColorDistanceSensor.debug] to find these values.
+ * Reccomened to use [hsv] for most cases.
+ * Example:
+ * ```
+ * val lime = NextColor.HSV(100f, 0.9f, 0.85f)
+ * val sameColor = NextColor.RGB(lime.red, lime.green, lime.blue)
+ * ```
+ *
+ *
+ * @author 28shettr
+ */
+
+data class NextColor(val red: Float, val green: Float, val blue: Float) {
+  /** The color as a `[red, green, blue]` float array (0–255). */
+  val rgb: FloatArray
+    get() = floatArrayOf(red, green, blue)
+
+  /** The color as a `[hue, saturation, value]` float array (hue: 0–360, saturation/value: 0–1). */
+  val hsv: FloatArray
+    get() {
+      val out = FloatArray(3)
+      Color.RGBToHSV(red.toInt(), green.toInt(), blue.toInt(), out)
+      return out
+    }
+
+  companion object {
+
+    /**
+     * Creates a color from red, green, and blue values.
+     * You can use the [NextColorDistanceSensor.debug] to find these values.
+     *
+     * @param red   How much red (0–255).
+     * @param green How much green (0–255).
+     * @param blue  How much blue (0–255).
+     */
+    fun rgb(red: Float, green: Float, blue: Float): NextColor {
+      require(red in 0f..255f) { "value must be 0-255 got $red" }
+      require(green in 0f..255f) { "value must be 0-255 got $green" }
+      require(blue in 0f..255f) { "value must be 0-255 got $blue" }
+
+      return NextColor(red, green, blue)
+    }
+
+    /**
+     * Creates a color from hue, saturation, and brightness values.
+     * You can use the [NextColorDistanceSensor.debug] to find these values.
+     *
+     * @param hue        The color's position on the color wheel, in degrees (0–360).
+     * @param saturation How vivid the color is (0 = grey, 1 = fully vivid).
+     * @param value      How bright the color is (0 = black, 1 = full brightness).
+     */
+    fun hsv(hue: Float, saturation: Float, value: Float): NextColor {
+      require(hue in 0f..360f) { "value must be 0-360 got $hue" }
+      require(saturation in 0f..1f) { "value must be 0-1 got $saturation" }
+      require(value in 0f..1f) { "value must be 0-1 got $value" }
+
+      val rgbInt = Color.HSVToColor(floatArrayOf(hue, saturation, value))
+      return NextColor(
+        Color.red(rgbInt).toFloat(),
+        Color.green(rgbInt).toFloat(),
+        Color.blue(rgbInt).toFloat(),
+      )
+    }
+  }
+}