From 14b5f267fb45b57297939f37ad38b63457a831fd Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 13 Jul 2024 23:09:55 -0400 Subject: [PATCH] Added new localizer that uses three tracking wheels, with the IMU for most heading tracking. Also fixed the math for the teleop centripetal force correction with help from @junkjunk123 on Discord --- .../pedroPathing/follower/Follower.java | 2 +- .../pedroPathing/localization/README.md | 57 +++- .../localization/ThreeWheelIMULocalizer.Java | 304 ++++++++++++++++++ .../localization/TwoWheelLocalizer.java | 1 + 4 files changed, 361 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java 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 16a9819..577c67f 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 @@ -789,7 +789,7 @@ public class Follower { } 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); + curvature = (yDoublePrime) / (Math.pow(Math.sqrt(1 + Math.pow(yPrime, 2)), 3)); } 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())); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md index 74267d3..9e4299a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/README.md @@ -12,11 +12,13 @@ with the localizer that applies to you: * If you're using two wheel odometry, put `new TwoWheelLocalizer(hardwareMap)` * If you're using three wheel odometry, put `new ThreeWheelLocalizer(hardwareMap)`, so basically don't change it from the default +* If you're using three wheel odometry with the IMU, put `new ThreeWheelIMULocalizer(hardwareMap)` ## Tuning To start, you'll want to select your localizer of choice. Below, I'll have instructions for the drive -encoder localizer, two tracking wheel localizer, and the three tracking wheel localizer offered in -Pedro Pathing. Scroll down to the section that applies to you and follow the directions there. +encoder localizer, two tracking wheel localizer, the three tracking wheel localizer, and the three +wheel with IMU localizer offered in Pedro Pathing. Scroll down to the section that applies to you +and follow the directions there. # Drive Encoders * First, you'll need all of your drive motors to have encoders attached. @@ -70,6 +72,8 @@ to. The names of the variables is where on the robot the corresponding motor sho * Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The variable names correspond to which tracking wheel should be connected. +* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that + of your robot's. * Then, reverse the direction of any encoders so that the forward encoder ticks up when the robot is moving forward and the strafe encoder ticks up when the robot moves right. * Now, you'll have to tune the multipliers. These convert your measurements from encoder ticks into @@ -144,6 +148,55 @@ to. The names of the variables is where on the robot the corresponding motor sho robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! +# Three Wheel Localizer with IMU +* First, you'll need three odometry wheels connected to motor encoder ports on a hub. +* Then, go to `ThreeWheelIMULocalizer.java`. First, in the constructor, enter in the positions of your + tracking wheels relative to the center of the wheels of the robot. The positions are in inches, so + convert measurements accordingly. Use the comment above the class declaration to help you with the + coordinates. +* Next, go to where it tells you to replace the current statements with your encoder ports in the constructor. + Replace the `deviceName` parameter with the name of the port that the encoder is connected to. The + variable names correspond to which tracking wheel should be connected. +* After that, go to the instantiation of the IMU and change the orientation of the IMU to match that + of your robot's. +* Then, reverse the direction of any encoders so that the forward encoders tick up when the robot + is moving forward and the strafe encoder ticks up when the robot moves right. +* Although heading localization is done mostly through the IMU, the tracking wheels are still used for + small angle adjustments for better stability. So, you will still need to tune your turning multiplier. +* First, start with the `Turn Localizer Tuner`. Before doing any tuning, go to FTC Dashboard and find + the `ThreeWheelIMULocalizer` dropdown and deselect `useIMU`. You'll want to position your robot to be facing + in a direction you can easily find again, like lining up an edge of the robot against a field tile edge. + By default, you should spin the robot for one rotation going counterclockwise. Once you've spun + exactly that one rotation, or whatever you set that value to, then the turn multiplier will be shown + as the second number shown. The first number is how far the robot thinks you've spun, and the second + number is the multiplier you need to have to scale your current readings to your goal of one rotation, + or the custom set angle. Feel free to run a few more tests and average the results. Once you have + this multiplier, then replace `TURN_TICKS_TO_RADIANS` in the localizer with your multiplier. Make sure + you replace the number, not add on or multiply it to the previous number. The tuner takes into + account your current multiplier. +* Next, go on to `Forward Localizer Tuner`. You should re-enable `useIMU` at this time. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches forward. Once you've pushed that far, or whatever + you set that value to, then the forward multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `FORWARD_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Finally, go to `Lateral Localizer Tuner`. `useIMU` should be enabled for this step. You'll want to position a rule alongside your robot. + By default, you'll want to push the robot 30 inches to the right. Once you've pushed that far, or whatever + you set that value to, then the lateral multiplier will be shown as the second number shown. The + first number is how far the robot thinks you've gone, and the second number is the multiplier you + need to have to scale your current readings to your goal of 30 inches, or the custom set angle. + Feel free to run a few more tests and average the results. Once you have this multiplier, then + replace `STRAFE_TICKS_TO_INCHES` in the localizer with your multiplier. Make sure you replace the number, + not add on or multiply it to the previous number. The tuner takes into account your current multiplier. +* Once you're done with all this, your localizer should be tuned. Make sure that `useIMU` is turned back on. To test it out, you can go to + `Localization Test` and push around or drive around your robot. Go to [FTC Dashboard](http://192.168.43.1:8080/dash) + and on the top right, switch the drop down from the default view to the field view. Then, on the bottom + left corner, you should see a field and the robot being drawn on the field. You can then move your + robot around and see if the movements look accurate on FTC Dashboard. If they don't, then you'll + want to re-run some of the previous steps. Otherwise, congrats on tuning your localizer! + ## Using Road Runner's Localizer Of course, many teams have experience using Road Runner in the past and so have localizers from Road Runner that are tuned. There is an adapter for the Road Runner three wheel localizer to the Pedro diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java new file mode 100644 index 0000000..69a1f03 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/ThreeWheelIMULocalizer.Java @@ -0,0 +1,304 @@ +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.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +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 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 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.002957;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = -0.003127403096038503;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.002995;//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(RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP))); + + // TODO: replace these with your encoder positions + leftEncoderPose = new Pose(-3, 5.7, 0); + rightEncoderPose = new Pose(-3, -5.7, 0); + strafeEncoderPose = new Pose(6.9, 1, Math.toRadians(90)); + + + + // TODO: replace these with your encoder ports + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lb")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "lf")); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rf")); + + // TODO: reverse any encoders necessary + leftEncoder.setDirection(Encoder.REVERSE); + rightEncoder.setDirection(Encoder.FORWARD); + strafeEncoder.setDirection(Encoder.FORWARD); + + 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; + } +} \ No newline at end of file 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 index 5c4e932..c4284b9 100644 --- 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 @@ -80,6 +80,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w 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