TwoWheel + PinpointIMU Localizer

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelPinpointIMULocalizer.java

@@ -0,0 +1,300 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.GoBildaPinpointDriver;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Localizer;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Matrix;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
+
+
+/**
+ * This is the TwoWheelLocalizer class. This class extends the Localizer superclass and is a
+ * localizer that uses the two wheel odometry with IMU set up. The diagram below, which is modified from
+ * Road Runner, shows a typical set up.
+ *
+ * The view is from the top of the robot looking downwards.
+ *
+ * left on robot is the y positive direction
+ *
+ * forward on robot is the x positive direction
+ *
+ *    /--------------\
+ *    |     ____     |
+ *    |     ----     |
+ *    | ||        || |
+ *    | ||        || |  ----> left (y positive)
+ *    |              |
+ *    |              |
+ *    \--------------/
+ *           |
+ *           |
+ *           V
+ *    forward (x positive)
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Havish Sripada - 12808 RevAmped Robotics
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 11/28/2024
+ */
+@Config
+public class TwoWheelPinpointIMULocalizer extends Localizer {
+    private HardwareMap hardwareMap;
+    private GoBildaPinpointDriver pinpoint;
+    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 previousHeading;
+    private double deltaRadians;
+    private double totalHeading;
+    public static double FORWARD_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5008239963;
+    public static double STRAFE_TICKS_TO_INCHES = 8192 * 1.37795 * 2 * Math.PI * 0.5018874659;
+
+    /**
+     * This creates a new TwoWheelLocalizer from a HardwareMap, with a starting Pose at (0,0)
+     * facing 0 heading.
+     *
+     * @param map the HardwareMap
+     */
+    public TwoWheelPinpointIMULocalizer(HardwareMap map) {
+        this(map, new Pose());
+    }
+
+    /**
+     * This creates a new TwoWheelLocalizer from a HardwareMap and a Pose, with the Pose
+     * specifying the starting pose of the localizer.
+     *
+     * @param map          the HardwareMap
+     * @param setStartPose the Pose to start from
+     */
+    public TwoWheelPinpointIMULocalizer(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;
+
+        pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
+        pinpoint.resetPosAndIMU();
+
+        // 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();
+        deltaRadians = 0;
+    }
+
+    /**
+     * This returns the current pose estimate.
+     *
+     * @return returns the current pose estimate as a Pose
+     */
+    @Override
+    public Pose getPose() {
+        return MathFunctions.addPoses(startPose, displacementPose);
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Pose
+     */
+    @Override
+    public Pose getVelocity() {
+        return currentVelocity.copy();
+    }
+
+    /**
+     * This returns the current velocity estimate.
+     *
+     * @return returns the current velocity estimate as a Vector
+     */
+    @Override
+    public Vector getVelocityVector() {
+        return currentVelocity.getVector();
+    }
+
+    /**
+     * This sets the start pose. Changing the start pose should move the robot as if all its
+     * previous movements were displacing it from its new start pose.
+     *
+     * @param setStart the new start pose
+     */
+    @Override
+    public void setStartPose(Pose setStart) {
+        startPose = setStart;
+    }
+
+    /**
+     * This sets the Matrix that contains the previous pose's heading rotation.
+     *
+     * @param heading the rotation of the Matrix
+     */
+    public void setPrevRotationMatrix(double heading) {
+        prevRotationMatrix = new Matrix(3, 3);
+        prevRotationMatrix.set(0, 0, Math.cos(heading));
+        prevRotationMatrix.set(0, 1, -Math.sin(heading));
+        prevRotationMatrix.set(1, 0, Math.sin(heading));
+        prevRotationMatrix.set(1, 1, Math.cos(heading));
+        prevRotationMatrix.set(2, 2, 1.0);
+    }
+
+    /**
+     * This sets the current pose estimate. Changing this should just change the robot's current
+     * pose estimate, not anything to do with the start pose.
+     *
+     * @param setPose the new current pose estimate
+     */
+    @Override
+    public void setPose(Pose setPose) {
+        displacementPose = MathFunctions.subtractPoses(setPose, startPose);
+        resetEncoders();
+    }
+
+    /**
+     * This updates the elapsed time timer that keeps track of time between updates, as well as the
+     * change position of the Encoders and the IMU readings. Then, the robot's global change in
+     * position is calculated using the pose exponential method.
+     */
+    @Override
+    public void update() {
+        deltaTimeNano = timer.getElapsedTime();
+        timer.resetTimer();
+
+        updateEncoders();
+        Matrix robotDeltas = getRobotDeltas();
+        Matrix globalDeltas;
+        setPrevRotationMatrix(getPose().getHeading());
+
+        Matrix transformation = new Matrix(3, 3);
+        if (Math.abs(robotDeltas.get(2, 0)) < 0.001) {
+            transformation.set(0, 0, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
+            transformation.set(0, 1, -robotDeltas.get(2, 0) / 2.0);
+            transformation.set(1, 0, robotDeltas.get(2, 0) / 2.0);
+            transformation.set(1, 1, 1.0 - (Math.pow(robotDeltas.get(2, 0), 2) / 6.0));
+            transformation.set(2, 2, 1.0);
+        } else {
+            transformation.set(0, 0, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
+            transformation.set(0, 1, (Math.cos(robotDeltas.get(2, 0)) - 1.0) / robotDeltas.get(2, 0));
+            transformation.set(1, 0, (1.0 - Math.cos(robotDeltas.get(2, 0))) / robotDeltas.get(2, 0));
+            transformation.set(1, 1, Math.sin(robotDeltas.get(2, 0)) / robotDeltas.get(2, 0));
+            transformation.set(2, 2, 1.0);
+        }
+
+        globalDeltas = Matrix.multiply(Matrix.multiply(prevRotationMatrix, transformation), robotDeltas);
+
+        displacementPose.add(new Pose(globalDeltas.get(0, 0), globalDeltas.get(1, 0), globalDeltas.get(2, 0)));
+        currentVelocity = new Pose(globalDeltas.get(0, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(1, 0) / (deltaTimeNano / Math.pow(10.0, 9)), globalDeltas.get(2, 0) / (deltaTimeNano / Math.pow(10.0, 9)));
+
+        totalHeading += globalDeltas.get(2, 0);
+    }
+
+    /**
+     * This updates the Encoders as well as the IMU.
+     */
+    public void updateEncoders() {
+        forwardEncoder.update();
+        strafeEncoder.update();
+
+        pinpoint.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING);
+        double currentHeading = startPose.getHeading() + MathFunctions.normalizeAngle(pinpoint.getHeading());
+        deltaRadians = MathFunctions.getTurnDirection(previousHeading, currentHeading) * MathFunctions.getSmallestAngleDifference(currentHeading, previousHeading);
+        previousHeading = currentHeading;
+    }
+
+    /**
+     * This resets the Encoders.
+     */
+    public void resetEncoders() {
+        forwardEncoder.reset();
+        strafeEncoder.reset();
+    }
+
+    /**
+     * This calculates the change in position from the perspective of the robot using information
+     * from the Encoders and IMU.
+     *
+     * @return returns a Matrix containing the robot relative movement.
+     */
+    public Matrix getRobotDeltas() {
+        Matrix returnMatrix = new Matrix(3, 1);
+        // x/forward movement
+        returnMatrix.set(0, 0, FORWARD_TICKS_TO_INCHES * (forwardEncoder.getDeltaPosition() - forwardEncoderPose.getY() * deltaRadians));
+        //y/strafe movement
+        returnMatrix.set(1, 0, STRAFE_TICKS_TO_INCHES * (strafeEncoder.getDeltaPosition() - strafeEncoderPose.getX() * deltaRadians));
+        // theta/turning
+        returnMatrix.set(2, 0, deltaRadians);
+        return returnMatrix;
+    }
+
+    /**
+     * This returns how far the robot has turned in radians, in a number not clamped between 0 and
+     * 2 * pi radians. This is used for some tuning things and nothing actually within the following.
+     *
+     * @return returns how far the robot has turned in total, in radians.
+     */
+    public double getTotalHeading() {
+        return totalHeading;
+    }
+
+    /**
+     * This returns the multiplier applied to forward movement measurement to convert from encoder
+     * ticks to inches. This is found empirically through a tuner.
+     *
+     * @return returns the forward ticks to inches multiplier
+     */
+    public double getForwardMultiplier() {
+        return FORWARD_TICKS_TO_INCHES;
+    }
+
+    /**
+     * This returns the multiplier applied to lateral/strafe movement measurement to convert from
+     * encoder ticks to inches. This is found empirically through a tuner.
+     *
+     * @return returns the lateral/strafe ticks to inches multiplier
+     */
+    public double getLateralMultiplier() {
+        return STRAFE_TICKS_TO_INCHES;
+    }
+
+    /**
+     * This returns the multiplier applied to turning movement measurement to convert from encoder
+     * ticks to radians. This is found empirically through a tuner.
+     *
+     * @return returns the turning ticks to radians multiplier
+     */
+    public double getTurningMultiplier() {
+        return 1;
+    }
+
+    /**
+     * This resets the IMU.
+     */
+
+    public void resetIMU() {
+        pinpoint.resetPosAndIMU();
+    }
+}
+