From 5595fcccd45711e743b140b3684feddcb475ddd0 Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Mon, 23 Dec 2024 16:32:21 -0800 Subject: [PATCH] Removing unnecessary files --- .../localizers/DriveEncoderLocalizer.java | 272 --------------- .../localizers/OTOSLocalizer.java | 218 ------------ .../RRToPedroThreeWheelLocalizer.java | 159 --------- .../localizers/RoadRunnerEncoder.java | 132 -------- .../RoadRunnerThreeWheelLocalizer.java | 123 ------- .../localizers/ThreeWheelIMULocalizer.java | 317 ------------------ .../localizers/TwoWheelLocalizer.java | 302 ----------------- 7 files changed, 1523 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java deleted file mode 100644 index a8e3902..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ /dev/null @@ -1,272 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; -import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the DriveEncoderLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the drive encoder set up. - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -@Config -public class DriveEncoderLocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder leftFront; - private Encoder rightFront; - private Encoder leftRear; - private Encoder rightRear; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 1; - public static double STRAFE_TICKS_TO_INCHES = 1; - public static double TURN_TICKS_TO_RADIANS = 1; - public static double ROBOT_WIDTH = 1; - public static double ROBOT_LENGTH = 1; - - /** - * This creates a new DriveEncoderLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public DriveEncoderLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new DriveEncoderLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; - - leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, leftFrontMotorName)); - leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, leftRearMotorName)); - rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, rightRearMotorName)); - rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, rightFrontMotorName)); - - // TODO: reverse any encoders necessary - leftFront.setDirection(Encoder.REVERSE); - rightRear.setDirection(Encoder.REVERSE); - leftRear.setDirection(Encoder.FORWARD); - rightRear.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders. Then, the robot's global change in position is calculated - * using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders. - */ - public void updateEncoders() { - leftFront.update(); - rightFront.update(); - leftRear.update(); - rightRear.update(); - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - leftFront.reset(); - rightFront.reset(); - leftRear.reset(); - rightRear.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() + rightRear.getDeltaPosition())); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() + leftRear.getDeltaPosition() - rightRear.getDeltaPosition())); - // theta/turning - returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((-leftFront.getDeltaPosition() + rightFront.getDeltaPosition() - leftRear.getDeltaPosition() + rightRear.getDeltaPosition()) / (ROBOT_WIDTH + ROBOT_LENGTH))); - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return TURN_TICKS_TO_RADIANS; - } - - /** - * This does nothing since this localizer does not use the IMU. - */ - public void resetIMU() { - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java deleted file mode 100644 index cdacd52..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ /dev/null @@ -1,218 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import com.qualcomm.hardware.sparkfun.SparkFunOTOS; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; - -/** - * This is the OTOSLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the SparkFun OTOS. The diagram below, which is modified from - * Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/20/2024 - */ -public class OTOSLocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private SparkFunOTOS otos; - private double previousHeading; - private double totalHeading; - - /** - * This creates a new OTOSLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public OTOSLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new OTOSLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public OTOSLocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; - - // TODO: replace this with your OTOS port - /* - TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the - 'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a - "SparkFunOTOS Corrected" in your robot confg - */ - SparkFunOTOS - otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); - - otos.setLinearUnit(DistanceUnit.INCH); - otos.setAngularUnit(AngleUnit.RADIANS); - - // TODO: replace this with your OTOS offset from the center of the robot - // For the OTOS, left/right is the y axis and forward/backward is the x axis, with left being - // positive y and forward being positive x. PI/2 radians is facing forward, and clockwise - // rotation is negative rotation. - otos.setOffset(new SparkFunOTOS.Pose2D(0,0,Math.PI / 2)); - - // TODO: replace these with your tuned multipliers - otos.setLinearScalar(1.0); - otos.setAngularScalar(1.0); - - otos.calibrateImu(); - otos.resetTracking(); - - setStartPose(setStartPose); - totalHeading = 0; - previousHeading = startPose.getHeading(); - - resetOTOS(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - SparkFunOTOS.Pose2D pose = otos.getPosition(); - return MathFunctions.addPoses(startPose, new Pose(pose.x, pose.y, pose.h)); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - SparkFunOTOS.Pose2D OTOSVelocity = otos.getVelocity(); - return new Pose(OTOSVelocity.x, OTOSVelocity.y, OTOSVelocity.h); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return getVelocity().getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - resetOTOS(); - Pose setOTOSPose = MathFunctions.subtractPoses(setPose, startPose); - otos.setPosition(new SparkFunOTOS.Pose2D(setOTOSPose.getX(), setOTOSPose.getY(), setOTOSPose.getHeading())); - } - - /** - * This updates the total heading of the robot. The OTOS handles all other updates itself. - */ - @Override - public void update() { - totalHeading += MathFunctions.getSmallestAngleDifference(otos.getPosition().h, previousHeading); - previousHeading = otos.getPosition().h; - } - - /** - * This resets the OTOS. - */ - public void resetOTOS() { - otos.resetTracking(); - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from OTOS - * ticks to inches. For the OTOS, this value is the same as the lateral multiplier. - * This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return otos.getLinearScalar(); - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * OTOS ticks to inches. For the OTOS, this value is the same as the forward multiplier. - * This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return otos.getLinearScalar(); - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from OTOS ticks - * to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return otos.getAngularScalar(); - } - - /** - * This does nothing since this localizer does not use the IMU. - */ - public void resetIMU() { - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java deleted file mode 100644 index 15275f0..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RRToPedroThreeWheelLocalizer.java +++ /dev/null @@ -1,159 +0,0 @@ -//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; -// -// /** -// * This creates a new RRToPedroThreeWheelLocalizer from a HardwareMap. This adapts the previously -// * used Road Runner localization system to the new Pedro Pathing localization system. -// * -// * @param hardwareMap the HardwareMap -// */ -// public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) { -// List lastTrackingEncPositions = new ArrayList<>(); -// List lastTrackingEncVels = new ArrayList<>(); -// -// localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); -// -// startPose = new Pose(); -// previousPose = new Pose(); -// } -// -// /** -// * This returns the current pose estimate as a Pose. -// * -// * @return returns the current pose estimate -// */ -// @Override -// public Pose getPose() { -// Pose2d pose = localizer.getPoseEstimate(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// /** -// * This returns the current velocity estimate as a Pose. -// * -// * @return returns the current velocity estimate -// */ -// @Override -// public Pose getVelocity() { -// Pose2d pose = localizer.getPoseVelocity(); -// return new Pose(pose.getX(), pose.getY(), pose.getHeading()); -// } -// -// /** -// * This returns the current velocity estimate as a Vector. -// * -// * @return returns the current velocity estimate -// */ -// @Override -// public Vector getVelocityVector() { -// Pose2d pose = localizer.getPoseVelocity(); -// Vector returnVector = new Vector(); -// returnVector.setOrthogonalComponents(pose.getX(), pose.getY()); -// return returnVector; -// } -// -// /** -// * This sets the start pose. Any movement of the robot is treated as a displacement from the -// * start pose, so moving the start pose will move the current pose estimate the same amount. -// * -// * @param setStart the new start pose -// */ -// @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())); -// } -// -// /** -// * This sets the current pose estimate. This has no effect on the start pose. -// * -// * @param setPose the new current pose estimate -// */ -// @Override -// public void setPose(Pose setPose) { -// localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading())); -// } -// -// /** -// * This updates the total heading and previous pose estimate. Everything else is handled by the -// * Road Runner localizer on its own, but updating this tells you how far the robot has really -// * turned. -// */ -// @Override -// public void update() { -// totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading()); -// previousPose = getPose(); -// } -// -// /** -// * This returns how far the robot has actually turned. -// * -// * @return returns the total angle turned, in degrees. -// */ -// @Override -// public double getTotalHeading() { -// return totalHeading; -// } -// -// /** -// * This returns the forward multiplier of the Road Runner localizer, which converts from ticks -// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything -// * multiplied together should be. If you do use that, then do be aware that the value returned is -// * the product of the Road Runner ticks to inches and the x multiplier. -// * -// * @return returns the forward multiplier -// */ -// @Override -// public double getForwardMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER; -// } -// -// /** -// * This returns the lateral multiplier of the Road Runner localizer, which converts from ticks -// * to inches. You can actually use the tuners in Pedro Pathing to find the value that everything -// * multiplied together should be. If you do use that, then do be aware that the value returned is -// * the product of the Road Runner ticks to inches and the y multiplier. -// * -// * @return returns the lateral multiplier -// */ -// @Override -// public double getLateralMultiplier() { -// return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER; -// } -// -// /** -// * This returns the turning multiplier of the Road Runner localizer, which doesn't actually exist. -// * There really isn't a point in tuning the turning for the Road Runner localizer. This will -// * actually just return the average of the two other multipliers. -// * -// * @return returns the turning multiplier -// */ -// @Override -// public double getTurningMultiplier() { -// return (getForwardMultiplier() + getLateralMultiplier()) / 2; -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java deleted file mode 100644 index d743b49..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerEncoder.java +++ /dev/null @@ -1,132 +0,0 @@ -//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; -// -///** -// * This class is adapted from the Road Runner Encoder class. Later, this will be replaced with a -// * custom encoder class. According to Road Runner, this wraps a motor instance to provide corrected -// * velocity counts and allow reversing independently of the corresponding slot's motor direction. -// * -// * I'm fairly sure I didn't make any changes to this class, just copied it so I wouldn't have to have -// * import statements, so I'm not crediting myself as an author for this. -// * -// * @author Road Runner dev team -// * @version 1.0, 5/9/2024 -// */ -//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/localizers/RoadRunnerThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java deleted file mode 100644 index 2ee75a9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/RoadRunnerThreeWheelLocalizer.java +++ /dev/null @@ -1,123 +0,0 @@ -//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. I made some minor changes, so I'm crediting myself as an -// * 'author' of sorts, but really this is pretty much Road Runner's code, just moved to be local to -// * Pedro Pathing to avoid having imports. -// * -// * @author Road Runner dev team -// * @author Anyi Lin - 10158 Scott's Bots -// * @version 1.0, 5/9/2024 -// */ -//@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 -// ); -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java deleted file mode 100644 index 71145e2..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ /dev/null @@ -1,317 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import static org.firstinspires.ftc.teamcode.PedroConstants.*; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the ThreeWheelIMULocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the three wheel odometry set up with the IMU to have more accurate heading - * readings. The diagram below, which is modified from Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) - * - * @author Logan Nash - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 7/9/2024 - */ -@Config -public class ThreeWheelIMULocalizer extends Localizer { - private HardwareMap hardwareMap; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder leftEncoder; - private Encoder rightEncoder; - private Encoder strafeEncoder; - private Pose leftEncoderPose; - private Pose rightEncoderPose; - private Pose strafeEncoderPose; - - public final IMU imu; - private double previousIMUOrientation; - private double deltaRadians; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - public static double TURN_TICKS_TO_RADIANS = 0.0022;//8192 * 1.37795 * 2 * Math.PI * 0.5; - - public static boolean useIMU = true; - - /** - * This creates a new ThreeWheelIMULocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public ThreeWheelIMULocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new ThreeWheelIMULocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public ThreeWheelIMULocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; - imu = hardwareMap.get(IMU.class, IMU); - - // TODO: replace this with your IMU's orientation - imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(IMU_LOGO_FACING_DIRECTION, IMU_USB_FACING_DIRECTION))); - - // TODO: replace these with your encoder positions - leftEncoderPose = new Pose(0, 6.19375, 0); - rightEncoderPose = new Pose(0, -6.19375, 0); - strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90)); - - // TODO: replace these with your encoder ports - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER)); - strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); - - // TODO: reverse any encoders necessary - leftEncoder.setDirection(LEFT_ENCODER_DIRECTION); - rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION); - strafeEncoder.setDirection(BACK_ENCODER_DIRECTION); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - totalHeading = 0; - - resetEncoders(); - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders. Then, the robot's global change in position is calculated - * using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders. - */ - public void updateEncoders() { - leftEncoder.update(); - rightEncoder.update(); - strafeEncoder.update(); - - double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); - previousIMUOrientation = currentIMUOrientation; - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - leftEncoder.reset(); - rightEncoder.reset(); - strafeEncoder.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * ((rightEncoder.getDeltaPosition() * leftEncoderPose.getY() - leftEncoder.getDeltaPosition() * rightEncoderPose.getY()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY())))); - // theta/turning - if (MathFunctions.getSmallestAngleDifference(0, deltaRadians) > 0.00005 && useIMU) { - returnMatrix.set(2, 0, deltaRadians); - } else { - returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); - } - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return TURN_TICKS_TO_RADIANS; - } - - /** - * This resets the IMU. - */ - public void resetIMU() { - imu.resetYaw(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java deleted file mode 100644 index 96f0688..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ /dev/null @@ -1,302 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers; - -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; -import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; - -/** - * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a - * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from - * Road Runner, shows a typical set up. - * - * The view is from the top of the robot looking downwards. - * - * left on robot is the y positive direction - * - * forward on robot is the x positive direction - * - * /--------------\ - * | ____ | - * | ---- | - * | || || | - * | || || | ----> left (y positive) - * | | - * | | - * \--------------/ - * | - * | - * V - * forward (x positive) - * - * @author Anyi Lin - 10158 Scott's Bots - * @version 1.0, 4/2/2024 - */ -@Config -public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo work - private HardwareMap hardwareMap; - private IMU imu; - private Pose startPose; - private Pose displacementPose; - private Pose currentVelocity; - private Matrix prevRotationMatrix; - private NanoTimer timer; - private long deltaTimeNano; - private Encoder forwardEncoder; - private Encoder strafeEncoder; - private Pose forwardEncoderPose; - private Pose strafeEncoderPose; - private double previousIMUOrientation; - private double deltaRadians; - private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - - /** - * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) - * facing 0 heading. - * - * @param map the HardwareMap - */ - public TwoWheelLocalizer(HardwareMap map) { - this(map, new Pose()); - } - - /** - * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose - * specifying the starting pose of the localizer. - * - * @param map the HardwareMap - * @param setStartPose the Pose to start from - */ - public TwoWheelLocalizer(HardwareMap map, Pose setStartPose) { - // TODO: replace these with your encoder positions - forwardEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); - strafeEncoderPose = new Pose(-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); - - hardwareMap = map; - - imu = hardwareMap.get(IMU.class, "imu"); - // TODO: replace this with your IMU's orientation - imu.initialize(new IMU.Parameters(new RevHubOrientationOnRobot(RevHubOrientationOnRobot.LogoFacingDirection.UP, RevHubOrientationOnRobot.UsbFacingDirection.LEFT))); - - // TODO: replace these with your encoder ports - forwardEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); - strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); - - // TODO: reverse any encoders necessary - forwardEncoder.setDirection(Encoder.REVERSE); - strafeEncoder.setDirection(Encoder.FORWARD); - - setStartPose(setStartPose); - timer = new NanoTimer(); - deltaTimeNano = 1; - displacementPose = new Pose(); - currentVelocity = new Pose(); - - previousIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - deltaRadians = 0; - } - - /** - * This returns the current pose estimate. - * - * @return returns the current pose estimate as a Pose - */ - @Override - public Pose getPose() { - return MathFunctions.addPoses(startPose, displacementPose); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Pose - */ - @Override - public Pose getVelocity() { - return currentVelocity.copy(); - } - - /** - * This returns the current velocity estimate. - * - * @return returns the current velocity estimate as a Vector - */ - @Override - public Vector getVelocityVector() { - return currentVelocity.getVector(); - } - - /** - * This sets the start pose. Changing the start pose should move the robot as if all its - * previous movements were displacing it from its new start pose. - * - * @param setStart the new start pose - */ - @Override - public void setStartPose(Pose setStart) { - startPose = setStart; - } - - /** - * This sets the Matrix that contains the previous pose's heading rotation. - * - * @param heading the rotation of the Matrix - */ - public void setPrevRotationMatrix(double heading) { - prevRotationMatrix = new Matrix(3,3); - prevRotationMatrix.set(0, 0, Math.cos(heading)); - prevRotationMatrix.set(0, 1, -Math.sin(heading)); - prevRotationMatrix.set(1, 0, Math.sin(heading)); - prevRotationMatrix.set(1, 1, Math.cos(heading)); - prevRotationMatrix.set(2, 2, 1.0); - } - - /** - * This sets the current pose estimate. Changing this should just change the robot's current - * pose estimate, not anything to do with the start pose. - * - * @param setPose the new current pose estimate - */ - @Override - public void setPose(Pose setPose) { - displacementPose = MathFunctions.subtractPoses(setPose, startPose); - resetEncoders(); - } - - /** - * This updates the elapsed time timer that keeps track of time between updates, as well as the - * change position of the Encoders and the IMU readings. Then, the robot's global change in - * position is calculated using the pose exponential method. - */ - @Override - public void update() { - deltaTimeNano = timer.getElapsedTime(); - timer.resetTimer(); - - updateEncoders(); - Matrix robotDeltas = getRobotDeltas(); - Matrix globalDeltas; - setPrevRotationMatrix(getPose().getHeading()); - - Matrix transformation = new Matrix(3,3); - if (Math.abs(robotDeltas.get(2, 0)) < 0.001) { - transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0); - transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0)); - transformation.set(2, 2, 1.0); - } else { - transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0)); - transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0)); - transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0)); - transformation.set(2, 2, 1.0); - } - - globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas); - - displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0))); - currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano * Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano * Math.pow(10.0, 9))); - - totalHeading += globalDeltas.get(2, 0); - } - - /** - * This updates the Encoders as well as the IMU. - */ - public void updateEncoders() { - forwardEncoder.update(); - strafeEncoder.update(); - - double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); - deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation); - previousIMUOrientation = currentIMUOrientation; - } - - /** - * This resets the Encoders. - */ - public void resetEncoders() { - forwardEncoder.reset(); - strafeEncoder.reset(); - } - - /** - * This calculates the change in position from the perspective of the robot using information - * from the Encoders and IMU. - * - * @return returns a Matrix containing the robot relative movement. - */ - public Matrix getRobotDeltas() { - Matrix returnMatrix = new Matrix(3,1); - // x/forward movement - returnMatrix.set(0,0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians)); - //y/strafe movement - returnMatrix.set(1,0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians)); - // theta/turning - returnMatrix.set(2,0, deltaRadians); - return returnMatrix; - } - - /** - * This returns how far the robot has turned in radians, in a number not clamped between 0 and - * 2 * pi radians. This is used for some tuning things and nothing actually within the following. - * - * @return returns how far the robot has turned in total, in radians. - */ - public double getTotalHeading() { - return totalHeading; - } - - /** - * This returns the multiplier applied to forward movement measurement to convert from encoder - * ticks to inches. This is found empirically through a tuner. - * - * @return returns the forward ticks to inches multiplier - */ - public double getForwardMultiplier() { - return FORWARD_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to lateral/strafe movement measurement to convert from - * encoder ticks to inches. This is found empirically through a tuner. - * - * @return returns the lateral/strafe ticks to inches multiplier - */ - public double getLateralMultiplier() { - return STRAFE_TICKS_TO_INCHES; - } - - /** - * This returns the multiplier applied to turning movement measurement to convert from encoder - * ticks to radians. This is found empirically through a tuner. - * - * @return returns the turning ticks to radians multiplier - */ - public double getTurningMultiplier() { - return 1; - } - - /** - * This resets the IMU. - */ - public void resetIMU() { - imu.resetYaw(); - } -}