ThreeDeadWheelLocalizer with X_MULTIPLIER and Y_MULTIPLIER

ThreeDeadWheelLocalizer.java

package org.firstinspires.ftc.teamcode.roadrunner;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;

@Config
public final class ThreeDeadWheelLocalizer implements Localizer {
    public static class Params {
        public double par0YTicks = -1485; // y position of the first parallel encoder (in tick units)
        public double par1YTicks = 1625; // y position of the second parallel encoder (in tick units)
        public double perpXTicks = 2123; // x position of the perpendicular encoder (in tick units)}`

        public double X_MULTIPLIER = 1;
        public double Y_MULTIPLIER = 1;
    }

    public static Params PARAMS = new Params();

    public final Encoder par0, par1, perp;

    public final double inPerTick;

    private int lastPar0Pos, lastPar1Pos, lastPerpPos;

    public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
        par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "rightBack")));
        par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftBack")));
        perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "leftFront")));
//        perp.setDirection(DcMotorEx.Direction.REVERSE);

        lastPar0Pos = (int) (par0.getPositionAndVelocity().position * PARAMS.X_MULTIPLIER);
        lastPar1Pos = (int) (par1.getPositionAndVelocity().position * PARAMS.X_MULTIPLIER);
        lastPerpPos = (int) (perp.getPositionAndVelocity().position * PARAMS.Y_MULTIPLIER);

        this.inPerTick = inPerTick;
    }

    public Twist2dDual<Time> update() {
        PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
        PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
        PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();

        int par0Position = (int) (par0PosVel.position * PARAMS.X_MULTIPLIER);
        int par1Position = (int) (par1PosVel.position * PARAMS.X_MULTIPLIER);
        int perpPosition = (int) (perpPosVel.position * PARAMS.Y_MULTIPLIER);

        int par0Velocity = (int) (par0PosVel.velocity * PARAMS.X_MULTIPLIER);
        int par1Velocity = (int) (par1PosVel.velocity * PARAMS.X_MULTIPLIER);
        int perpVelocity = (int) (perpPosVel.velocity * PARAMS.Y_MULTIPLIER);

        int par0PosDelta = par0Position - lastPar0Pos;
        int par1PosDelta = par1Position - lastPar1Pos;
        int perpPosDelta = perpPosition - lastPerpPos;

        Twist2dDual<Time> twist = new Twist2dDual<>(
                new Vector2dDual<>(
                        new DualNum<Time>(new double[] {
                                (PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                                (PARAMS.par0YTicks * par1Velocity - PARAMS.par1YTicks * par0Velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                        }).times(inPerTick),
                        new DualNum<Time>(new double[] {
                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
                                (PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1Velocity - par0Velocity) + perpVelocity),
                        }).times(inPerTick)
                ),
                new DualNum<>(new double[] {
                        (par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                        (par0Velocity - par1Velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
                })
        );

        lastPar0Pos = par0Position;
        lastPar1Pos = par1Position;
        lastPerpPos = perpPosDelta;

        return twist;
    }
}