diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt index edbe9ad3..73c791b6 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextCRServo.kt @@ -49,7 +49,9 @@ open class NextCRServo @JvmOverloads constructor( port: Int, cacheTolerance: Double = 0.01, ) : this( - { CRServoImplEx(module.servoController, port, ServoConfigurationType.getStandardServoType()) }, + { + module.crServo(port) + }, cacheTolerance, ) { require(port in 0..5) { "Expected port in range [0, 5], got $port" } 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 39be2e87..cf2c749d 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackCRServo.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackCRServo.kt @@ -73,11 +73,7 @@ class NextFeedbackCRServo @JvmOverloads constructor( cacheTolerance: Double = 0.01, ) : this( { - CRServoImplEx( - module.servoController, - port, - ServoConfigurationType.getStandardServoType(), - ) + module.crServo(port) }, { feedback }, cacheTolerance, diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackServo.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackServo.kt index 8782b169..ae0f09d1 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackServo.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextFeedbackServo.kt @@ -79,7 +79,9 @@ class NextFeedbackServo @JvmOverloads constructor( feedback: AnalogFeedback, cacheTolerance: Double = 0.01, ) : this( - { ServoImplEx(module.servoController, port, ServoConfigurationType.getStandardServoType()) }, + { + module.servo(port) + }, { feedback }, cacheTolerance, ) diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt index ff0b6f0f..575303a2 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt @@ -79,7 +79,9 @@ class NextMotor @JvmOverloads constructor( anglePerCount: Angle = 1.0.radians, cacheTolerance: Double = 0.01, ) : this( - { DcMotorImplEx(module.motorController, port) }, + { + module.motor(port) + }, anglePerCount, cacheTolerance, ) { @@ -100,7 +102,9 @@ class NextMotor @JvmOverloads constructor( anglePerCount: Angle = 1.0.radians, cacheTolerance: Double = 0.01, ) : this( - { RobotController.hardwareMap[name] as DcMotorImplEx }, + { + RobotController.hardwareMap[name] as DcMotorImplEx + }, anglePerCount, cacheTolerance, ) diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt index c3c50ec3..a4bd455c 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextServo.kt @@ -60,7 +60,9 @@ open class NextServo @JvmOverloads constructor( cacheTolerance: Double = 0.01, direction: NextMotor.Direction = NextMotor.Direction.FORWARD, ) : this( - { ServoImplEx(module.servoController, port, ServoConfigurationType.getStandardServoType()) }, + { + module.servo(port) + }, cacheTolerance, direction, ) { diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt index 76e8a9b0..5f7c344e 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/lynx/NextLynxModule.kt @@ -1,12 +1,21 @@ package dev.nextftc.hardware.lynx +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver import com.qualcomm.hardware.lynx.LynxAnalogInputController import com.qualcomm.hardware.lynx.LynxDcMotorController import com.qualcomm.hardware.lynx.LynxDigitalChannelController +import com.qualcomm.hardware.lynx.LynxI2cColorRangeSensor import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch import com.qualcomm.hardware.lynx.LynxI2cDeviceSynchV2 import com.qualcomm.hardware.lynx.LynxModule import com.qualcomm.hardware.lynx.LynxServoController +import com.qualcomm.robotcore.hardware.AnalogInput +import com.qualcomm.robotcore.hardware.CRServoImplEx +import com.qualcomm.robotcore.hardware.DcMotorImplEx +import com.qualcomm.robotcore.hardware.DigitalChannel +import com.qualcomm.robotcore.hardware.DigitalChannelImpl +import com.qualcomm.robotcore.hardware.ServoImplEx +import com.qualcomm.robotcore.hardware.configuration.typecontainers.ServoConfigurationType import dev.nextftc.hardware.RobotController import dev.nextftc.hardware.util.LazyHardware import dev.nextftc.units.celsius @@ -34,6 +43,24 @@ class NextLynxModule internal constructor(initializer: () -> LynxModule, @JvmFie LazyHardware { LynxI2cDeviceSynchV2(RobotController.appContext, module, bus) } } + enum class PortKind { + MOTOR, + SERVO, + I2C, + DIGITAL, + ANALOG, + } + + /** Tracks used ports. Resets each OpMode. */ + private val usedPorts by LazyHardware { HashSet>() } + + /** Throws if this port is already used on this hub. */ + private fun claim(kind: PortKind, port: Int) { + check(usedPorts.add(kind to port)) { + "Port conflict on $type: ${kind.name.lowercase()} port $port is used by more than one device" + } + } + /** Current module temperature. */ val temperature: Temperature get() = module.getTemperature(TempUnit.CELSIUS).celsius @@ -71,4 +98,46 @@ class NextLynxModule internal constructor(initializer: () -> LynxModule, @JvmFie val analogController: LynxAnalogInputController by LazyHardware { LynxAnalogInputController(RobotController.appContext, module) } + + /** Motor controller for [port], claiming the port. */ + internal fun motor(port: Int): DcMotorImplEx { + claim(PortKind.MOTOR, port) + return DcMotorImplEx(motorController, port) + } + + /** Servo controller for [port], claiming the port. */ + internal fun servo(port: Int): ServoImplEx { + claim(PortKind.SERVO, port) + return ServoImplEx(servoController, port, ServoConfigurationType.getStandardServoType()) + } + + /** CRServo controller for [port], claiming the port. */ + internal fun crServo(port: Int): CRServoImplEx { + claim(PortKind.SERVO, port) + return CRServoImplEx(servoController, port, ServoConfigurationType.getStandardServoType()) + } + + /** Creates a [LynxI2cColorRangeSensor] on [bus], claiming the bus. */ + internal fun colorRangeSensor(bus: Int): LynxI2cColorRangeSensor { + claim(PortKind.I2C, bus) + return LynxI2cColorRangeSensor(i2cController(bus), true) + } + + /** Creates a [GoBildaPinpointDriver] on [bus], claiming the bus. */ + internal fun pinpoint(bus: Int): GoBildaPinpointDriver { + claim(PortKind.I2C, bus) + return GoBildaPinpointDriver(i2cController(bus), true) + } + + /** Digital controller for [port], claiming the port. */ + internal fun digitalChannel(port: Int): DigitalChannel { + claim(PortKind.DIGITAL, port) + return DigitalChannelImpl(digitalController, port) + } + + /** Analog controller for [port], claiming the port. */ + internal fun analogChannel(port: Int): AnalogInput { + claim(PortKind.ANALOG, port) + return AnalogInput(analogController, port) + } } diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextAnalogInput.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextAnalogInput.kt index dc615488..36190882 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextAnalogInput.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextAnalogInput.kt @@ -79,7 +79,7 @@ open class NextAnalogInput @JvmOverloads constructor( customTransformation: (Double) -> Double = { n: Double -> n }, maxVoltage: Voltage = 3.3.volts, ) : this( - { AnalogInput(module.analogController, channel) }, + { module.analogChannel(channel) }, customTransformation, maxVoltage, ) { diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt index d8531a82..8b0ba560 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextColorDistanceSensor.kt @@ -83,9 +83,14 @@ class NextColorDistanceSensor @JvmOverloads constructor( bus: Int, hasDistance: Boolean = false, ) : this( - { LynxI2cColorRangeSensor(module.i2cController(bus), true) }, + { + module.colorRangeSensor(bus) + }, if (hasDistance) { - { LynxI2cColorRangeSensor(module.i2cController(bus), true) as DistanceSensor } + { + // Same device as the color sensor so allows it to claim same port. + LynxI2cColorRangeSensor(module.i2cController(bus), true) as DistanceSensor + } } else { null }, diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDigitalSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDigitalSensor.kt index 97025b11..dac326ad 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDigitalSensor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDigitalSensor.kt @@ -8,7 +8,6 @@ package dev.nextftc.hardware.sensors -import com.qualcomm.hardware.lynx.LynxModule import com.qualcomm.robotcore.hardware.DigitalChannel import com.qualcomm.robotcore.hardware.DigitalChannelImpl import dev.nextftc.hardware.RobotController @@ -56,7 +55,9 @@ class NextDigitalSensor @JvmOverloads constructor( port: Int, inverted: Boolean = true, ) : this( - { DigitalChannelImpl(module.digitalController, port) }, + { + module.digitalChannel(port) + }, inverted, ) { require(port in 0..7) { "Expected port in range [0, 7], got $port" } @@ -67,7 +68,9 @@ class NextDigitalSensor @JvmOverloads constructor( name: String, inverted: Boolean = true, ) : this( - { RobotController.hardwareMap[name] as DigitalChannel }, + { + RobotController.hardwareMap[name] as DigitalChannel + }, inverted, ) diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt index 5a0e6cb8..2e8c745c 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextDistanceSensor.kt @@ -56,7 +56,9 @@ class NextDistanceSensor(initializer: () -> DistanceSensor) { module: NextLynxModule, bus: Int, ) : this( - { LynxI2cColorRangeSensor(module.i2cController(bus), true) }, + { + module.colorRangeSensor(bus) + }, ) { require(bus in 0..3) { "Expected bus in range [0, 3], got $bus" } } diff --git a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextPinpoint.kt b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextPinpoint.kt index 5252ceb1..afc4d10d 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextPinpoint.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/sensors/NextPinpoint.kt @@ -32,7 +32,7 @@ class NextPinpoint(initializer: () -> GoBildaPinpointDriver) { bus: Int, ) : this( { - GoBildaPinpointDriver(module.i2cController(bus), true) + module.pinpoint(bus) }, ) { require(bus in 0..3) { "Expected bus in range [0, 3], got $bus" }