|
| 1 | +package org.firstinspires.ftc.teamcode.roadrunner.kinematics |
| 2 | + |
| 3 | +import com.acmerobotics.roadrunner.drive.Drive |
| 4 | +import com.acmerobotics.roadrunner.drive.DriveSignal |
| 5 | +import com.acmerobotics.roadrunner.geometry.Pose2d |
| 6 | +import com.acmerobotics.roadrunner.kinematics.Kinematics |
| 7 | +import com.acmerobotics.roadrunner.localization.Localizer |
| 8 | +import com.acmerobotics.roadrunner.util.Angle |
| 9 | +import java.lang.Math.PI |
| 10 | + |
| 11 | +/** |
| 12 | + * This class provides the basic functionality of a swerve drive using [SwerveKinematics]. |
| 13 | + * |
| 14 | + * @param kV velocity feedforward |
| 15 | + * @param kA acceleration feedforward |
| 16 | + * @param kStatic additive constant feedforward |
| 17 | + * @param trackWidth lateral distance between pairs of wheels on different sides of the robot |
| 18 | + * @param wheelBase distance between pairs of wheels on the same side of the robot |
| 19 | + */ |
| 20 | +abstract class SwerveDrive @JvmOverloads constructor( |
| 21 | + private val kV: Double, |
| 22 | + private val kA: Double, |
| 23 | + private val kStatic: Double, |
| 24 | + private val trackWidth: Double, |
| 25 | + private val wheelBase: Double = trackWidth |
| 26 | +) : Drive() { |
| 27 | + |
| 28 | + /** |
| 29 | + * Default localizer for swerve drives based on the drive encoder positions, module orientations, and (optionally) a |
| 30 | + * heading sensor. |
| 31 | + * |
| 32 | + * @param drive drive |
| 33 | + * @param useExternalHeading use external heading provided by an external sensor (e.g., IMU, gyroscope) |
| 34 | + */ |
| 35 | + class SwerveLocalizer @JvmOverloads constructor( |
| 36 | + private val drive: SwerveDrive, |
| 37 | + private val useExternalHeading: Boolean = true |
| 38 | + ) : Localizer { |
| 39 | + private var _poseEstimate = Pose2d() |
| 40 | + override var poseEstimate: Pose2d |
| 41 | + get() = _poseEstimate |
| 42 | + set(value) { |
| 43 | + lastWheelPositions = emptyList() |
| 44 | + lastExtHeading = Double.NaN |
| 45 | + if (useExternalHeading) drive.externalHeading = value.heading |
| 46 | + _poseEstimate = value |
| 47 | + } |
| 48 | + override var poseVelocity: Pose2d? = null |
| 49 | + private set |
| 50 | + private var lastWheelPositions = emptyList<Double>() |
| 51 | + private var lastExtHeading = Double.NaN |
| 52 | + |
| 53 | + override fun update() { |
| 54 | + val wheelPositions = drive.getWheelPositions() |
| 55 | + val moduleOrientations = drive.getModuleOrientations() |
| 56 | + val extHeading = if (useExternalHeading) drive.externalHeading else Double.NaN |
| 57 | + if (lastWheelPositions.isNotEmpty()) { |
| 58 | + val wheelDeltas = wheelPositions |
| 59 | + .zip(lastWheelPositions) |
| 60 | + .map { it.first - it.second } |
| 61 | + val robotPoseDelta = SwerveKinematics.wheelToRobotVelocities( |
| 62 | + wheelDeltas, |
| 63 | + moduleOrientations, |
| 64 | + drive.wheelBase, |
| 65 | + drive.trackWidth |
| 66 | + ) |
| 67 | + val finalHeadingDelta = if (useExternalHeading) { |
| 68 | + Angle.normDelta(extHeading - lastExtHeading) |
| 69 | + } else { |
| 70 | + robotPoseDelta.heading |
| 71 | + } |
| 72 | + _poseEstimate = Kinematics.relativeOdometryUpdate( |
| 73 | + _poseEstimate, |
| 74 | + Pose2d(robotPoseDelta.vec(), finalHeadingDelta) |
| 75 | + ) |
| 76 | + } |
| 77 | + |
| 78 | + val wheelVelocities = drive.getWheelVelocities() |
| 79 | + val extHeadingVel = drive.getExternalHeadingVelocity() |
| 80 | + if (wheelVelocities != null) { |
| 81 | + poseVelocity = SwerveKinematics.wheelToRobotVelocities( |
| 82 | + wheelVelocities, |
| 83 | + moduleOrientations, |
| 84 | + drive.wheelBase, |
| 85 | + drive.trackWidth |
| 86 | + ) |
| 87 | + if (useExternalHeading && extHeadingVel != null) { |
| 88 | + poseVelocity = Pose2d(poseVelocity!!.vec(), extHeadingVel) |
| 89 | + } |
| 90 | + } |
| 91 | + |
| 92 | + lastWheelPositions = wheelPositions |
| 93 | + lastExtHeading = extHeading |
| 94 | + } |
| 95 | + } |
| 96 | + |
| 97 | + override var localizer: Localizer = SwerveLocalizer(this) |
| 98 | + |
| 99 | + override fun setDriveSignal(driveSignal: DriveSignal) { |
| 100 | + val velocities = SwerveKinematics.robotToWheelVelocities( |
| 101 | + driveSignal.vel, |
| 102 | + trackWidth, |
| 103 | + wheelBase |
| 104 | + ) |
| 105 | + val accelerations = SwerveKinematics.robotToWheelAccelerations( |
| 106 | + driveSignal.vel, |
| 107 | + driveSignal.accel, |
| 108 | + trackWidth, |
| 109 | + wheelBase |
| 110 | + ) |
| 111 | + val powers = Kinematics.calculateMotorFeedforward(velocities, accelerations, kV, kA, kStatic) |
| 112 | + val orientations = SwerveKinematics.robotToModuleOrientations( |
| 113 | + driveSignal.vel, |
| 114 | + trackWidth, |
| 115 | + wheelBase |
| 116 | + ) |
| 117 | + setMotorPowers(powers[0], powers[1], powers[2], powers[3]) |
| 118 | + setModuleOrientations(orientations[0], orientations[1], orientations[2], orientations[3]) |
| 119 | + } |
| 120 | + |
| 121 | + override fun setDrivePower(drivePower: Pose2d) { |
| 122 | + val avg = (trackWidth + wheelBase) / 2.0 |
| 123 | + val powers = SwerveKinematics.robotToWheelVelocities(drivePower, trackWidth / avg, wheelBase / avg) |
| 124 | + val orientations = SwerveKinematics.robotToModuleOrientations(drivePower, trackWidth / avg, wheelBase / avg) |
| 125 | + setMotorPowers(powers[0], powers[1], powers[2], powers[3]) |
| 126 | + setModuleOrientations(orientations[0] - PI / 2, orientations[1] - PI / 2, orientations[2] - PI / 2, orientations[3] - PI / 2) |
| 127 | + } |
| 128 | + |
| 129 | + /** |
| 130 | + * Sets the following motor powers (normalized voltages). All arguments are on the interval `[-1.0, 1.0]`. |
| 131 | + */ |
| 132 | + abstract fun setMotorPowers(frontLeft: Double, rearLeft: Double, rearRight: Double, frontRight: Double) |
| 133 | + |
| 134 | + /** |
| 135 | + * Sets the module orientations. All values are in radians. |
| 136 | + */ |
| 137 | + abstract fun setModuleOrientations(frontLeft: Double, rearLeft: Double, rearRight: Double, frontRight: Double) |
| 138 | + |
| 139 | + /** |
| 140 | + * Returns the positions of the wheels in linear distance units. Positions should exactly match the ordering in |
| 141 | + * [setMotorPowers]. |
| 142 | + */ |
| 143 | + abstract fun getWheelPositions(): List<Double> |
| 144 | + |
| 145 | + /** |
| 146 | + * Returns the velocities of the wheels in linear distance units. Positions should exactly match the ordering in |
| 147 | + * [setMotorPowers]. |
| 148 | + */ |
| 149 | + open fun getWheelVelocities(): List<Double>? = null |
| 150 | + |
| 151 | + /** |
| 152 | + * Returns the current module orientations in radians. Orientations should exactly match the order in |
| 153 | + * [setModuleOrientations]. |
| 154 | + */ |
| 155 | + abstract fun getModuleOrientations(): List<Double> |
| 156 | +} |
0 commit comments