diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java index 4df27fb..5ad8cd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RRToPedroThreeWheelLocalizer.java @@ -1,93 +1,93 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -// -//import java.util.ArrayList; -//import java.util.List; -// -///** -// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and -// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing -// * localizer system. -// * -// * @author Anyi Lin - 10158 Scott's Bots -// * @version 1.0, 5/9/2024 -// */ -//public class RRToPedroThreeWheelLocalizer extends Localizer { -// private RoadRunnerThreeWheelLocalizer localizer; -// private double totalHeading; -// private Pose startPose; -// private Pose previousPose; -// -// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { -// List lastTrackingEncPositions = new ArrayList<>(); -// List lastTrackingEncVels = new ArrayList<>(); -// -// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); -// -// startPose = new Pose(); -// previousPose = new Pose(); -// } -// @Override -// public Pose getPose() { -// Pose2d pose = localizer.getPoseEstimate(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// @Override -// public Pose getVelocity() { -// Pose2d pose = localizer.getPoseVelocity(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// @Override -// public Vector getVelocityVector() { -// Pose2d pose = localizer.getPoseVelocity(); -// Vector returnVector = new Vector(); -// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); -// return returnVector; -// } -// -// @Override -// public void setStartPose(Pose setStart) { -// Pose oldStart = startPose; -// startPose = setStart; -// Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); -// localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); -// } -// -// @Override -// public void setPose(Pose setPose) { -// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); -// } -// -// @Override -// public void update() { -// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); -// previousPose = getPose(); -// } -// -// @Override -// public double getTotalHeading() { -// return totalHeading; -// } -// -// @Override -// public double getForwardMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; -// } -// -// @Override -// public double getLateralMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; -// } -// -// @Override -// public double getTurningMultiplier() { -// return (getForwardMultiplier() + getLateralMultiplier()) / 2; -// } -//} +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and + * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing + * localizer system. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/9/2024 + */ +public class RRToPedroThreeWheelLocalizer extends Localizer { + private RoadRunnerThreeWheelLocalizer localizer; + private double totalHeading; + private Pose startPose; + private Pose previousPose; + + public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { + List lastTrackingEncPositions = new ArrayList<>(); + List lastTrackingEncVels = new ArrayList<>(); + + localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); + + startPose = new Pose(); + previousPose = new Pose(); + } + @Override + public Pose getPose() { + Pose2d pose = localizer.getPoseEstimate(); + return new Pose(pose.getX(), pose.getY(), pose.getHeading()); + } + + @Override + public Pose getVelocity() { + Pose2d pose = localizer.getPoseVelocity(); + return new Pose(pose.getX(), pose.getY(), pose.getHeading()); + } + + @Override + public Vector getVelocityVector() { + Pose2d pose = localizer.getPoseVelocity(); + Vector returnVector = new Vector(); + returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); + return returnVector; + } + + @Override + public void setStartPose(Pose setStart) { + Pose oldStart = startPose; + startPose = setStart; + Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart); + localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading())); + } + + @Override + public void setPose(Pose setPose) { + localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); + } + + @Override + public void update() { + totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); + previousPose = getPose(); + } + + @Override + public double getTotalHeading() { + return totalHeading; + } + + @Override + public double getForwardMultiplier() { + return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; + } + + @Override + public double getLateralMultiplier() { + return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; + } + + @Override + public double getTurningMultiplier() { + return (getForwardMultiplier() + getLateralMultiplier()) / 2; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java index 35b8789..edbe337 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerEncoder.java @@ -1,125 +1,125 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import com.acmerobotics.roadrunner.util.NanoClock; -//import com.qualcomm.robotcore.hardware.DcMotorEx; -//import com.qualcomm.robotcore.hardware.DcMotorSimple; -// -///** -// * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding -// * slot's motor direction -// */ -//public class RoadRunnerEncoder { -// private final static int CPS_STEP = 0x10000; -// -// private static double inverseOverflow(double input, double estimate) { -// // convert to uint16 -// int real = (int) input & 0xffff; -// // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits -// // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window -// real += ((real % 20) / 4) * CPS_STEP; -// // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by -// real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; -// return real; -// } -// -// public enum Direction { -// FORWARD(1), -// REVERSE(-1); -// -// private int multiplier; -// -// Direction(int multiplier) { -// this.multiplier = multiplier; -// } -// -// public int getMultiplier() { -// return multiplier; -// } -// } -// -// private DcMotorEx motor; -// private NanoClock clock; -// -// private Direction direction; -// -// private int lastPosition; -// private int velocityEstimateIdx; -// private double[] velocityEstimates; -// private double lastUpdateTime; -// -// public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { -// this.motor = motor; -// this.clock = clock; -// -// this.direction = Direction.FORWARD; -// -// this.lastPosition = 0; -// this.velocityEstimates = new double[3]; -// this.lastUpdateTime = clock.seconds(); -// } -// -// public RoadRunnerEncoder(DcMotorEx motor) { -// this(motor, NanoClock.system()); -// } -// -// public Direction getDirection() { -// return direction; -// } -// -// private int getMultiplier() { -// return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); -// } -// -// /** -// * Allows you to set the direction of the counts and velocity without modifying the motor's direction state -// * @param direction either reverse or forward depending on if encoder counts should be negated -// */ -// public void setDirection(Direction direction) { -// this.direction = direction; -// } -// -// /** -// * Gets the position from the underlying motor and adjusts for the set direction. -// * Additionally, this method updates the velocity estimates used for compensated velocity -// * -// * @return encoder position -// */ -// public int getCurrentPosition() { -// int multiplier = getMultiplier(); -// int currentPosition = motor.getCurrentPosition() * multiplier; -// if (currentPosition != lastPosition) { -// double currentTime = clock.seconds(); -// double dt = currentTime - lastUpdateTime; -// velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; -// velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; -// lastPosition = currentPosition; -// lastUpdateTime = currentTime; -// } -// return currentPosition; -// } -// -// /** -// * Gets the velocity directly from the underlying motor and compensates for the direction -// * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) -// * -// * @return raw velocity -// */ -// public double getRawVelocity() { -// int multiplier = getMultiplier(); -// return motor.getVelocity() * multiplier; -// } -// -// /** -// * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity -// * that are lost in overflow due to velocity being transmitted as 16 bits. -// * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. -// * -// * @return corrected velocity -// */ -// public double getCorrectedVelocity() { -// double median = velocityEstimates[0] > velocityEstimates[1] -// ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) -// : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); -// return inverseOverflow(getRawVelocity(), median); -// } -//} +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +public class RoadRunnerEncoder { + private final static int CPS_STEP = 0x10000; + + private static double inverseOverflow(double input, double estimate) { + // convert to uint16 + int real = (int) input & 0xffff; + // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits + // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window + real += ((real % 20) / 4) * CPS_STEP; + // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by + real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; + return real; + } + + public enum Direction { + FORWARD(1), + REVERSE(-1); + + private int multiplier; + + Direction(int multiplier) { + this.multiplier = multiplier; + } + + public int getMultiplier() { + return multiplier; + } + } + + private DcMotorEx motor; + private NanoClock clock; + + private Direction direction; + + private int lastPosition; + private int velocityEstimateIdx; + private double[] velocityEstimates; + private double lastUpdateTime; + + public RoadRunnerEncoder(DcMotorEx motor, NanoClock clock) { + this.motor = motor; + this.clock = clock; + + this.direction = Direction.FORWARD; + + this.lastPosition = 0; + this.velocityEstimates = new double[3]; + this.lastUpdateTime = clock.seconds(); + } + + public RoadRunnerEncoder(DcMotorEx motor) { + this(motor, NanoClock.system()); + } + + public Direction getDirection() { + return direction; + } + + private int getMultiplier() { + return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * Allows you to set the direction of the counts and velocity without modifying the motor's direction state + * @param direction either reverse or forward depending on if encoder counts should be negated + */ + public void setDirection(Direction direction) { + this.direction = direction; + } + + /** + * Gets the position from the underlying motor and adjusts for the set direction. + * Additionally, this method updates the velocity estimates used for compensated velocity + * + * @return encoder position + */ + public int getCurrentPosition() { + int multiplier = getMultiplier(); + int currentPosition = motor.getCurrentPosition() * multiplier; + if (currentPosition != lastPosition) { + double currentTime = clock.seconds(); + double dt = currentTime - lastUpdateTime; + velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; + velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; + lastPosition = currentPosition; + lastUpdateTime = currentTime; + } + return currentPosition; + } + + /** + * Gets the velocity directly from the underlying motor and compensates for the direction + * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) + * + * @return raw velocity + */ + public double getRawVelocity() { + int multiplier = getMultiplier(); + return motor.getVelocity() * multiplier; + } + + /** + * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity + * that are lost in overflow due to velocity being transmitted as 16 bits. + * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. + * + * @return corrected velocity + */ + public double getCorrectedVelocity() { + double median = velocityEstimates[0] > velocityEstimates[1] + ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) + : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); + return inverseOverflow(getRawVelocity(), median); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java index ce879cc..93d13bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -1,117 +1,117 @@ -//package org.firstinspires.ftc.teamcode.pedroPathing.localization; -// -//import androidx.annotation.NonNull; -// -//import com.acmerobotics.dashboard.config.Config; -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; -//import com.qualcomm.robotcore.hardware.DcMotorEx; -//import com.qualcomm.robotcore.hardware.HardwareMap; -// -//import java.util.Arrays; -//import java.util.List; -// -///* -// * Sample tracking wheel localizer implementation assuming the standard configuration: -// * -// * left on robot is y pos -// * -// * front of robot is x pos -// * -// * /--------------\ -// * | ____ | -// * | ---- | -// * | || || | -// * | || || | -// * | | -// * | | -// * \--------------/ -// * -// */ -// -///** -// * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will -// * be replaced with a custom localizer. -// */ -//@Config -//public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { -// public static double TICKS_PER_REV = 8192; -// public static double WHEEL_RADIUS = 1.37795; // in -// public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed -// -// public static double X_MULTIPLIER = 0.5008239963; -// public static double Y_MULTIPLIER = 0.5018874659; -// -// public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; -// -// private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; -// -// private List lastEncPositions, lastEncVels; -// -// public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { -// super(Arrays.asList( -// new Pose2d(leftX, leftY, 0), // left -// new Pose2d(rightX, rightY, 0), // right -// new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe -// )); -// -// lastEncPositions = lastTrackingEncPositions; -// lastEncVels = lastTrackingEncVels; -// -// // TODO: redo the configs here -// leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); -// rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); -// strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); -// -// // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) -// leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); -// rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); -// strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); -// } -// -// public void resetHeading(double heading) { -// setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); -// } -// -// public static double encoderTicksToInches(double ticks) { -// return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; -// } -// -// @NonNull -// @Override -// public List getWheelPositions() { -// int leftPos = leftEncoder.getCurrentPosition(); -// int rightPos = rightEncoder.getCurrentPosition(); -// int frontPos = strafeEncoder.getCurrentPosition(); -// -// lastEncPositions.clear(); -// lastEncPositions.add(leftPos); -// lastEncPositions.add(rightPos); -// lastEncPositions.add(frontPos); -// -// return Arrays.asList( -// encoderTicksToInches(leftPos) * X_MULTIPLIER, -// encoderTicksToInches(rightPos) * X_MULTIPLIER, -// encoderTicksToInches(frontPos) * Y_MULTIPLIER -// ); -// } -// -// @NonNull -// @Override -// public List getWheelVelocities() { -// int leftVel = (int) leftEncoder.getCorrectedVelocity(); -// int rightVel = (int) rightEncoder.getCorrectedVelocity(); -// int frontVel = (int) strafeEncoder.getCorrectedVelocity(); -// -// lastEncVels.clear(); -// lastEncVels.add(leftVel); -// lastEncVels.add(rightVel); -// lastEncVels.add(frontVel); -// -// return Arrays.asList( -// encoderTicksToInches(leftVel) * X_MULTIPLIER, -// encoderTicksToInches(rightVel) * X_MULTIPLIER, -// encoderTicksToInches(frontVel) * Y_MULTIPLIER -// ); -// } -//} +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * left on robot is y pos + * + * front of robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ + +/** + * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will + * be replaced with a custom localizer. + */ +@Config +public class RoadRunnerThreeWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 1.37795; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double X_MULTIPLIER = 0.5008239963; + public static double Y_MULTIPLIER = 0.5018874659; + + public static double leftX = -18.5/25.4 - 0.1, leftY = 164.4/25.4, rightX = -18.4/25.4 - 0.1, rightY = -159.6/25.4, strafeX = -107.9/25.4+0.25, strafeY = -1.1/25.4-0.23; + + private RoadRunnerEncoder leftEncoder, rightEncoder, strafeEncoder; + + private List lastEncPositions, lastEncVels; + + public RoadRunnerThreeWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { + super(Arrays.asList( + new Pose2d(leftX, leftY, 0), // left + new Pose2d(rightX, rightY, 0), // right + new Pose2d(strafeX, strafeY, Math.toRadians(90)) // strafe + )); + + lastEncPositions = lastTrackingEncPositions; + lastEncVels = lastTrackingEncVels; + + // TODO: redo the configs here + leftEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + strafeEncoder = new RoadRunnerEncoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + leftEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); + rightEncoder.setDirection(RoadRunnerEncoder.Direction.REVERSE); + strafeEncoder.setDirection(RoadRunnerEncoder.Direction.FORWARD); + } + + public void resetHeading(double heading) { + setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + int leftPos = leftEncoder.getCurrentPosition(); + int rightPos = rightEncoder.getCurrentPosition(); + int frontPos = strafeEncoder.getCurrentPosition(); + + lastEncPositions.clear(); + lastEncPositions.add(leftPos); + lastEncPositions.add(rightPos); + lastEncPositions.add(frontPos); + + return Arrays.asList( + encoderTicksToInches(leftPos) * X_MULTIPLIER, + encoderTicksToInches(rightPos) * X_MULTIPLIER, + encoderTicksToInches(frontPos) * Y_MULTIPLIER + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + int leftVel = (int) leftEncoder.getCorrectedVelocity(); + int rightVel = (int) rightEncoder.getCorrectedVelocity(); + int frontVel = (int) strafeEncoder.getCorrectedVelocity(); + + lastEncVels.clear(); + lastEncVels.add(leftVel); + lastEncVels.add(rightVel); + lastEncVels.add(frontVel); + + return Arrays.asList( + encoderTicksToInches(leftVel) * X_MULTIPLIER, + encoderTicksToInches(rightVel) * X_MULTIPLIER, + encoderTicksToInches(frontVel) * Y_MULTIPLIER + ); + } +}