diff --git a/.idea/markdown.xml b/.idea/markdown.xml new file mode 100644 index 0000000..c61ea33 --- /dev/null +++ b/.idea/markdown.xml @@ -0,0 +1,8 @@ + + + + + + \ No newline at end of file diff --git a/.idea/migrations.xml b/.idea/migrations.xml new file mode 100644 index 0000000..f8051a6 --- /dev/null +++ b/.idea/migrations.xml @@ -0,0 +1,10 @@ + + + + + + \ No newline at end of file diff --git a/.idea/runConfigurations.xml b/.idea/runConfigurations.xml new file mode 100644 index 0000000..16660f1 --- /dev/null +++ b/.idea/runConfigurations.xml @@ -0,0 +1,17 @@ + + + + + + \ No newline at end of file diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackCRServo.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackCRServo.kt index d60f1ff..0e60a92 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackCRServo.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackCRServo.kt @@ -71,7 +71,13 @@ class NextFeedbackCRServo( feedbackName: String, cacheTolerance: Double = 0.01, ) : this( - { CRServoImplEx(LynxServoController(RobotController.appContext, module), port, ServoConfigurationType.getStandardServoType()) }, + { + CRServoImplEx( + LynxServoController(RobotController.appContext, module), + port, + ServoConfigurationType.getStandardServoType(), + ) + }, feedbackName, cacheTolerance, ) diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt new file mode 100644 index 0000000..630ad38 --- /dev/null +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt @@ -0,0 +1,144 @@ +/* + * 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]. */ + @JvmOverloads + fun getDistance(unit: DistanceUnit = DistanceUnit.CM): Double = + unit.fromUnit(DistanceUnit.CM, cachedDistanceCm) + + /** True if a distance sensor senses an object within [threshold] in the given [unit]. */ + @JvmOverloads + fun isWithinDistance(threshold: Double, unit: DistanceUnit = DistanceUnit.CM): Boolean { + val distance = getDistance(unit) + return !distance.isNaN() && distance <= threshold + } + + /** True if the cached color reading matches [profile]. */ + fun isColor(profile: ColorProfile): Boolean = profile.matches(cachedColor) + + /** True if the cached color reading matches [profile] and an object is within [threshold] in the given [unit]. */ + @JvmOverloads + fun isColorWithinDistance( + profile: ColorProfile, + threshold: Double, + unit: DistanceUnit = DistanceUnit.CM, + ): Boolean = isWithinDistance(threshold, unit) && isColor(profile) + + /** 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" + } +} diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt new file mode 100644 index 0000000..b0fdd73 --- /dev/null +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt @@ -0,0 +1,59 @@ +/* + * 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]. */ + @JvmOverloads + fun getDistance(unit: DistanceUnit = DistanceUnit.CM): Double = + unit.fromUnit(DistanceUnit.CM, cachedDistanceCm) + + /** True if a distance sensor senses an object within [threshold] in the given [unit]. */ + @JvmOverloads + fun isWithinDistance(threshold: Double, unit: DistanceUnit = DistanceUnit.CM): Boolean { + val distance = getDistance(unit) + return !distance.isNaN() && distance <= threshold + } +} diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/colors/ColorProfile.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/colors/ColorProfile.kt new file mode 100644 index 0000000..c0d434f --- /dev/null +++ b/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 + } +} diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/colors/NextColor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/colors/NextColor.kt new file mode 100644 index 0000000..aff04de --- /dev/null +++ b/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(), + ) + } + } +}