Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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

Expand Down Expand Up @@ -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.
*
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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()
}
Expand Down
24 changes: 12 additions & 12 deletions robot/src/main/kotlin/dev/nextftc/robot/drive/DriveCommands.kt
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

/**
Expand Down Expand Up @@ -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. */
Expand All @@ -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
}
Loading