diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index 7aa7279..a41a4b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -1,11 +1,11 @@ ## Prerequisites Obviously, you have to have a robot to use Pedro Pathing. Also, Pedro Pathing is only able to work with omnidirectional drives, like mecanum drive. There is currently no support for swerve drives. -You must also have a localizer of some sort. Pedro Pathing currently has the three-wheel localizer -from Road Runner, but we have plans to create our own localizer soon. You will need to have your -localizer tuned before starting tuning Pedro Pathing, since Pedro Pathing needs to know where your -robot is at all times. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) will help -a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). +You must also have a localizer of some sort. Pedro Pathing has both a three-wheel localizer and a +two-wheel localizer. You will need to have your localizer tuned before starting to tune PedroPathing. +Check out the tuning guide under the localization tab if you're planning on using one of the +localizers available in Pedro Pathing. Additionally, using [FTC Dashboard](http://192.168.43.1:8080/dash) +will help a lot in tuning, and we have a slightly scuffed Desmos path visualizer [here](https://www.desmos.com/calculator/3so1zx0hcd). One last thing to note is that Pedro Pathing operates in inches and radians. ## Tuning diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 3cadba6..3a7fa37 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -12,9 +12,10 @@ import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstan import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.smallTranslationalPIDFFeedForward; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -22,6 +23,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; @@ -29,8 +31,10 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathCallback; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; import org.firstinspires.ftc.teamcode.pedroPathing.util.PIDFController; import java.util.ArrayList; @@ -60,7 +64,7 @@ public class Follower { private PoseUpdater poseUpdater; - private Pose2d closestPose; + private Pose closestPose; private Path currentPath; @@ -118,6 +122,7 @@ public class Follower { private PIDFController largeDrivePIDF = new PIDFController(FollowerConstants.largeDrivePIDFCoefficients); + public static boolean drawStuff = true; public static boolean useTranslational = true; public static boolean useCentripetal = true; public static boolean useHeading = true; @@ -184,6 +189,7 @@ public class Follower { accelerations.add(new Vector()); } calculateAveragedVelocityAndAcceleration(); + draw(); } /** @@ -206,21 +212,34 @@ public class Follower { } } + /** + * This gets a Point from the current Path from a specified t-value. + * + * @return returns the Point. + */ + public Point getPointFromPath(double t) { + if (currentPath != null) { + return currentPath.getPoint(t); + } else { + return null; + } + } + /** * This returns the current pose from the PoseUpdater. * * @return returns the pose */ - public Pose2d getPose() { + public Pose getPose() { return poseUpdater.getPose(); } /** - * This sets the current pose in the PoseUpdater using Road Runner's setPoseEstimate() method. + * This sets the current pose in the PoseUpdater without using offsets. * * @param pose The pose to set the current pose to. */ - public void setPose(Pose2d pose) { + public void setPose(Pose pose) { poseUpdater.setPose(pose); } @@ -256,7 +275,7 @@ public class Follower { * * @param pose the pose to set the starting pose to. */ - public void setStartingPose(Pose2d pose) { + public void setStartingPose(Pose pose) { poseUpdater.setStartingPose(pose); } @@ -267,7 +286,7 @@ public class Follower { * * @param set The pose to set the current pose to. */ - public void setCurrentPoseWithOffset(Pose2d set) { + public void setCurrentPoseWithOffset(Pose set) { poseUpdater.setCurrentPoseWithOffset(set); } @@ -327,7 +346,7 @@ public class Follower { /** * This resets all offsets set to the PoseUpdater. If you have reset your pose using the - * setCurrentPoseUsingOffset(Pose2d set) method, then your pose will be returned to what the + * setCurrentPoseUsingOffset(Pose set) method, then your pose will be returned to what the * PoseUpdater thinks your pose would be, not the pose you reset to. */ public void resetOffset() { @@ -476,6 +495,14 @@ public class Follower { motors.get(i).setPower(drivePowers[i]); } } + draw(); + } + + public void draw() { + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), getPose()); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } /** @@ -757,19 +784,18 @@ public class Follower { * @return returns the centripetal force correction vector. */ public Vector getCentripetalForceCorrection() { - return new Vector(); -// if (!useCentripetal) return new Vector(); -// double curvature; -// if (auto) { -// curvature = currentPath.getClosestPointCurvature(); -// } else { -// double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); -// double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); -// curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); -// } -// if (Double.isNaN(curvature)) return new Vector(); -// centripetalVector = new Vector(MathFunctions.clamp(Math.abs(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature), -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); -// return centripetalVector; + if (!useCentripetal) return new Vector(); + double curvature; + if (auto) { + curvature = currentPath.getClosestPointCurvature(); + } else { + double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent(); + double yDoublePrime = averageAcceleration.getYComponent() / averageVelocity.getXComponent(); + curvature = (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)) / (yDoublePrime); + } + if (Double.isNaN(curvature)) return new Vector(); + centripetalVector = new Vector(MathFunctions.clamp(FollowerConstants.centripetalScaling * FollowerConstants.mass * Math.pow(MathFunctions.dotProduct(poseUpdater.getVelocity(), MathFunctions.normalizeVector(currentPath.getClosestPointTangentVector())), 2) * curvature, -1, 1), currentPath.getClosestPointTangentVector().getTheta() + Math.PI / 2 * MathFunctions.getSign(currentPath.getClosestPointNormalVector().getTheta())); + return centripetalVector; } /** @@ -779,7 +805,7 @@ public class Follower { * * @return returns the closest pose. */ - public Pose2d getClosestPose() { + public Pose getClosestPose() { return closestPose; } @@ -863,9 +889,18 @@ public class Follower { telemetry.addData("x", getPose().getX()); telemetry.addData("y", getPose().getY()); telemetry.addData("heading", getPose().getHeading()); + telemetry.addData("total heading", poseUpdater.getTotalHeading()); telemetry.addData("velocity magnitude", getVelocity().getMagnitude()); telemetry.addData("velocity heading", getVelocity().getTheta()); telemetry.update(); + TelemetryPacket packet = new TelemetryPacket(); + //Draws the current target path that the robot would like to follow + packet.fieldOverlay().setStroke("#4CAF50"); + Drawing.drawRobot(packet.fieldOverlay(), new Pose(getPose().getX(), getPose().getY(), getPose().getHeading())); + //Draws the current path that the robot is following + packet.fieldOverlay().setStroke("#3F51B5"); + if (currentPath != null) Drawing.drawRobot(packet.fieldOverlay(), getPointFromPath(currentPath.getClosestPointTValue())); + FtcDashboard.getInstance().sendTelemetryPacket(packet); } /** @@ -877,4 +912,13 @@ public class Follower { public void telemetryDebug(Telemetry telemetry) { telemetryDebug(new MultipleTelemetry(telemetry)); } + + /** + * This returns the total number of radians the robot has turned. + * + * @return the total heading. + */ + public double getTotalHeading() { + return poseUpdater.getTotalHeading(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java new file mode 100644 index 0000000..3a10761 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java @@ -0,0 +1,173 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +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 { // todo: make drive encoders work + 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; + + public DriveEncoderLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { + + hardwareMap = map; + + // TODO: replace these with your encoder ports + leftFront = new Encoder(hardwareMap.get(DcMotorEx.class, "leftFront")); + rightFront = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + leftRear = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightRear = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); + + // 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(); + } + + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + 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); + } + + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + @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); + } + + public void updateEncoders() { + leftFront.update(); + rightFront.update(); + leftRear.update(); + rightRear.update(); + } + + public void resetEncoders() { + leftFront.reset(); + rightFront.reset(); + leftRear.reset(); + rightRear.reset(); + } + + 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; + } + + public double getTotalHeading() { + return totalHeading; + } + + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java new file mode 100644 index 0000000..3f8f72f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Encoder.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * This is the Encoder class. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Encoder { + private DcMotorEx motor; + private double previousPosition; + private double currentPosition; + private double multiplier; + + public final static double FORWARD = 1, REVERSE = -1; + + public Encoder(DcMotorEx setMotor) { + motor = setMotor; + multiplier = FORWARD; + reset(); + } + + public void setDirection(double setMultiplier) { + multiplier = setMultiplier; + } + + public void reset() { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + previousPosition = motor.getCurrentPosition(); + currentPosition = motor.getCurrentPosition(); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public void update() { + previousPosition = currentPosition; + currentPosition = motor.getCurrentPosition(); + } + + public double getMultiplier() { + return multiplier * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + public double getDeltaPosition() { + return getMultiplier() * (currentPosition - previousPosition); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java new file mode 100644 index 0000000..a16a3db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Matrix.java @@ -0,0 +1,152 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +import java.util.Arrays; + +/** + * This is the Matrix class. This defines matrices, primarily for use in the localizers. However, if + * matrices and matrix operations are necessary, this class as well as some operations in the + * MathFunctions class can absolutely be used there as well. It's similar to Mats in OpenCV if you've + * used them before, but with more limited functionality. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 + */ +public class Matrix { + private double[][] matrix; + + public Matrix() { + matrix = new double[0][0]; + } + + public Matrix(int rows, int columns) { + matrix = new double[rows][columns]; + } + + public Matrix(double[][] setMatrix) { + setMatrix(setMatrix); + } + + public Matrix(Matrix setMatrix) { + setMatrix(setMatrix); + } + + public static double[][] deepCopy(double[][] copyMatrix) { + double[][] returnMatrix = new double[copyMatrix.length][copyMatrix[0].length]; + for (int i = 0; i < copyMatrix.length; i++) { + returnMatrix[i] = Arrays.copyOf(copyMatrix[i], copyMatrix[i].length); + } + return returnMatrix; + } + + public double[][] getMatrix() { + return deepCopy(matrix); + } + + public double[] get(int row) { + return Arrays.copyOf(matrix[row], matrix[row].length); + } + + public double get(int row, int column) { + return get(row)[column]; + } + + public int getRows() { + return matrix.length; + } + + public int getColumns() { + return matrix[0].length; + } + + public boolean setMatrix(Matrix setMatrix) { + return setMatrix(setMatrix.getMatrix()); + } + + public boolean setMatrix(double[][] setMatrix) { + int columns = setMatrix[0].length; + for (int i = 0; i < setMatrix.length; i++) { + if (setMatrix[i].length != columns) { + return false; + } + } + matrix = deepCopy(setMatrix); + return true; + } + + public boolean set(int row, double[] input) { + if (input.length != getColumns()) { + return false; + } + matrix[row] = Arrays.copyOf(input, input.length); + return true; + } + + public boolean set(int row, int column, double input) { + matrix[row][column] = input; + return true; + } + + public boolean add(Matrix input) { + if (input.getRows() == getRows() && input.getColumns() == getColumns()) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, get(i,j) + input.get(i,j)); + } + } + return true; + } + return false; + } + + public boolean subtract(Matrix input) { + if (input.getRows() == getRows() && input.getColumns() == getColumns()) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, get(i,j) - input.get(i,j)); + } + } + return true; + } + return false; + } + + public boolean scalarMultiply(double scalar) { + for (int i = 0; i < getRows(); i++) { + for (int j = 0; j < getColumns(); j++) { + set(i, j, scalar * get(i,j)); + } + } + return true; + } + + public boolean flipSigns() { + return scalarMultiply(-1); + } + + public boolean multiply(Matrix input) { + if (getColumns() == input.getRows()) { + Matrix product = new Matrix(getRows(), input.getColumns()); + for (int i = 0; i < product.getRows(); i++) { + for (int j = 0; j < product.getColumns(); j++) { + double value = 0; + for (int k = 0; k < get(i).length; k++) { + value += get(i, k) * input.get(k, j); + } + product.set(i, j, value); + } + } + setMatrix(product); + return true; + } + return false; + } + + public static Matrix multiply(Matrix one, Matrix two) { + Matrix returnMatrix = new Matrix(one); + if (returnMatrix.multiply(two)) { + return returnMatrix; + } else { + return new Matrix(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index d8d9b32..a010f6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; -import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -9,9 +8,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; 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 PoseUpdater class. This class handles getting pose data from the localizer and returning * the information in a useful way to the Follower. @@ -26,13 +22,13 @@ public class PoseUpdater { private IMU imu; - private ThreeWheelLocalizer localizer; + private Localizer localizer; - private Pose2d startingPose = new Pose2d(0,0,0); + private Pose startingPose = new Pose(0,0,0); - private Pose2d currentPose = startingPose; + private Pose currentPose = startingPose; - private Pose2d previousPose = startingPose; + private Pose previousPose = startingPose; private Vector currentVelocity = new Vector(); @@ -59,9 +55,8 @@ public class PoseUpdater { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - List lastTrackingEncPositions = new ArrayList<>(); - List lastTrackingEncVels = new ArrayList<>(); - localizer = new ThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels); + // TODO: change this to your preferred localizer + localizer = new ThreeWheelLocalizer(hardwareMap); } /** @@ -85,23 +80,23 @@ public class PoseUpdater { * * @param set the Pose to set the starting pose to. */ - public void setStartingPose(Pose2d set) { + public void setStartingPose(Pose set) { startingPose = set; previousPose = startingPose; previousPoseTime = System.nanoTime(); currentPoseTime = System.nanoTime(); - localizer.setPoseEstimate(set); + localizer.setStartPose(set); } /** - * This sets the current pose, using offsets so no reset time delay. This is better than the - * Road Runner reset, in general. Think of using offsets as setting trim in an aircraft. This can - * be reset as well, so beware of using the resetOffset() method. + * This sets the current pose, using offsets. Think of using offsets as setting trim in an + * aircraft. This can be reset as well, so beware of using the resetOffset() method. + * * * @param set The pose to set the current pose to. */ - public void setCurrentPoseWithOffset(Pose2d set) { - Pose2d currentPose = getRawPose(); + public void setCurrentPoseWithOffset(Pose set) { + Pose currentPose = getRawPose(); setXOffset(set.getX() - currentPose.getX()); setYOffset(set.getY() - currentPose.getY()); setHeadingOffset(MathFunctions.getTurnDirection(currentPose.getHeading(), set.getHeading()) * MathFunctions.getSmallestAngleDifference(currentPose.getHeading(), set.getHeading())); @@ -167,8 +162,8 @@ public class PoseUpdater { * @param pose The pose to be offset. * @return This returns a new Pose with the offset applied. */ - public Pose2d applyOffset(Pose2d pose) { - return new Pose2d(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset); + public Pose applyOffset(Pose pose) { + return new Pose(pose.getX()+xOffset, pose.getY()+yOffset, pose.getHeading()+headingOffset); } /** @@ -189,9 +184,9 @@ public class PoseUpdater { * * @return returns the current pose. */ - public Pose2d getPose() { + public Pose getPose() { if (currentPose == null) { - currentPose = localizer.getPoseEstimate(); + currentPose = localizer.getPose(); return applyOffset(currentPose); } else { return applyOffset(currentPose); @@ -205,9 +200,9 @@ public class PoseUpdater { * * @return returns the raw pose. */ - public Pose2d getRawPose() { + public Pose getRawPose() { if (currentPose == null) { - currentPose = localizer.getPoseEstimate(); + currentPose = localizer.getPose(); return currentPose; } else { return currentPose; @@ -215,13 +210,13 @@ public class PoseUpdater { } /** - * This sets the current pose using the Road Runner pose reset. This is slow. + * This sets the current pose without using resettable offsets. * * @param set the pose to set the current pose to. */ - public void setPose(Pose2d set) { + public void setPose(Pose set) { resetOffset(); - localizer.setPoseEstimate(set); + localizer.setPose(set); } /** @@ -229,7 +224,7 @@ public class PoseUpdater { * * @return returns the robot's previous pose. */ - public Pose2d getPreviousPose() { + public Pose getPreviousPose() { return previousPose; } @@ -238,8 +233,10 @@ public class PoseUpdater { * * @return returns the robot's delta pose. */ - public Pose2d getDeltaPose() { - return getPose().minus(previousPose); + public Pose getDeltaPose() { + Pose returnPose = getPose(); + returnPose.subtract(previousPose); + return returnPose; } /** @@ -290,7 +287,7 @@ public class PoseUpdater { * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. */ public void resetHeadingToIMU() { - localizer.resetHeading(getNormalizedIMUHeading() + startingPose.getHeading()); + localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); } /** @@ -299,7 +296,7 @@ public class PoseUpdater { * method. */ public void resetHeadingToIMUWithOffsets() { - setCurrentPoseWithOffset(new Pose2d(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); } /** @@ -310,4 +307,22 @@ public class PoseUpdater { public double getNormalizedIMUHeading() { return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); } + + /** + * This returns the total number of radians the robot has turned. + * + * @return the total heading. + */ + public double getTotalHeading() { + return localizer.getTotalHeading(); + } + + /** + * This returns the Localizer. + * + * @return the Localizer + */ + public Localizer getLocalizer() { + return localizer; + } } 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 edbe337..35b8789 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 new file mode 100644 index 0000000..ce879cc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/RoadRunnerThreeWheelLocalizer.java @@ -0,0 +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 +// ); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java index 88f1e7c..cdbc35d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelLocalizer.java @@ -1,117 +1,193 @@ 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; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; +import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer; -/* - * Sample tracking wheel localizer implementation assuming the standard configuration: +/** + * This is the ThreeWheelLocalizer class. This class extends the Localizer superclass and is a + * localizer that uses the three wheel odometry set up. The diagram below, which is taken from + * Road Runner, shows a typical set up. + * + * The view is from the bottom of the robot looking upwards. * * left on robot is y pos * - * front of robot is x pos + * front on robot is x pos * * /--------------\ * | ____ | * | ---- | * | || || | - * | || || | + * | || || | left (y pos) * | | * | | * \--------------/ + * front (x pos) * - */ - -/** - * This class is adapted from the Road Runner StandardTrackingWheelLocalizer class. Later, this will - * be replaced with a custom localizer. + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 4/2/2024 */ @Config -public class ThreeWheelLocalizer 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 class ThreeWheelLocalizer 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; + private double totalHeading; + public static double FORWARD_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = 0.00052189;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.00053717;//8192 * 1.37795 * 2 * Math.PI * 0.5; - 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 ThreeWheelLocalizer(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 ThreeWheelLocalizer(HardwareMap map) { + this(map, new Pose()); } - public void resetHeading(double heading) { - setPoseEstimate(new Pose2d(getPoseEstimate().getX(), getPoseEstimate().getY(), heading)); + public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { + // TODO: replace these with your encoder positions + leftEncoderPose = new Pose(-18.5/25.4 - 0.1, 164.4/25.4, 0); + rightEncoderPose = new Pose(-18.4/25.4 - 0.1, -159.6/25.4, 0); + strafeEncoderPose = new Pose(0*(-107.9/25.4+8)+-107.9/25.4+0.25, -1.1/25.4-0.23, Math.toRadians(90)); + + hardwareMap = map; + + // TODO: replace these with your encoder ports + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftRear")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // TODO: reverse any encoders necessary + leftEncoder.setDirection(Encoder.REVERSE); + rightEncoder.setDirection(Encoder.REVERSE); + strafeEncoder.setDirection(Encoder.FORWARD); + + setStartPose(setStartPose); + timer = new NanoTimer(); + deltaTimeNano = 1; + displacementPose = new Pose(); + currentVelocity = new Pose(); + totalHeading = 0; + + resetEncoders(); } - 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 - ); + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); } - @NonNull @Override - public List getWheelVelocities() { - int leftVel = (int) leftEncoder.getCorrectedVelocity(); - int rightVel = (int) rightEncoder.getCorrectedVelocity(); - int frontVel = (int) strafeEncoder.getCorrectedVelocity(); + public Pose getVelocity() { + return currentVelocity.copy(); + } - lastEncVels.clear(); - lastEncVels.add(leftVel); - lastEncVels.add(rightVel); - lastEncVels.add(frontVel); + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } - return Arrays.asList( - encoderTicksToInches(leftVel) * X_MULTIPLIER, - encoderTicksToInches(rightVel) * X_MULTIPLIER, - encoderTicksToInches(frontVel) * Y_MULTIPLIER - ); + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + 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); + } + + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + @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); + } + + public void updateEncoders() { + leftEncoder.update(); + rightEncoder.update(); + strafeEncoder.update(); + } + + public void resetEncoders() { + leftEncoder.reset(); + rightEncoder.reset(); + strafeEncoder.reset(); + } + + 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 + returnMatrix.set(2,0, TURN_TICKS_TO_RADIANS * ((rightEncoder.getDeltaPosition() - leftEncoder.getDeltaPosition()) / (leftEncoderPose.getY() - rightEncoderPose.getY()))); + return returnMatrix; + } + + public double getTotalHeading() { + return totalHeading; + } + + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + public double getTurningMultiplier() { + return TURN_TICKS_TO_RADIANS; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java new file mode 100644 index 0000000..26e46bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/TwoWheelLocalizer.java @@ -0,0 +1,198 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization; + +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.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 taken from + * Road Runner, shows a typical set up. + * + * The view is from the bottom of the robot looking upwards. + * + * left on robot is y pos + * + * front on robot is x pos + * + * /--------------\ + * | ____ | + * | ---- | + * | || | + * | || | left (y pos) + * | | + * | | + * \--------------/ + * front (x pos) + * + * @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; + + public TwoWheelLocalizer(HardwareMap map) { + this(map, new Pose()); + } + + 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"); + 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, "leftRear")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "strafeEncoder")); + + // 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; + } + + @Override + public Pose getPose() { + return MathFunctions.addPoses(startPose, displacementPose); + } + + @Override + public Pose getVelocity() { + return currentVelocity.copy(); + } + + @Override + public Vector getVelocityVector() { + return currentVelocity.getVector(); + } + + @Override + public void setStartPose(Pose setStart) { + startPose = setStart; + } + + 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); + } + + @Override + public void setPose(Pose setPose) { + displacementPose = MathFunctions.subtractPoses(setPose, startPose); + resetEncoders(); + } + + @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); + } + + 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; + } + + public void resetEncoders() { + forwardEncoder.reset(); + strafeEncoder.reset(); + } + + 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; + } + + public double getTotalHeading() { + return totalHeading; + } + + public double getForwardMultiplier() { + return FORWARD_TICKS_TO_INCHES; + } + + public double getLateralMultiplier() { + return STRAFE_TICKS_TO_INCHES; + } + + public double getTurningMultiplier() { + return 1; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java new file mode 100644 index 0000000..525d4ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/FowardTuner.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Forward Localizer Tuner", group = "Autonomous Pathing Tuning") +public class FowardTuner extends OpMode { + private PoseUpdater poseUpdater; + + private Telemetry telemetryA; + + public static double DISTANCE = 30; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryA.update(); + } + + @Override + public void loop() { + poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getX()); + telemetryA.addLine("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getForwardMultiplier())); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java new file mode 100644 index 0000000..2995a2e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * You can adjust the target distance on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Lateral Localizer Tuner", group = "Autonomous Pathing Tuning") +public class LateralTuner extends OpMode { + private PoseUpdater poseUpdater; + + private Telemetry telemetryA; + + public static double DISTANCE = 30; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryA.update(); + } + + @Override + public void loop() { + poseUpdater.update(); + telemetryA.addData("distance moved", poseUpdater.getPose().getY()); + telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getX() / poseUpdater.getLocalizer().getLateralMultiplier())); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java new file mode 100644 index 0000000..a6c19e3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -0,0 +1,114 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; +import org.firstinspires.ftc.teamcode.pedroPathing.util.Drawing; + +import java.util.Arrays; +import java.util.List; + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot + * on FTC Dashboard (192/168/43/1:8080/dash). You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@TeleOp(group = "Pedro Pathing Tuning", name = "Localization Test") +public class LocalizationTest extends OpMode { + private PoseUpdater poseUpdater; + private Telemetry telemetryA; + + private DcMotorEx leftFront; + private DcMotorEx leftRear; + private DcMotorEx rightFront; + private DcMotorEx rightRear; + private List motors; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryA.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + + @Override + public void start() { + //poseUpdater.setPose(new Pose()); + } + + @Override + public void loop() { + poseUpdater.update(); + + double y = -gamepad1.left_stick_y; // Remember, this is reversed! + double x = gamepad1.left_stick_x; // this is strafing + double rx = gamepad1.right_stick_x; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double leftFrontPower = (y + x + rx) / denominator; + double leftRearPower = (y - x + rx) / denominator; + double rightFrontPower = (y - x - rx) / denominator; + double rightRearPower = (y + x - rx) / denominator; + + leftFront.setPower(leftFrontPower); + leftRear.setPower(leftRearPower); + rightFront.setPower(rightFrontPower); + rightRear.setPower(rightRearPower); + + telemetryA.addData("x", poseUpdater.getPose().getX()); + telemetryA.addData("y", poseUpdater.getPose().getY()); + telemetryA.addData("heading", poseUpdater.getPose().getHeading()); + telemetryA.addData("total heading", poseUpdater.getTotalHeading()); + telemetryA.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), poseUpdater.getPose()); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java new file mode 100644 index 0000000..4b2838d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/TurnTuner.java @@ -0,0 +1,49 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.PoseUpdater; + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * You can adjust the target angle on FTC Dashboard: 192/168/43/1:8080/dash + * + * @author Anyi Lin - 10158 Scott's Bots + * @version 1.0, 5/6/2024 + */ +@Config +@Autonomous(name = "Turn Localizer Tuner", group = "Autonomous Pathing Tuning") +public class TurnTuner extends OpMode { + private PoseUpdater poseUpdater; + + private Telemetry telemetryA; + + public static double ANGLE = 2 * Math.PI; + + @Override + public void init() { + poseUpdater = new PoseUpdater(hardwareMap); + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.addLine("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryA.update(); + } + + @Override + public void loop() { + poseUpdater.update(); + telemetryA.addData("total angle", poseUpdater.getTotalHeading()); + telemetryA.addLine("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryA.addData("multiplier", ANGLE / (poseUpdater.getTotalHeading() / poseUpdater.getLocalizer().getTurningMultiplier())); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java index 450da65..190bf8a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/BezierCurve.java @@ -164,7 +164,7 @@ public class BezierCurve { public double getCurvature(double t) { t = MathFunctions.clamp(t, 0, 1); Vector derivative = getDerivative(t); - Vector secondDerivative = new Vector(getSecondDerivative(t).getMagnitude(), getApproxSecondDerivative(t).getTheta()); + Vector secondDerivative = getSecondDerivative(t); if (derivative.getMagnitude() == 0) return 0; return (MathFunctions.crossProduct(derivative, secondDerivative))/Math.pow(derivative.getMagnitude(),3); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java index b66193a..0edfbfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; -import com.acmerobotics.roadrunner.geometry.Pose2d; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** * This is the MathFunctions class. This contains many useful math related methods that I use in @@ -105,24 +105,24 @@ public class MathFunctions { } /** - * This returns the distance between a Pose2d and a Point, + * This returns the distance between a Pose and a Point, * - * @param pose this is the Pose2d. + * @param pose this is the Pose. * @param point this is the Point. * @return returns the distance between the two. */ - public static double distance(Pose2d pose, Point point) { + public static double distance(Pose pose, Point point) { return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2)); } /** - * This returns the distance between a Pose2d and another Pose2d. + * This returns the distance between a Pose and another Pose. * - * @param one this is the first Pose2d. - * @param two this is the second Pose2d. + * @param one this is the first Pose. + * @param two this is the second Pose. * @return returns the distance between the two. */ - public static double distance(Pose2d one, Pose2d two) { + public static double distance(Pose one, Pose two) { return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2)); } @@ -137,6 +137,17 @@ public class MathFunctions { return new Point(one.getX() + two.getX(), one.getY() + two.getY(), Point.CARTESIAN); } + /** + * This returns a Pose that is the sum of the two input Pose. + * + * @param one the first Pose + * @param two the second Pose + * @return returns the sum of the two Pose. + */ + public static Pose addPoses(Pose one, Pose two) { + return new Pose(one.getX() + two.getX(), one.getY() + two.getY(), one.getHeading() + two.getHeading()); + } + /** * This subtracts the second Point from the first Point and returns the result as a Point. * Do note that order matters here. @@ -149,6 +160,18 @@ public class MathFunctions { return new Point(one.getX() - two.getX(), one.getY() - two.getY(), Point.CARTESIAN); } + /** + * This subtracts the second Pose from the first Pose and returns the result as a Pose. + * Do note that order matters here. + * + * @param one the first Pose. + * @param two the second Pose. + * @return returns the difference of the two Pose. + */ + public static Pose subtractPoses(Pose one, Pose two) { + return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading()); + } + /** * This multiplies a Point by a scalar and returns the result as a Point * diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java index 563197d..5a2f453 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Path.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; -import com.acmerobotics.roadrunner.geometry.Pose2d; - +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; import org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants; import java.util.ArrayList; @@ -132,7 +131,7 @@ public class Path { * @param searchStepLimit the binary search step limit. * @return returns the closest Point. */ - public Pose2d getClosestPoint(Pose2d pose, int searchStepLimit) { + public Pose getClosestPoint(Pose pose, int searchStepLimit) { double lower = 0; double upper = 1; Point returnPoint; @@ -156,7 +155,7 @@ public class Path { closestPointCurvature = curve.getCurvature(closestPointTValue); - return new Pose2d(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); + return new Pose(returnPoint.getX(), returnPoint.getY(), getClosestPointHeadingGoal()); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java index b37a140..7322447 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration; -import com.acmerobotics.roadrunner.geometry.Pose2d; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; /** * This is the Point class. This class handles storing information about the location of points in @@ -45,11 +45,11 @@ public class Point { } /** - * This creates a new Point from a Pose2d. + * This creates a new Point from a Pose. * - * @param pose the Pose2d. + * @param pose the Pose. */ - public Point(Pose2d pose) { + public Point(Pose pose) { setCoordinates(pose.getX(), pose.getY(), CARTESIAN); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java new file mode 100644 index 0000000..6d9af9c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.util; + +import com.acmerobotics.dashboard.canvas.Canvas; + +import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; + +/* +* Drawing class needed for putting your robot onto the dashboard +* Edit the radius needed to align to your robot the best possible simulation represented + */ +public class Drawing { + private Drawing() {} + + public static void drawRobot(Canvas c, Point t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + + Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta()); + Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta())); + Vector p2 = MathFunctions.addVectors(p1, halfv); + c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent()); + } + + public static void drawRobot(Canvas c, Pose t) { + final double ROBOT_RADIUS = 9; + + c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS); + Vector v = t.getHeadingVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = t.getX() + v.getXComponent() / 2, y1 = t.getY() + v.getYComponent() / 2; + double x2 = t.getX() + v.getXComponent(), y2 = t.getY() + v.getYComponent(); + c.strokeLine(x1, y1, x2, y2); + } +}