DeadWheelTuner

DeadWheelTuner.java

package org.firstinspires.ftc.teamcode.roadrunner.tuning;


import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer;

//@Disabled
@Config
@TeleOp(group = "tuning")
public class DeadWheelTuner extends LinearOpMode {
    public static int NUM_TURNS = 10;

    @Override
    public void runOpMode() throws InterruptedException {
        MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));


        telemetry.addLine("Press play to begin the tuning routine.");
        telemetry.addData("Task: ","Turn the robot %d times", NUM_TURNS);
        telemetry.update();

        waitForStart();

        if (isStopRequested()) return;

        telemetry.clearAll();
        telemetry.update();

        double headingAccumulator = 0;
        double lastHeading = 0;

        boolean tuningFinished = false;

        while (!isStopRequested() && !tuningFinished) {
            drive.setDrivePowers(new PoseVelocity2d(new Vector2d(0, 0), -gamepad1.right_stick_x));

            drive.updatePoseEstimate();

            double heading = drive.pose.heading.log();
            double deltaHeading = heading - lastHeading;

            headingAccumulator += Math.atan2(Math.sin(deltaHeading), Math.cos(deltaHeading));
            lastHeading = heading;

            telemetry.clearAll();
            telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
            telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
            telemetry.addData("X", drive.pose.position.x);
            telemetry.addData("Y", drive.pose.position.x);
            telemetry.addLine();
            telemetry.addLine("Press Y/△ to conclude routine");
            telemetry.update();

            if (gamepad1.y)
                tuningFinished = true;
        }

        double configuredLateralDistance = Math.abs(ThreeDeadWheelLocalizer.PARAMS.par0YTicks)+Math.abs(ThreeDeadWheelLocalizer.PARAMS.par1YTicks);

        telemetry.clearAll();
        telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
        telemetry.addLine("Configured LateralDistance: " + configuredLateralDistance);
        telemetry.addLine("Effective LateralDistance: " +
                (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * configuredLateralDistance);
        telemetry.addData("X", drive.pose.position.x);
        telemetry.addData("Y", drive.pose.position.x);

        telemetry.update();

        while (!isStopRequested()) idle();
    }
}