MoonshotPose.java

package frc.robot;

import frc.robot.controllers.DriveController;
import edu.wpi.first.wpilibj.drive.Vector2d;
import java.lang.Math;
public class Odometry {
    DriveController driveController;
    private double prevX, prevY, prevDistance;
    private double currX, currY, currDistance;
    public Vector2d robotVector;
    
    public Odometry(DriveController driveController) {
        this.driveController = driveController;
        prevX = prevY = prevDistance = currX = currY = currDistance = 0;
        robotVector = new Vector2d(currX, currY);
    }

    public void update() {
        currDistance = driveController.getAverageTotalDistance() - prevDistance;
        currX = prevX + currDistance*Math.cos(driveController.getTotalGyroAngle());
        currY = prevY + currDistance*Math.sin(driveController.getTotalGyroAngle());
        robotVector.x = currX;
        robotVector.y = currY;

        prevX = currX;
        prevY = currY;
        
        prevDistance = currDistance;
    }
    public Vector2d applyRotation(double angle) {
        Vector2d targetVector = new Vector2d(robotVector.x, robotVector.y);
        targetVector.rotate(angle);
        return targetVector;
    }
    
    public double getVectorAngle(Vector2d vector) {
        return Math.atan2(vector.x, vector.y);
    }
}