+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds. + *
+ * Further fine tuning of MAX_ANG_VEL may be desired. + */ + +@Config +@Autonomous(group = "tuning") +public class MaxAngularVeloTuner extends LinearOpMode { + public static double RUNTIME = 4.0; + + private ElapsedTime timer; + private double maxAngVelocity = 0.0; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(0, 0, 1)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity); + } + + drive.setDrivePower(new Pose2d()); + + telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity); + telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)); + telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8); + telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8)); + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java new file mode 100644 index 0000000..3b9d0a1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MaxVelocityTuner.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum velocity your bot can achieve under load. It + * will also calculate the effective kF value for your velocity PID. + *
+ * Upon pressing start, your bot will run at max power for RUNTIME seconds. + *
+ * Further fine tuning of kF may be desired. + */ +@Config +@Autonomous(group = "tuning") +public class MaxVelocityTuner extends LinearOpMode { + public static double RUNTIME = 2.0; + + private ElapsedTime timer; + private double maxVelocity = 0.0; + + private VoltageSensor batteryVoltageSensor; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); + } + + drive.setDrivePower(new Pose2d()); + + double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); + + telemetry.addData("Max Velocity", maxVelocity); + telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8); + telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) idle(); + } + + private double veloInchesToTicks(double inchesPerSec) { + return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java new file mode 100644 index 0000000..97cb52e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MotorDirectionDebugger.java @@ -0,0 +1,91 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for debugging your motor configuration. + * Pressing each of the buttons will power its respective motor. + *
+ * Button Mappings: + *
+ * Xbox/PS4 Button - Motor + * X / ▢ - Front Left + * Y / Δ - Front Right + * B / O - Rear Right + * A / X - Rear Left + * The buttons are mapped to match the wheels spatially if you + * were to rotate the gamepad 45deg°. x/square is the front left + * ________ and each button corresponds to the wheel as you go clockwise + * / ______ \ + * ------------.-' _ '-..+ Front of Bot + * / _ ( Y ) _ \ ^ + * | ( X ) _ ( B ) | Front Left \ Front Right + * ___ '. ( A ) /| Wheel \ Wheel + * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) + * | | | \ + * '.___.' '. | Rear Left \ Rear Right + * '. / Wheel \ Wheel + * \. .' (A/X) \ (B/O) + * \________/ + *
+ * Uncomment the @Disabled tag below to use this opmode. + */ +@Config +@TeleOp(group = "tuning") +public class MotorDirectionDebugger extends LinearOpMode { + public static double MOTOR_POWER = 0.7; + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + telemetry.addLine("Press play to begin the debugging opmode"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + while (!isStopRequested()) { + telemetry.addLine("Press each button to turn on its respective motor"); + telemetry.addLine(); + telemetry.addLine("Xbox/PS4 Button - Motor"); + telemetry.addLine(" X / ▢ - Front Left"); + telemetry.addLine(" Y / Δ - Front Right"); + telemetry.addLine(" B / O - Rear Right"); + telemetry.addLine(" A / X - Rear Left"); + telemetry.addLine(); + + if (gamepad1.x) { + drive.setMotorPowers(MOTOR_POWER, 0, 0, 0); + telemetry.addLine("Running Motor: Front Left"); + } else if (gamepad1.y) { + drive.setMotorPowers(0, 0, 0, MOTOR_POWER); + telemetry.addLine("Running Motor: Front Right"); + } else if (gamepad1.b) { + drive.setMotorPowers(0, 0, MOTOR_POWER, 0); + telemetry.addLine("Running Motor: Rear Right"); + } else if (gamepad1.a) { + drive.setMotorPowers(0, MOTOR_POWER, 0, 0); + telemetry.addLine("Running Motor: Rear Left"); + } else { + drive.setMotorPowers(0, 0, 0, 0); + telemetry.addLine("Running Motor: None"); + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java new file mode 100644 index 0000000..c3e5bb9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; + +/* + * This is an example of a more complex path to really test the tuning. + */ +@Config +@Autonomous(group = "tuning") +public class SplineTest extends LinearOpMode { + public static int TRIALS = 1; + public static int DELAY = 2000; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + for (int i = 0; i < TRIALS; i++) { + Trajectory traj = drive.trajectoryBuilder(new Pose2d()) + .splineTo(new Vector2d(30, 30), 0) + .build(); + + drive.followTrajectory(traj); + + sleep(DELAY); + + drive.followTrajectory( + drive.trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + ); + + sleep(DELAY); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java new file mode 100644 index 0000000..f58e7ae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StrafeTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "tuning") +public class StrafeTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .strafeRight(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java new file mode 100644 index 0000000..37d5cb2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/StraightTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "tuning") +public class StraightTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java new file mode 100644 index 0000000..d24502e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackWidthTuner.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.subsystem.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; + +/* + * This routine determines the effective track width. The procedure works by executing a point turn + * with a given angle and measuring the difference between that angle and the actual angle (as + * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient + * given angle / actual angle gives a multiplicative adjustment to the estimated track width + * (effective track width = estimated track width * given angle / actual angle). The routine repeats + * this procedure a few times and averages the values for additional accuracy. Note: a relatively + * accurate track width estimate is important or else the angular constraints will be thrown off. + */ +@Config +@Autonomous(group = "tuning") +public class TrackWidthTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + // TODO: if you haven't already, set the localizer to something that doesn't depend on + // drive encoders for computing the heading + + telemetry.addLine("Press play to begin the track width tuner routine"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.normDelta(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator; + trackWidthStats.add(trackWidth); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)", + trackWidthStats.getMean(), + trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java new file mode 100644 index 0000000..2108295 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelForwardOffsetTuner.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.subsystem.drive.StandardTrackingWheelLocalizer; + +/** + * This routine determines the effective forward offset for the lateral tracking wheel. + * The procedure executes a point turn at a given angle for a certain number of trials, + * along with a specified delay in milliseconds. The purpose of this is to track the + * change in the y position during the turn. The offset, or distance, of the lateral tracking + * wheel from the center or rotation allows the wheel to spin during a point turn, leading + * to an incorrect measurement for the y position. This creates an arc around around + * the center of rotation with an arc length of change in y and a radius equal to the forward + * offset. We can compute this offset by calculating (change in y position) / (change in heading) + * which returns the radius if the angle (change in heading) is in radians. This is based + * on the arc length formula of length = theta * radius. + *
+ * To run this routine, simply adjust the desired angle and specify the number of trials + * and the desired delay. Then, run the procedure. Once it finishes, it will print the + * average of all the calculated forward offsets derived from the calculation. This calculated + * forward offset is then added onto the current forward offset to produce an overall estimate + * for the forward offset. You can run this procedure as many times as necessary until a + * satisfactory result is produced. + */ +@Config +@Autonomous(group = "tuning") +public class TrackingWheelForwardOffsetTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Press play to begin the forward offset tuner"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.norm(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET + + drive.getPoseEstimate().getY() / headingAccumulator; + forwardOffsetStats.add(forwardOffset); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)", + forwardOffsetStats.getMean(), + forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java new file mode 100644 index 0000000..f24396f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TrackingWheelLateralDistanceTuner.java @@ -0,0 +1,130 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.subsystem.drive.StandardTrackingWheelLocalizer; + +/** + * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s + * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel + * wheels. + *
+ * Tuning Routine: + *
+ * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical + * measured value. This need only be an estimated value as you will be tuning it anyways. + *
+ * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an + * similar mark right below the indicator on your bot. This will be your reference point to + * ensure you've turned exactly 360°. + *
+ * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help + * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash + * if you are using the Control Hub. + * Ensure the field is showing (select the field view in top right of the page). + *
+ * 4. Press play to begin the tuning routine. + *
+ * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. + *
+ * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. + *
+ * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * on the bot and on the ground you created earlier should be lined up. + *
+ * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your + * StandardTrackingWheelLocalizer.java class. + *
+ * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value + * yourself. Read the heading output and follow the advice stated in the note below to manually + * nudge the values yourself. + *
+ * Note: + * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with + * a line from the circumference to the center should be present, representing the bot. The line + * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in + * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than + * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the + * actual bot, the LATERAL_DISTANCE should be increased. + *
+ * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
+ * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
+ * effective center of rotation. You can ignore this effect. The center of rotation will be offset
+ * slightly but your heading will still be fine. This does not affect your overall tracking
+ * precision. The heading should still line up.
+ */
+@Config
+@TeleOp(group = "tuning")
+public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
+ public static int NUM_TURNS = 10;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+ RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+ }
+
+ telemetry.addLine("Prior to beginning the routine, please read the directions "
+ + "located in the comments of the opmode file.");
+ telemetry.addLine("Press play to begin the tuning routine.");
+ telemetry.addLine("");
+ telemetry.addLine("Press Y/△ to stop the routine.");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ boolean tuningFinished = false;
+
+ while (!isStopRequested() && !tuningFinished) {
+ Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
+ drive.setDrivePower(vel);
+
+ drive.update();
+
+ double heading = drive.getPoseEstimate().getHeading();
+ double deltaHeading = heading - lastHeading;
+
+ headingAccumulator += Angle.normDelta(deltaHeading);
+ lastHeading = heading;
+
+ telemetry.clearAll();
+ telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
+ telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
+ telemetry.addLine();
+ telemetry.addLine("Press Y/△ to conclude routine");
+ telemetry.update();
+
+ if (gamepad1.y)
+ tuningFinished = true;
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
+ telemetry.addLine("Effective LATERAL_DISTANCE: " +
+ (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
+
+ telemetry.update();
+
+ while (!isStopRequested()) idle();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
new file mode 100644
index 0000000..1b28823
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurnTest.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.subsystem.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+@Config
+@Autonomous(group = "tuning")
+public class TurnTest extends LinearOpMode {
+ public static double ANGLE = 90; // deg
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.turn(Math.toRadians(ANGLE));
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
new file mode 100644
index 0000000..1b34a95
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.IOException;
+import java.io.InputStream;
+
+/**
+ * Set of utilities for loading trajectories from assets (the plugin save location).
+ */
+public class AssetsTrajectoryManager {
+
+ /**
+ * Loads the group config.
+ */
+ public static @Nullable
+ TrajectoryGroupConfig loadGroupConfig() {
+ try {
+ InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+ "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
+ return TrajectoryConfigManager.loadGroupConfig(inputStream);
+ } catch (IOException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Loads a trajectory config with the given name.
+ */
+ public static @Nullable TrajectoryConfig loadConfig(String name) {
+ try {
+ InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+ "trajectory/" + name + ".yaml");
+ return TrajectoryConfigManager.loadConfig(inputStream);
+ } catch (IOException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Loads a trajectory builder with the given name.
+ */
+ public static @Nullable TrajectoryBuilder loadBuilder(String name) {
+ TrajectoryGroupConfig groupConfig = loadGroupConfig();
+ TrajectoryConfig config = loadConfig(name);
+ if (groupConfig == null || config == null) {
+ return null;
+ }
+ return config.toTrajectoryBuilder(groupConfig);
+ }
+
+ /**
+ * Loads a trajectory with the given name.
+ */
+ public static @Nullable Trajectory load(String name) {
+ TrajectoryBuilder builder = loadBuilder(name);
+ if (builder == null) {
+ return null;
+ }
+ return builder.build();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
new file mode 100644
index 0000000..42271d6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * IMU axes signs in the order XYZ (after remapping).
+ */
+public enum AxesSigns {
+ PPP(0b000),
+ PPN(0b001),
+ PNP(0b010),
+ PNN(0b011),
+ NPP(0b100),
+ NPN(0b101),
+ NNP(0b110),
+ NNN(0b111);
+
+ public final int bVal;
+
+ AxesSigns(int bVal) {
+ this.bVal = bVal;
+ }
+
+ public static AxesSigns fromBinaryValue(int bVal) {
+ int maskedVal = bVal & 0x07;
+ switch (maskedVal) {
+ case 0b000:
+ return AxesSigns.PPP;
+ case 0b001:
+ return AxesSigns.PPN;
+ case 0b010:
+ return AxesSigns.PNP;
+ case 0b011:
+ return AxesSigns.PNN;
+ case 0b100:
+ return AxesSigns.NPP;
+ case 0b101:
+ return AxesSigns.NPN;
+ case 0b110:
+ return AxesSigns.NNP;
+ case 0b111:
+ return AxesSigns.NNN;
+ default:
+ throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
new file mode 100644
index 0000000..5b8279b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * A direction for an axis to be remapped to
+ */
+public enum AxisDirection {
+ POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
new file mode 100644
index 0000000..ce11d8d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.Path;
+
+import java.util.List;
+
+/**
+ * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
+ */
+public class DashboardUtil {
+ private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
+ private static final double ROBOT_RADIUS = 9; // in
+
+
+ public static void drawPoseHistory(Canvas canvas, List");
+ File[] fs = Objects.requireNonNull(ROOT.listFiles());
+ Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified()));
+ for (File f : fs) {
+ sb.append("
");
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK,
+ NanoHTTPD.MIME_HTML, sb.toString());
+ });
+
+ manager.register("/logs/download", session -> {
+ final String[] pairs = session.getQueryParameterString().split("&");
+ if (pairs.length != 1) {
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+ NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length);
+ }
+
+ final String[] parts = pairs[0].split("=");
+ if (!parts[0].equals("file")) {
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+ NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]);
+ }
+
+ File f = new File(ROOT, parts[1]);
+ if (!f.exists()) {
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+ NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist");
+ }
+
+ return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+ "application/json", new FileInputStream(f));
+ });
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
new file mode 100644
index 0000000..2c558f1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * Utility functions for log files.
+ */
+public class LoggingUtil {
+ public static final File ROAD_RUNNER_FOLDER =
+ new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
+
+ private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
+
+ private static void buildLogList(List