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 20dd422..654892b 100644 --- a/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt +++ b/hardware/src/main/kotlin/dev/nextftc/hardware/actuators/NextMotor.kt @@ -8,6 +8,7 @@ package dev.nextftc.hardware.actuators +import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.DcMotorImplEx import com.qualcomm.robotcore.hardware.DcMotorSimple import com.qualcomm.robotcore.hardware.Servo @@ -25,6 +26,7 @@ import dev.nextftc.units.measuretypes.Angle import dev.nextftc.units.measuretypes.AngularVelocity import dev.nextftc.units.radians import dev.nextftc.units.seconds +import dev.nextftc.units.volts import kotlin.math.sign import dev.nextftc.units.measuretypes.Voltage as VoltageMeasure @@ -173,6 +175,21 @@ class NextMotor @JvmOverloads constructor( } } + /** + * Motor zero power behavior (FLOAT or BRAKE). + * + * When set, automatically updates the underlying motor's zero power behavior. + */ + var zeroPowerBehavior = ZeroPowerBehavior.FLOAT + set(value) { + field = value + if (lazyMotor.isInitialized) { + motor.zeroPowerBehavior = value.sdkZeroPowerBehavior + } else { + lazyMotor.applyAfterInit { it.zeroPowerBehavior = value.sdkZeroPowerBehavior } + } + } + /** * Current encoder position in physical angle units. * @@ -231,7 +248,8 @@ class NextMotor @JvmOverloads constructor( ) + positionConstants.kS * positionPID.error.sign + positionConstants.kG + - positionConstants.kCos * kotlin.math.cos(encoderPosition.magnitude * positionConstants.kCosRatio) + positionConstants.kCos * + kotlin.math.cos(encoderPosition.magnitude * positionConstants.kCosRatio) } is ControlType.AbsolutePosition -> { val setpoint = mode.setpoint @@ -273,24 +291,24 @@ class NextMotor @JvmOverloads constructor( * Set throttle control mode and power. * * This is the simplest control mode: power is applied directly. - * - * @param throttle Power in the range [-1.0, 1.0]. Positive is forward. */ - fun setThrottle(throttle: Double) { - controlType = ControlType.Throttle(throttle) - } + var throttle: Double + get() = power + set(value) { + controlType = ControlType.Throttle(value) + } /** - * Set voltage control mode and voltage setpoint. - * - * Adjusts power to maintain the specified voltage, accounting for the - * current input rail voltage (to remain relatively hardware-independent). + * Returns the approximate motor voltage command. * - * @param voltage Target voltage setpoint. + * Calculated from the current motor power output and battery voltage. + * This is not a measured voltage. */ - fun setVoltage(voltage: VoltageMeasure) { - controlType = ControlType.Voltage(voltage) - } + var voltage: VoltageMeasure + get() = (power * RobotController.inputVoltage.magnitude).volts + set(value) { + controlType = ControlType.Voltage(value) + } /** * Set position control mode and position setpoint. @@ -403,6 +421,21 @@ class NextMotor @JvmOverloads constructor( REVERSE(DcMotorSimple.Direction.REVERSE, Servo.Direction.REVERSE), } + /** + * Motor zero power behavior. + */ + enum class ZeroPowerBehavior(val sdkZeroPowerBehavior: DcMotor.ZeroPowerBehavior) { + /** + * Motor stops and floats (no resistance after stopping). + */ + FLOAT(DcMotor.ZeroPowerBehavior.FLOAT), + + /** + * Motor stops and brakes (resists movement after stopping). + */ + BRAKE(DcMotor.ZeroPowerBehavior.BRAKE), + } + companion object { val motorEventLoop: EventLoop = EventLoop() } diff --git a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt index 70e2b97..21f24d8 100644 --- a/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt +++ b/robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt @@ -41,10 +41,10 @@ fun mecanumDrive( rx = gamepad.right_stick_x.toDouble() * scalar, ), ) - frontLeft.setThrottle(powers.frontLeft) - frontRight.setThrottle(powers.frontRight) - backLeft.setThrottle(powers.backLeft) - backRight.setThrottle(powers.backRight) + frontLeft.throttle = powers.frontLeft + frontRight.throttle = powers.frontRight + backLeft.throttle = powers.backLeft + backRight.throttle = powers.backRight } /** @@ -79,10 +79,10 @@ fun mecanumDriveFieldCentric( rx = gamepad.right_stick_x.toDouble() * scalar, ), ) - frontLeft.setThrottle(powers.frontLeft) - frontRight.setThrottle(powers.frontRight) - backLeft.setThrottle(powers.backLeft) - backRight.setThrottle(powers.backRight) + frontLeft.throttle = powers.frontLeft + frontRight.throttle = powers.frontRight + backLeft.throttle = powers.backLeft + backRight.throttle = powers.backRight } /** Command that drives a tank drivetrain from [gamepad]'s sticks. */ @@ -101,8 +101,8 @@ fun tankDrive( rx = gamepad.right_stick_x.toDouble() * scalar, ), ) - frontLeft.setThrottle(powers.left) - backLeft.setThrottle(powers.left) - frontRight.setThrottle(powers.right) - backRight.setThrottle(powers.right) + frontLeft.throttle = powers.left + backLeft.throttle = powers.left + frontRight.throttle = powers.right + backRight.throttle = powers.right }