diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/drive/DualDeadwheelMecanumDrive.java b/src/main/java/org/murraybridgebunyips/bunyipslib/drive/DualDeadwheelMecanumDrive.java index 5b4fe4962..f845f45a6 100644 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/drive/DualDeadwheelMecanumDrive.java +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/drive/DualDeadwheelMecanumDrive.java @@ -7,9 +7,8 @@ import org.murraybridgebunyips.bunyipslib.roadrunner.drive.DriveConstants; import org.murraybridgebunyips.bunyipslib.roadrunner.drive.MecanumCoefficients; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelTrackingLocalizer; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.TwoWheelTrackingLocalizer; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.TwoWheelTrackingLocalizerCoefficients; +import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelLocalizer; +import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.TwoWheelLocalizer; import org.murraybridgebunyips.bunyipslib.roadrunner.util.Deadwheel; /** @@ -34,10 +33,10 @@ public class DualDeadwheelMecanumDrive extends MecanumDrive { * @param parallel the parallel deadwheel encoder * @param perpendicular the perpendicular deadwheel encoder */ - public DualDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients mecanumCoefficients, HardwareMap.DeviceMapping voltageSensor, IMU imu, DcMotorEx frontLeft, DcMotorEx frontRight, DcMotorEx backLeft, DcMotorEx backRight, TwoWheelTrackingLocalizerCoefficients localizerCoefficients, Deadwheel parallel, Deadwheel perpendicular) { + public DualDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients mecanumCoefficients, HardwareMap.DeviceMapping voltageSensor, IMU imu, DcMotorEx frontLeft, DcMotorEx frontRight, DcMotorEx backLeft, DcMotorEx backRight, TwoWheelLocalizer.Coefficients localizerCoefficients, Deadwheel parallel, Deadwheel perpendicular) { super(constants, mecanumCoefficients, voltageSensor, imu, frontLeft, frontRight, backLeft, backRight); if (!assertParamsNotNull(localizerCoefficients, parallel, perpendicular)) return; - setLocalizer(new TwoWheelTrackingLocalizer(localizerCoefficients, parallel, perpendicular, getInstance())); + setLocalizer(new TwoWheelLocalizer(localizerCoefficients, parallel, perpendicular, getInstance())); updatePoseFromMemory(); } @@ -47,7 +46,7 @@ public DualDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients m * @return this */ public DualDeadwheelMecanumDrive enableOverflowCompensation() { - ThreeWheelTrackingLocalizer localizer = (ThreeWheelTrackingLocalizer) getLocalizer(); + ThreeWheelLocalizer localizer = (ThreeWheelLocalizer) getLocalizer(); if (localizer != null) localizer.enableOverflowCompensation(); return this; diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/drive/TriDeadwheelMecanumDrive.java b/src/main/java/org/murraybridgebunyips/bunyipslib/drive/TriDeadwheelMecanumDrive.java index 7f86ae6ea..547565921 100644 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/drive/TriDeadwheelMecanumDrive.java +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/drive/TriDeadwheelMecanumDrive.java @@ -7,8 +7,7 @@ import org.murraybridgebunyips.bunyipslib.roadrunner.drive.DriveConstants; import org.murraybridgebunyips.bunyipslib.roadrunner.drive.MecanumCoefficients; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelTrackingLocalizer; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelTrackingLocalizerCoefficients; +import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelLocalizer; import org.murraybridgebunyips.bunyipslib.roadrunner.util.Deadwheel; import java.util.ArrayList; @@ -39,11 +38,11 @@ public class TriDeadwheelMecanumDrive extends MecanumDrive { * @param lastTrackingEncPositions The last tracking encoder positions * @param lastTrackingEncVels The last tracking encoder velocities */ - public TriDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients mecanumCoefficients, HardwareMap.DeviceMapping voltageSensor, IMU imu, DcMotorEx frontLeft, DcMotorEx frontRight, DcMotorEx backLeft, DcMotorEx backRight, ThreeWheelTrackingLocalizerCoefficients localizerCoefficients, Deadwheel enc_left, Deadwheel enc_right, Deadwheel enc_x, List lastTrackingEncPositions, List lastTrackingEncVels) { + public TriDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients mecanumCoefficients, HardwareMap.DeviceMapping voltageSensor, IMU imu, DcMotorEx frontLeft, DcMotorEx frontRight, DcMotorEx backLeft, DcMotorEx backRight, ThreeWheelLocalizer.Coefficients localizerCoefficients, Deadwheel enc_left, Deadwheel enc_right, Deadwheel enc_x, List lastTrackingEncPositions, List lastTrackingEncVels) { super(constants, mecanumCoefficients, voltageSensor, imu, frontLeft, frontRight, backLeft, backRight); if (!assertParamsNotNull(localizerCoefficients, enc_left, enc_right, enc_x, lastTrackingEncPositions, lastTrackingEncVels)) return; - setLocalizer(new ThreeWheelTrackingLocalizer(localizerCoefficients, enc_left, enc_right, enc_x, lastTrackingEncPositions, lastTrackingEncVels)); + setLocalizer(new ThreeWheelLocalizer(localizerCoefficients, enc_left, enc_right, enc_x, lastTrackingEncPositions, lastTrackingEncVels)); } /** @@ -62,10 +61,10 @@ public TriDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients me * @param enc_right The right y encoder * @param enc_x The x encoder */ - public TriDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients mecanumCoefficients, HardwareMap.DeviceMapping voltageSensor, IMU imu, DcMotorEx frontLeft, DcMotorEx frontRight, DcMotorEx backLeft, DcMotorEx backRight, ThreeWheelTrackingLocalizerCoefficients localizerCoefficients, Deadwheel enc_left, Deadwheel enc_right, Deadwheel enc_x) { + public TriDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients mecanumCoefficients, HardwareMap.DeviceMapping voltageSensor, IMU imu, DcMotorEx frontLeft, DcMotorEx frontRight, DcMotorEx backLeft, DcMotorEx backRight, ThreeWheelLocalizer.Coefficients localizerCoefficients, Deadwheel enc_left, Deadwheel enc_right, Deadwheel enc_x) { super(constants, mecanumCoefficients, voltageSensor, imu, frontLeft, frontRight, backLeft, backRight); if (!assertParamsNotNull(localizerCoefficients, enc_left, enc_right, enc_x)) return; - setLocalizer(new ThreeWheelTrackingLocalizer(localizerCoefficients, enc_left, enc_right, enc_x, new ArrayList<>(), new ArrayList<>())); + setLocalizer(new ThreeWheelLocalizer(localizerCoefficients, enc_left, enc_right, enc_x, new ArrayList<>(), new ArrayList<>())); updatePoseFromMemory(); } @@ -75,7 +74,7 @@ public TriDeadwheelMecanumDrive(DriveConstants constants, MecanumCoefficients me * @return this */ public TriDeadwheelMecanumDrive enableOverflowCompensation() { - ThreeWheelTrackingLocalizer localizer = (ThreeWheelTrackingLocalizer) getLocalizer(); + ThreeWheelLocalizer localizer = (ThreeWheelLocalizer) getLocalizer(); if (localizer != null) localizer.enableOverflowCompensation(); return this; diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelLocalizer.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelLocalizer.java new file mode 100644 index 000000000..481b4bb84 --- /dev/null +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelLocalizer.java @@ -0,0 +1,272 @@ +package org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers; + +import static org.murraybridgebunyips.bunyipslib.external.units.Units.Inches; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; + +import org.murraybridgebunyips.bunyipslib.external.units.Distance; +import org.murraybridgebunyips.bunyipslib.external.units.Measure; +import org.murraybridgebunyips.bunyipslib.roadrunner.util.Deadwheel; + +import java.util.Arrays; +import java.util.List; + +/** + * Standard 3 tracking wheel localizer implementation. + */ +/* + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + */ +public class ThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { + private final ThreeWheelLocalizer.Coefficients coefficients; + private final Deadwheel leftDeadwheel; + private final Deadwheel rightDeadwheel; + private final Deadwheel frontDeadwheel; + private final List lastEncPositions; + private final List lastEncVels; + private final double xMul; + private final double yMul; + private boolean usingOverflowCompensation; + + /** + * Create a new StandardTrackingWheelLocalizer with coefficients, encoders, and last encoder positions and velocities. + * + * @param coefficients The coefficients for the localizer + * @param leftDeadwheel The left encoder + * @param rightDeadwheel The right encoder + * @param frontDeadwheel The front encoder + * @param lastTrackingEncPositions The last encoder positions + * @param lastTrackingEncVels The last encoder velocities + */ + public ThreeWheelLocalizer(ThreeWheelLocalizer.Coefficients coefficients, Deadwheel leftDeadwheel, Deadwheel rightDeadwheel, Deadwheel frontDeadwheel, List lastTrackingEncPositions, List lastTrackingEncVels) { + super(Arrays.asList( + new Pose2d(0, coefficients.LATERAL_DISTANCE / 2, 0), // left + new Pose2d(0, -coefficients.LATERAL_DISTANCE / 2, 0), // right + new Pose2d(coefficients.FORWARD_OFFSET, 0, Math.toRadians(90)) // front + )); + + this.coefficients = coefficients; + + lastEncPositions = lastTrackingEncPositions; + lastEncVels = lastTrackingEncVels; + + assert leftDeadwheel != null && rightDeadwheel != null && frontDeadwheel != null; + + this.leftDeadwheel = leftDeadwheel; + this.rightDeadwheel = rightDeadwheel; + this.frontDeadwheel = frontDeadwheel; + + xMul = coefficients.X_MULTIPLIER; + yMul = coefficients.Y_MULTIPLIER; + } + + /** + * Enable overflow compensation if your encoders exceed 32767 counts / second. + * + * @return this + */ + public ThreeWheelLocalizer enableOverflowCompensation() { + usingOverflowCompensation = true; + return this; + } + + public ThreeWheelLocalizer.Coefficients getCoefficients() { + return coefficients; + } + + /** + * Convert encoder ticks to inches. + * + * @param ticks The encoder ticks + * @return The inches traveled + */ + public double encoderTicksToInches(double ticks) { + return coefficients.WHEEL_RADIUS * 2 * Math.PI * coefficients.GEAR_RATIO * ticks / coefficients.TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + int leftPos = leftDeadwheel.getCurrentPosition(); + int rightPos = rightDeadwheel.getCurrentPosition(); + int frontPos = frontDeadwheel.getCurrentPosition(); + + lastEncPositions.clear(); + lastEncPositions.add(leftPos); + lastEncPositions.add(rightPos); + lastEncPositions.add(frontPos); + + return Arrays.asList( + encoderTicksToInches(leftPos) * xMul, + encoderTicksToInches(rightPos) * xMul, + encoderTicksToInches(frontPos) * yMul + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + int leftVel = usingOverflowCompensation ? (int) leftDeadwheel.getCorrectedVelocity() : (int) leftDeadwheel.getRawVelocity(); + int rightVel = usingOverflowCompensation ? (int) rightDeadwheel.getCorrectedVelocity() : (int) rightDeadwheel.getRawVelocity(); + int frontVel = usingOverflowCompensation ? (int) frontDeadwheel.getCorrectedVelocity() : (int) frontDeadwheel.getRawVelocity(); + + lastEncVels.clear(); + lastEncVels.add(leftVel); + lastEncVels.add(rightVel); + lastEncVels.add(frontVel); + + return Arrays.asList( + encoderTicksToInches(leftVel) * xMul, + encoderTicksToInches(rightVel) * xMul, + encoderTicksToInches(frontVel) * yMul + ); + } + + /** + * Constants for RoadRunner standard tracking wheels. + * Reworked to use a builder for multiple robot configurations. + * + * @author Lucas Bubner, 2023 + */ + public static class Coefficients { + /** + * Ticks per revolution of the tracking wheel encoder. + */ + public double TICKS_PER_REV; + /** + * Radius of the tracking wheel. + */ + public double WHEEL_RADIUS = 2; // in + /** + * Gear ratio of the tracking wheel. + */ + public double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + /** + * Inches of lateral distance between the left and right wheels. + */ + public double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels + /** + * Forward offset of the lateral wheel to the center of rotation. + */ + public double FORWARD_OFFSET = 4; // in; offset of the lateral wheel + /** + * Multiplicative scale of the ticks from the x (forward) axis. + */ + public double X_MULTIPLIER = 1; + /** + * Multiplicative scale of the ticks from the y (strafe) axis. + */ + public double Y_MULTIPLIER = 1; + + /** + * Utility builder for creating new coefficients. + */ + public static class Builder { + + private final ThreeWheelLocalizer.Coefficients trackingWheelCoefficients; + + /** + * Start building. + */ + public Builder() { + trackingWheelCoefficients = new ThreeWheelLocalizer.Coefficients(); + } + + /** + * Set the ticks per revolution of the tracking wheel encoder. + * + * @param ticksPerRev The ticks per revolution + * @return The builder + */ + public Builder setTicksPerRev(double ticksPerRev) { + trackingWheelCoefficients.TICKS_PER_REV = ticksPerRev; + return this; + } + + /** + * Set the radius of the tracking wheel. + * + * @param wheelRadius The radius of the tracking wheel + * @return The builder + */ + public Builder setWheelRadius(Measure wheelRadius) { + trackingWheelCoefficients.WHEEL_RADIUS = wheelRadius.in(Inches); + return this; + } + + /** + * Set the gear ratio of the tracking wheel. + * + * @param gearRatio The gear ratio of the tracking wheel (output wheel speed / input encoder speed) + * @return The builder + */ + public Builder setGearRatio(double gearRatio) { + trackingWheelCoefficients.GEAR_RATIO = gearRatio; + return this; + } + + /** + * Set the lateral distance between the left and right tracking wheels. + * + * @param lateralDistance The lateral distance between the left and right wheels + * @return The builder + */ + public Builder setLateralDistance(Measure lateralDistance) { + trackingWheelCoefficients.LATERAL_DISTANCE = lateralDistance.in(Inches); + return this; + } + + /** + * Set the forward offset of the lateral tracking wheel to the center of rotation. + * + * @param forwardOffset The forward offset of the lateral tracking wheel + * @return The builder + */ + public Builder setForwardOffset(Measure forwardOffset) { + trackingWheelCoefficients.FORWARD_OFFSET = forwardOffset.in(Inches); + return this; + } + + /** + * Set the forward (x) multiplier for the ticks reported by the forward deadwheels. + * + * @param forwardMul the multiplier + * @return The builder + */ + public Builder setXMultiplier(double forwardMul) { + trackingWheelCoefficients.X_MULTIPLIER = forwardMul; + return this; + } + + /** + * Set the strafe (y) multiplier for the ticks reported by the side deadwheel. + * + * @param strafeMul the multiplier + * @return The builder + */ + public Builder setYMultiplier(double strafeMul) { + trackingWheelCoefficients.Y_MULTIPLIER = strafeMul; + return this; + } + + /** + * Build the coefficients. + * + * @return The coefficients + */ + public ThreeWheelLocalizer.Coefficients build() { + return trackingWheelCoefficients; + } + } + } +} diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelTrackingLocalizer.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelTrackingLocalizer.java deleted file mode 100644 index 968a99f2c..000000000 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelTrackingLocalizer.java +++ /dev/null @@ -1,130 +0,0 @@ -package org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers; - -import androidx.annotation.NonNull; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; - -import org.murraybridgebunyips.bunyipslib.roadrunner.util.Deadwheel; - -import java.util.Arrays; -import java.util.List; - -/** - * Standard 3 tracking wheel localizer implementation. - */ -/* - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | - * | | - * | | - * \--------------/ - */ -public class ThreeWheelTrackingLocalizer extends ThreeTrackingWheelLocalizer { - private final ThreeWheelTrackingLocalizerCoefficients coefficients; - private final Deadwheel leftDeadwheel; - private final Deadwheel rightDeadwheel; - private final Deadwheel frontDeadwheel; - private final List lastEncPositions; - private final List lastEncVels; - private final double xMul; - private final double yMul; - private boolean usingOverflowCompensation; - - /** - * Create a new StandardTrackingWheelLocalizer with coefficients, encoders, and last encoder positions and velocities. - * - * @param coefficients The coefficients for the localizer - * @param leftDeadwheel The left encoder - * @param rightDeadwheel The right encoder - * @param frontDeadwheel The front encoder - * @param lastTrackingEncPositions The last encoder positions - * @param lastTrackingEncVels The last encoder velocities - */ - public ThreeWheelTrackingLocalizer(ThreeWheelTrackingLocalizerCoefficients coefficients, Deadwheel leftDeadwheel, Deadwheel rightDeadwheel, Deadwheel frontDeadwheel, List lastTrackingEncPositions, List lastTrackingEncVels) { - super(Arrays.asList( - new Pose2d(0, coefficients.LATERAL_DISTANCE / 2, 0), // left - new Pose2d(0, -coefficients.LATERAL_DISTANCE / 2, 0), // right - new Pose2d(coefficients.FORWARD_OFFSET, 0, Math.toRadians(90)) // front - )); - - this.coefficients = coefficients; - - lastEncPositions = lastTrackingEncPositions; - lastEncVels = lastTrackingEncVels; - - assert leftDeadwheel != null && rightDeadwheel != null && frontDeadwheel != null; - - this.leftDeadwheel = leftDeadwheel; - this.rightDeadwheel = rightDeadwheel; - this.frontDeadwheel = frontDeadwheel; - - xMul = coefficients.X_MULTIPLIER; - yMul = coefficients.Y_MULTIPLIER; - } - - /** - * Enable overflow compensation if your encoders exceed 32767 counts / second. - * - * @return this - */ - public ThreeWheelTrackingLocalizer enableOverflowCompensation() { - usingOverflowCompensation = true; - return this; - } - - public ThreeWheelTrackingLocalizerCoefficients getCoefficients() { - return coefficients; - } - - /** - * Convert encoder ticks to inches. - * - * @param ticks The encoder ticks - * @return The inches traveled - */ - public double encoderTicksToInches(double ticks) { - return coefficients.WHEEL_RADIUS * 2 * Math.PI * coefficients.GEAR_RATIO * ticks / coefficients.TICKS_PER_REV; - } - - @NonNull - @Override - public List getWheelPositions() { - int leftPos = leftDeadwheel.getCurrentPosition(); - int rightPos = rightDeadwheel.getCurrentPosition(); - int frontPos = frontDeadwheel.getCurrentPosition(); - - lastEncPositions.clear(); - lastEncPositions.add(leftPos); - lastEncPositions.add(rightPos); - lastEncPositions.add(frontPos); - - return Arrays.asList( - encoderTicksToInches(leftPos) * xMul, - encoderTicksToInches(rightPos) * xMul, - encoderTicksToInches(frontPos) * yMul - ); - } - - @NonNull - @Override - public List getWheelVelocities() { - int leftVel = usingOverflowCompensation ? (int) leftDeadwheel.getCorrectedVelocity() : (int) leftDeadwheel.getRawVelocity(); - int rightVel = usingOverflowCompensation ? (int) rightDeadwheel.getCorrectedVelocity() : (int) rightDeadwheel.getRawVelocity(); - int frontVel = usingOverflowCompensation ? (int) frontDeadwheel.getCorrectedVelocity() : (int) frontDeadwheel.getRawVelocity(); - - lastEncVels.clear(); - lastEncVels.add(leftVel); - lastEncVels.add(rightVel); - lastEncVels.add(frontVel); - - return Arrays.asList( - encoderTicksToInches(leftVel) * xMul, - encoderTicksToInches(rightVel) * xMul, - encoderTicksToInches(frontVel) * yMul - ); - } -} diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelTrackingLocalizerCoefficients.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelTrackingLocalizerCoefficients.java deleted file mode 100644 index e4beaa543..000000000 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/ThreeWheelTrackingLocalizerCoefficients.java +++ /dev/null @@ -1,145 +0,0 @@ -package org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers; - -import static org.murraybridgebunyips.bunyipslib.external.units.Units.Inches; - -import org.murraybridgebunyips.bunyipslib.external.units.Distance; -import org.murraybridgebunyips.bunyipslib.external.units.Measure; - -/** - * Constants for RoadRunner standard tracking wheels. - * Reworked to use a builder for multiple robot configurations. - * - * @author Lucas Bubner, 2023 - */ -public class ThreeWheelTrackingLocalizerCoefficients { - - /** - * Ticks per revolution of the tracking wheel encoder. - */ - public double TICKS_PER_REV; - /** - * Radius of the tracking wheel. - */ - public double WHEEL_RADIUS = 2; // in - /** - * Gear ratio of the tracking wheel. - */ - public double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - /** - * Inches of lateral distance between the left and right wheels. - */ - public double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels - /** - * Forward offset of the lateral wheel to the center of rotation. - */ - public double FORWARD_OFFSET = 4; // in; offset of the lateral wheel - /** - * Multiplicative scale of the ticks from the x (forward) axis. - */ - public double X_MULTIPLIER = 1; - /** - * Multiplicative scale of the ticks from the y (strafe) axis. - */ - public double Y_MULTIPLIER = 1; - - /** - * Utility builder for creating new coefficients. - */ - public static class Builder { - - private final ThreeWheelTrackingLocalizerCoefficients trackingWheelCoefficients; - - /** - * Start building. - */ - public Builder() { - trackingWheelCoefficients = new ThreeWheelTrackingLocalizerCoefficients(); - } - - /** - * Set the ticks per revolution of the tracking wheel encoder. - * - * @param ticksPerRev The ticks per revolution - * @return The builder - */ - public Builder setTicksPerRev(double ticksPerRev) { - trackingWheelCoefficients.TICKS_PER_REV = ticksPerRev; - return this; - } - - /** - * Set the radius of the tracking wheel. - * - * @param wheelRadius The radius of the tracking wheel - * @return The builder - */ - public Builder setWheelRadius(Measure wheelRadius) { - trackingWheelCoefficients.WHEEL_RADIUS = wheelRadius.in(Inches); - return this; - } - - /** - * Set the gear ratio of the tracking wheel. - * - * @param gearRatio The gear ratio of the tracking wheel (output wheel speed / input encoder speed) - * @return The builder - */ - public Builder setGearRatio(double gearRatio) { - trackingWheelCoefficients.GEAR_RATIO = gearRatio; - return this; - } - - /** - * Set the lateral distance between the left and right tracking wheels. - * - * @param lateralDistance The lateral distance between the left and right wheels - * @return The builder - */ - public Builder setLateralDistance(Measure lateralDistance) { - trackingWheelCoefficients.LATERAL_DISTANCE = lateralDistance.in(Inches); - return this; - } - - /** - * Set the forward offset of the lateral tracking wheel to the center of rotation. - * - * @param forwardOffset The forward offset of the lateral tracking wheel - * @return The builder - */ - public Builder setForwardOffset(Measure forwardOffset) { - trackingWheelCoefficients.FORWARD_OFFSET = forwardOffset.in(Inches); - return this; - } - - /** - * Set the forward (x) multiplier for the ticks reported by the forward deadwheels. - * - * @param forwardMul the multiplier - * @return The builder - */ - public Builder setXMultiplier(double forwardMul) { - trackingWheelCoefficients.X_MULTIPLIER = forwardMul; - return this; - } - - /** - * Set the strafe (y) multiplier for the ticks reported by the side deadwheel. - * - * @param strafeMul the multiplier - * @return The builder - */ - public Builder setYMultiplier(double strafeMul) { - trackingWheelCoefficients.Y_MULTIPLIER = strafeMul; - return this; - } - - /** - * Build the coefficients. - * - * @return The coefficients - */ - public ThreeWheelTrackingLocalizerCoefficients build() { - return trackingWheelCoefficients; - } - } -} diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelLocalizer.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelLocalizer.java new file mode 100644 index 000000000..d9ac68f81 --- /dev/null +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelLocalizer.java @@ -0,0 +1,301 @@ +package org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers; + +import static org.murraybridgebunyips.bunyipslib.external.units.Units.Inches; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer; + +import org.murraybridgebunyips.bunyipslib.external.units.Distance; +import org.murraybridgebunyips.bunyipslib.external.units.Measure; +import org.murraybridgebunyips.bunyipslib.roadrunner.drive.RoadRunnerDrive; +import org.murraybridgebunyips.bunyipslib.roadrunner.util.Deadwheel; + +import java.util.Arrays; +import java.util.List; + +/** + * Dual tracking wheel localizer implementation. + */ +/* + * Tracking wheel localizer implementation assuming the standard configuration: + * + * ^ + * | + * | ( x direction) + * | + * v + * <----( y direction )----> + + * (forward) + * /--------------\ + * | ____ | + * | ---- | <- Perpendicular Wheel + * | || | + * | || | <- Parallel Wheel + * | | + * | | + * \--------------/ + */ +public class TwoWheelLocalizer extends TwoTrackingWheelLocalizer { + private final TwoWheelLocalizer.Coefficients coefficients; + + // Parallel/Perpendicular to the forward axis + // Parallel wheel is parallel to the forward axis + // Perpendicular is perpendicular to the forward axis + private final Deadwheel parallelDeadwheel; + private final Deadwheel perpendicularDeadwheel; + private final double xMul; + private final double yMul; + private final RoadRunnerDrive drive; + private boolean usingOverflowCompensation; + + /** + * Create a new TwoWheelTrackingLocalizer + * + * @param coefficients The coefficients to use + * @param parallelDeadwheel The parallel encoder + * @param perpendicularDeadwheel The perpendicular encoder + * @param drive The drivetrain + */ + public TwoWheelLocalizer(TwoWheelLocalizer.Coefficients coefficients, Deadwheel parallelDeadwheel, Deadwheel perpendicularDeadwheel, RoadRunnerDrive drive) { + super(Arrays.asList( + new Pose2d(coefficients.PARALLEL_X, coefficients.PARALLEL_Y, 0), + new Pose2d(coefficients.PERPENDICULAR_X, coefficients.PERPENDICULAR_Y, Math.toRadians(90)) + )); + + this.drive = drive; + this.coefficients = coefficients; + + this.parallelDeadwheel = parallelDeadwheel; + this.perpendicularDeadwheel = perpendicularDeadwheel; + + xMul = coefficients.X_MULTIPLIER; + yMul = coefficients.Y_MULTIPLIER; + } + + /** + * Enable overflow compensation if your encoders exceed 32767 counts / second. + * + * @return this + */ + public TwoWheelLocalizer enableOverflowCompensation() { + usingOverflowCompensation = true; + return this; + } + + public TwoWheelLocalizer.Coefficients getCoefficients() { + return coefficients; + } + + /** + * Convert encoder ticks to inches + * + * @param ticks The ticks to convert + * @return The inches traveled + */ + public double encoderTicksToInches(double ticks) { + return coefficients.WHEEL_RADIUS * 2 * Math.PI * coefficients.GEAR_RATIO * ticks / coefficients.TICKS_PER_REV; + } + + @Override + public double getHeading() { + return drive.getRawExternalHeading(); + } + + @Override + public Double getHeadingVelocity() { + return drive.getExternalHeadingVelocity(); + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(parallelDeadwheel.getCurrentPosition()) * xMul, + encoderTicksToInches(perpendicularDeadwheel.getCurrentPosition()) * yMul + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + // If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other + // high resolution encoders), enable overflow compensation with enableOverflowCompensation. + return Arrays.asList( + encoderTicksToInches(usingOverflowCompensation ? parallelDeadwheel.getCorrectedVelocity() : parallelDeadwheel.getRawVelocity()) * xMul, + encoderTicksToInches(usingOverflowCompensation ? perpendicularDeadwheel.getCorrectedVelocity() : perpendicularDeadwheel.getRawVelocity()) * yMul + ); + } + + /** + * Coefficients for RoadRunner two wheel tracking localizer. + * Reworked to use a builder for multiple robot configurations. + * + * @author Lucas Bubner, 2023 + */ + public static class Coefficients { + /** + * The number of ticks per revolution of the encoder + */ + public double TICKS_PER_REV; + /** + * The radius of the tracking wheel in inches + */ + public double WHEEL_RADIUS = 2; // in + /** + * The gear ratio of the tracking wheel, (output wheel speed / input encoder speed) + */ + public double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + /** + * Position in the forward direction of the parallel wheel in inches + */ + public double PARALLEL_X; // X is the up and down direction + /** + * Position in the strafe direction of the parallel wheel in inches + */ + public double PARALLEL_Y; // Y is the strafe direction + + /** + * Position in the forward direction of the perpendicular wheel in inches + */ + public double PERPENDICULAR_X; + /** + * Position in the strafe direction of the perpendicular wheel in inches + */ + public double PERPENDICULAR_Y; + /** + * Multiplicative scale of the ticks from the x (forward) axis. + */ + public double X_MULTIPLIER = 1; + /** + * Multiplicative scale of the ticks from the y (strafe) axis. + */ + public double Y_MULTIPLIER = 1; + + /** + * Utility class for building TwoWheelLocalizer.Coefficients + */ + public static class Builder { + + private final TwoWheelLocalizer.Coefficients twoWheelTrackingCoefficients; + + /** + * Start building. + */ + public Builder() { + twoWheelTrackingCoefficients = new TwoWheelLocalizer.Coefficients(); + } + + /** + * Set the ticks per revolution of the tracking wheel encoder. + * + * @param ticksPerRev The ticks per revolution + * @return The builder + */ + public Builder setTicksPerRev(double ticksPerRev) { + twoWheelTrackingCoefficients.TICKS_PER_REV = ticksPerRev; + return this; + } + + /** + * Set the radius of the tracking wheel. + * + * @param wheelRadius The radius of the tracking wheel + * @return The builder + */ + public Builder setWheelRadius(Measure wheelRadius) { + twoWheelTrackingCoefficients.WHEEL_RADIUS = wheelRadius.in(Inches); + return this; + } + + /** + * Set the gear ratio of the tracking wheel. + * + * @param gearRatio The gear ratio of the tracking wheel (output wheel speed / input encoder speed) + * @return The builder + */ + public Builder setGearRatio(double gearRatio) { + twoWheelTrackingCoefficients.GEAR_RATIO = gearRatio; + return this; + } + + /** + * Set the position of the parallel wheel in the forward direction. + * + * @param parallelX The position of the parallel wheel in the forward direction from the center of rotation + * @return The builder + */ + public Builder setParallelX(Measure parallelX) { + twoWheelTrackingCoefficients.PARALLEL_X = parallelX.in(Inches); + return this; + } + + /** + * Set the position of the parallel wheel in the strafe direction. + * + * @param parallelY The position of the parallel wheel in the strafe direction from the center of rotation + * @return The builder + */ + public Builder setParallelY(Measure parallelY) { + twoWheelTrackingCoefficients.PARALLEL_Y = parallelY.in(Inches); + return this; + } + + /** + * Set the position of the perpendicular wheel in the forward direction. + * + * @param perpendicularX The position of the perpendicular wheel in the forward direction from the center of rotation + * @return The builder + */ + public Builder setPerpendicularX(Measure perpendicularX) { + twoWheelTrackingCoefficients.PERPENDICULAR_X = perpendicularX.in(Inches); + return this; + } + + /** + * Set the position of the perpendicular wheel in the strafe direction. + * + * @param perpendicularY The position of the perpendicular wheel in the strafe direction from the center of rotation + * @return The builder + */ + public Builder setPerpendicularY(Measure perpendicularY) { + twoWheelTrackingCoefficients.PERPENDICULAR_Y = perpendicularY.in(Inches); + return this; + } + + /** + * Set the forward (x) multiplier for the ticks reported by the forward deadwheels. + * + * @param forwardMul the multiplier + * @return The builder + */ + public TwoWheelLocalizer.Coefficients.Builder setXMultiplier(double forwardMul) { + twoWheelTrackingCoefficients.X_MULTIPLIER = forwardMul; + return this; + } + + /** + * Set the strafe (y) multiplier for the ticks reported by the side deadwheel. + * + * @param strafeMul the multiplier + * @return The builder + */ + public TwoWheelLocalizer.Coefficients.Builder setYMultiplier(double strafeMul) { + twoWheelTrackingCoefficients.Y_MULTIPLIER = strafeMul; + return this; + } + + /** + * Build the coefficients. + * + * @return The coefficients + */ + public TwoWheelLocalizer.Coefficients build() { + return twoWheelTrackingCoefficients; + } + } + } +} \ No newline at end of file diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelTrackingLocalizer.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelTrackingLocalizer.java deleted file mode 100644 index e5c32d526..000000000 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelTrackingLocalizer.java +++ /dev/null @@ -1,127 +0,0 @@ -package org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers; - -import androidx.annotation.NonNull; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer; - -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.RoadRunnerDrive; -import org.murraybridgebunyips.bunyipslib.roadrunner.util.Deadwheel; - -import java.util.Arrays; -import java.util.List; - -/** - * Dual tracking wheel localizer implementation. - */ -/* - * Tracking wheel localizer implementation assuming the standard configuration: - * - * ^ - * | - * | ( x direction) - * | - * v - * <----( y direction )----> - - * (forward) - * /--------------\ - * | ____ | - * | ---- | <- Perpendicular Wheel - * | || | - * | || | <- Parallel Wheel - * | | - * | | - * \--------------/ - */ -public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { - private final TwoWheelTrackingLocalizerCoefficients coefficients; - - // Parallel/Perpendicular to the forward axis - // Parallel wheel is parallel to the forward axis - // Perpendicular is perpendicular to the forward axis - private final Deadwheel parallelDeadwheel; - private final Deadwheel perpendicularDeadwheel; - private final double xMul; - private final double yMul; - private final RoadRunnerDrive drive; - private boolean usingOverflowCompensation; - - /** - * Create a new TwoWheelTrackingLocalizer - * - * @param coefficients The coefficients to use - * @param parallelDeadwheel The parallel encoder - * @param perpendicularDeadwheel The perpendicular encoder - * @param drive The drivetrain - */ - public TwoWheelTrackingLocalizer(TwoWheelTrackingLocalizerCoefficients coefficients, Deadwheel parallelDeadwheel, Deadwheel perpendicularDeadwheel, RoadRunnerDrive drive) { - super(Arrays.asList( - new Pose2d(coefficients.PARALLEL_X, coefficients.PARALLEL_Y, 0), - new Pose2d(coefficients.PERPENDICULAR_X, coefficients.PERPENDICULAR_Y, Math.toRadians(90)) - )); - - this.drive = drive; - this.coefficients = coefficients; - - this.parallelDeadwheel = parallelDeadwheel; - this.perpendicularDeadwheel = perpendicularDeadwheel; - - xMul = coefficients.X_MULTIPLIER; - yMul = coefficients.Y_MULTIPLIER; - } - - /** - * Enable overflow compensation if your encoders exceed 32767 counts / second. - * - * @return this - */ - public TwoWheelTrackingLocalizer enableOverflowCompensation() { - usingOverflowCompensation = true; - return this; - } - - public TwoWheelTrackingLocalizerCoefficients getCoefficients() { - return coefficients; - } - - /** - * Convert encoder ticks to inches - * - * @param ticks The ticks to convert - * @return The inches traveled - */ - public double encoderTicksToInches(double ticks) { - return coefficients.WHEEL_RADIUS * 2 * Math.PI * coefficients.GEAR_RATIO * ticks / coefficients.TICKS_PER_REV; - } - - @Override - public double getHeading() { - return drive.getRawExternalHeading(); - } - - @Override - public Double getHeadingVelocity() { - return drive.getExternalHeadingVelocity(); - } - - @NonNull - @Override - public List getWheelPositions() { - return Arrays.asList( - encoderTicksToInches(parallelDeadwheel.getCurrentPosition()) * xMul, - encoderTicksToInches(perpendicularDeadwheel.getCurrentPosition()) * yMul - ); - } - - @NonNull - @Override - public List getWheelVelocities() { - // If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other - // high resolution encoders), enable overflow compensation with enableOverflowCompensation. - return Arrays.asList( - encoderTicksToInches(usingOverflowCompensation ? parallelDeadwheel.getCorrectedVelocity() : parallelDeadwheel.getRawVelocity()) * xMul, - encoderTicksToInches(usingOverflowCompensation ? perpendicularDeadwheel.getCorrectedVelocity() : perpendicularDeadwheel.getRawVelocity()) * yMul - ); - } -} \ No newline at end of file diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelTrackingLocalizerCoefficients.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelTrackingLocalizerCoefficients.java deleted file mode 100644 index 2e48442dc..000000000 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/localizers/TwoWheelTrackingLocalizerCoefficients.java +++ /dev/null @@ -1,177 +0,0 @@ -package org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers; - -import static org.murraybridgebunyips.bunyipslib.external.units.Units.Inches; - -import org.murraybridgebunyips.bunyipslib.external.units.Distance; -import org.murraybridgebunyips.bunyipslib.external.units.Measure; - -/** - * Coefficients for RoadRunner two wheel tracking localizer. - * Reworked to use a builder for multiple robot configurations. - * - * @author Lucas Bubner, 2023 - */ -public class TwoWheelTrackingLocalizerCoefficients { - /** - * The number of ticks per revolution of the encoder - */ - public double TICKS_PER_REV; - /** - * The radius of the tracking wheel in inches - */ - public double WHEEL_RADIUS = 2; // in - /** - * The gear ratio of the tracking wheel, (output wheel speed / input encoder speed) - */ - public double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed - - /** - * Position in the forward direction of the parallel wheel in inches - */ - public double PARALLEL_X; // X is the up and down direction - /** - * Position in the strafe direction of the parallel wheel in inches - */ - public double PARALLEL_Y; // Y is the strafe direction - - /** - * Position in the forward direction of the perpendicular wheel in inches - */ - public double PERPENDICULAR_X; - /** - * Position in the strafe direction of the perpendicular wheel in inches - */ - public double PERPENDICULAR_Y; - /** - * Multiplicative scale of the ticks from the x (forward) axis. - */ - public double X_MULTIPLIER = 1; - /** - * Multiplicative scale of the ticks from the y (strafe) axis. - */ - public double Y_MULTIPLIER = 1; - - /** - * Utility class for building TwoWheelTrackingLocalizerCoefficients - */ - public static class Builder { - - private final TwoWheelTrackingLocalizerCoefficients twoWheelTrackingCoefficients; - - /** - * Start building. - */ - public Builder() { - twoWheelTrackingCoefficients = new TwoWheelTrackingLocalizerCoefficients(); - } - - /** - * Set the ticks per revolution of the tracking wheel encoder. - * - * @param ticksPerRev The ticks per revolution - * @return The builder - */ - public Builder setTicksPerRev(double ticksPerRev) { - twoWheelTrackingCoefficients.TICKS_PER_REV = ticksPerRev; - return this; - } - - /** - * Set the radius of the tracking wheel. - * - * @param wheelRadius The radius of the tracking wheel - * @return The builder - */ - public Builder setWheelRadius(Measure wheelRadius) { - twoWheelTrackingCoefficients.WHEEL_RADIUS = wheelRadius.in(Inches); - return this; - } - - /** - * Set the gear ratio of the tracking wheel. - * - * @param gearRatio The gear ratio of the tracking wheel (output wheel speed / input encoder speed) - * @return The builder - */ - public Builder setGearRatio(double gearRatio) { - twoWheelTrackingCoefficients.GEAR_RATIO = gearRatio; - return this; - } - - /** - * Set the position of the parallel wheel in the forward direction. - * - * @param parallelX The position of the parallel wheel in the forward direction from the center of rotation - * @return The builder - */ - public Builder setParallelX(Measure parallelX) { - twoWheelTrackingCoefficients.PARALLEL_X = parallelX.in(Inches); - return this; - } - - /** - * Set the position of the parallel wheel in the strafe direction. - * - * @param parallelY The position of the parallel wheel in the strafe direction from the center of rotation - * @return The builder - */ - public Builder setParallelY(Measure parallelY) { - twoWheelTrackingCoefficients.PARALLEL_Y = parallelY.in(Inches); - return this; - } - - /** - * Set the position of the perpendicular wheel in the forward direction. - * - * @param perpendicularX The position of the perpendicular wheel in the forward direction from the center of rotation - * @return The builder - */ - public Builder setPerpendicularX(Measure perpendicularX) { - twoWheelTrackingCoefficients.PERPENDICULAR_X = perpendicularX.in(Inches); - return this; - } - - /** - * Set the position of the perpendicular wheel in the strafe direction. - * - * @param perpendicularY The position of the perpendicular wheel in the strafe direction from the center of rotation - * @return The builder - */ - public Builder setPerpendicularY(Measure perpendicularY) { - twoWheelTrackingCoefficients.PERPENDICULAR_Y = perpendicularY.in(Inches); - return this; - } - - /** - * Set the forward (x) multiplier for the ticks reported by the forward deadwheels. - * - * @param forwardMul the multiplier - * @return The builder - */ - public TwoWheelTrackingLocalizerCoefficients.Builder setXMultiplier(double forwardMul) { - twoWheelTrackingCoefficients.X_MULTIPLIER = forwardMul; - return this; - } - - /** - * Set the strafe (y) multiplier for the ticks reported by the side deadwheel. - * - * @param strafeMul the multiplier - * @return The builder - */ - public TwoWheelTrackingLocalizerCoefficients.Builder setYMultiplier(double strafeMul) { - twoWheelTrackingCoefficients.Y_MULTIPLIER = strafeMul; - return this; - } - - /** - * Build the coefficients. - * - * @return The coefficients - */ - public TwoWheelTrackingLocalizerCoefficients build() { - return twoWheelTrackingCoefficients; - } - } - -} diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelForwardOffsetTuner.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelForwardOffsetTuner.java index ae57bb693..4c27242ff 100644 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelForwardOffsetTuner.java +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelForwardOffsetTuner.java @@ -12,7 +12,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; import org.murraybridgebunyips.bunyipslib.roadrunner.drive.RoadRunnerDrive; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelTrackingLocalizer; +import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelLocalizer; /** * This routine determines the effective forward offset for the lateral tracking wheel. @@ -57,12 +57,12 @@ public void runOpMode() { // Must set localizer to a StandardTrackingWheelLocalizer, at the moment this will not run Localizer localizer = drive.getLocalizer(); - if (!(localizer instanceof ThreeWheelTrackingLocalizer)) { + if (!(localizer instanceof ThreeWheelLocalizer)) { RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + "(...));\" is called somewhere else."); } - assert localizer instanceof ThreeWheelTrackingLocalizer; + assert localizer instanceof ThreeWheelLocalizer; telemetry.addLine("Press play to begin the forward offset tuner"); telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); @@ -94,7 +94,7 @@ public void runOpMode() { drive.update(); } - double forwardOffset = ((ThreeWheelTrackingLocalizer) localizer).getCoefficients().FORWARD_OFFSET + + double forwardOffset = ((ThreeWheelLocalizer) localizer).getCoefficients().FORWARD_OFFSET + drive.getPoseEstimate().getY() / headingAccumulator; forwardOffsetStats.add(forwardOffset); diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelLateralDistanceTuner.java b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelLateralDistanceTuner.java index 2bb1780bb..d35a84575 100644 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelLateralDistanceTuner.java +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/tuning/TrackingWheelLateralDistanceTuner.java @@ -7,7 +7,7 @@ import com.qualcomm.robotcore.util.RobotLog; import org.murraybridgebunyips.bunyipslib.roadrunner.drive.RoadRunnerDrive; -import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelTrackingLocalizer; +import org.murraybridgebunyips.bunyipslib.roadrunner.drive.localizers.ThreeWheelLocalizer; /** * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s @@ -75,12 +75,12 @@ public void runOpMode() { // Must set localizer to a StandardTrackingWheelLocalizer, at the moment this will not run Localizer localizer = drive.getLocalizer(); - if (!(localizer instanceof ThreeWheelTrackingLocalizer)) { + if (!(localizer instanceof ThreeWheelLocalizer)) { RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + "(hardwareMap));\" is called in SampleMecanumDrive.java"); } - assert localizer instanceof ThreeWheelTrackingLocalizer; + assert localizer instanceof ThreeWheelLocalizer; telemetry.addLine("Prior to beginning the routine, please read the directions " + "located in the comments of the opmode file."); @@ -127,7 +127,7 @@ public void runOpMode() { telemetry.clearAll(); telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); telemetry.addLine("Effective LATERAL_DISTANCE: " + - (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * ((ThreeWheelTrackingLocalizer) localizer).getCoefficients().LATERAL_DISTANCE); + (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * ((ThreeWheelLocalizer) localizer).getCoefficients().LATERAL_DISTANCE); telemetry.update();