From b23e0fafbf90874c216d4c23b3401fadf5a60600 Mon Sep 17 00:00:00 2001 From: keshavanandcode Date: Thu, 10 Sep 2026 08:37:22 -0500 Subject: [PATCH] upgrade pedro --- TeamCode/build.gradle | 5 - .../ftc/teamcode/pedro/Constants.java | 11 + .../ftc/teamcode/pedro/Tuning.java | 5 + .../pedro/procedures/ForesightTuner.java | 1222 +++++++++++ .../pedro/procedures/MecanumTuner.java | 96 + .../teamcode/pedro/procedures/OTOSTuner.java | 210 ++ .../pedro/procedures/OctoQuadTuner.java | 350 ++++ .../pedro/procedures/PinpointTuner.java | 256 +++ .../ftc/teamcode/pedro/procedures/Tests.java | 327 +++ .../pedro/procedures/ThreeWheelIMUTuner.java | 328 +++ .../pedro/procedures/ThreeWheelTuner.java | 290 +++ .../pedro/procedures/TwoWheelTuner.java | 404 ++++ .../ftc/teamcode/pedroPathing/Constants.java | 19 - .../ftc/teamcode/pedroPathing/Tuning.java | 1793 ----------------- build.dependencies.gradle | 6 +- 15 files changed, 3503 insertions(+), 1819 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/MecanumTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OTOSTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OctoQuadTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/PinpointTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/Tests.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelIMUTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/TwoWheelTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index ea2f869..bdb7617 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -24,11 +24,6 @@ android { } } -repositories { - maven { - url "https://repo.dairy.foundation/releases" - } -} dependencies { implementation project(':FtcRobotController') diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.java new file mode 100644 index 0000000..c4da652 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Constants.java @@ -0,0 +1,11 @@ +package org.firstinspires.ftc.teamcode.pedro; + +import com.pedropathing.follower.Follower; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Constants { + public static Follower create(HardwareMap h) { + // return new Follower(Drivetrain, Localizer, Foresight); + return null; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java new file mode 100644 index 0000000..e3eb2ea --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/Tuning.java @@ -0,0 +1,5 @@ +package org.firstinspires.ftc.teamcode.pedro; + +public class Tuning { + // Tuners go here +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java new file mode 100644 index 0000000..681f5bb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java @@ -0,0 +1,1222 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.drivetrain.DrivePowers; +import com.pedropathing.drivetrain.Drivetrain; +import com.pedropathing.localization.Localizer; +import com.pedropathing.math.Pose; +import com.pedropathing.math.Vector2D; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.pedropathing.utils.Angle; +import com.pedropathing.utils.Utils; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.util.*; +import java.util.function.Function; + +import static com.pedropathing.utils.Utils.linearFit; +import static com.pedropathing.utils.Utils.quadraticFit; + +public class ForesightTuner extends Procedure { + Function localizerFunction; + Function drivetrainFunction; + + public ForesightTuner(Function localizerFunction, Function drivetrainFunction) { + super("Foresight Tuner", "A procedure for tuning the Foresight Algorithm."); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + } + + @Override + public void run() throws InterruptedException { + Inputs distanceInput = inputs("Distance", "The distance to drive in inches for the Max Achievable Forward and Strafe Identifiers"); + Inputs.Field distance = distanceInput.d("Distance").withDefault(48.0); + awaitInputs(distanceInput); + + double forwardVelocity = runOpMode(new ForwardVelocity(localizerFunction, drivetrainFunction, distance.get())); + double strafeVelocity = runOpMode(new StrafeVelocity(localizerFunction, drivetrainFunction, distance.get())); + + Inputs velocityInput = inputs("Velocity", "The velocity to drive to in inches for the Max Achievable Forward and Strafe Deceleration Identifiers"); + Inputs.Field velocity = velocityInput.d("Velocity").withDefault(30.0); + awaitInputs(velocityInput); + + double forwardDeceleration = runOpMode(new ForwardDeceleration(localizerFunction, drivetrainFunction, velocity.get())); + double strafeDeceleration = runOpMode(new StrafeDeceleration(localizerFunction, drivetrainFunction, velocity.get())); + + List headingBraking = runOpMode(new HeadingBraking(localizerFunction, drivetrainFunction)); + double heading = runOpMode(new HeadingTuner(localizerFunction, drivetrainFunction)); + + double headingLinear = headingBraking.get(0); + double headingQuadratic = headingBraking.get(1); + + Inputs distanceBrakingInput = inputs("Distance", "The distance to drive in inches for the Forward and Strafe Braking Identifiers"); + Inputs.Field distanceBraking = distanceBrakingInput.d("Distance").withDefault(36.0); + awaitInputs(distanceBrakingInput); + + List forwardBraking = runOpMode(new ForwardBraking(localizerFunction, drivetrainFunction, headingLinear, headingQuadratic, heading, distanceBraking.get())); + List strafeBraking = runOpMode(new StrafeBraking(localizerFunction, drivetrainFunction, headingLinear, headingQuadratic, heading, distanceBraking.get())); + + double forwardLinear = forwardBraking.get(0); + double forwardQuadratic = forwardBraking.get(1); + double strafeLinear = strafeBraking.get(0); + double strafeQuadratic = strafeBraking.get(1); + + List forwardTranslational = runOpMode(new ForwardTranslational(localizerFunction, drivetrainFunction)); + List strafeTranslational = runOpMode(new StrafeTranslational(localizerFunction, drivetrainFunction)); + + double forwardTranslationalPrimary = forwardTranslational.get(0); + double forwardTranslationalSecondary = forwardTranslational.get(1); + double coast = forwardTranslational.get(2); + double brake = forwardTranslational.get(3); + + double strafeTranslationalPrimary = strafeTranslational.get(0); + double strafeTranslationalSecondary = strafeTranslational.get(1); + + result("maxAchievableForwardVelocity", forwardVelocity); + result("maxAchievableStrafeVelocity", strafeVelocity); + result("naturalForwardDeceleration", forwardDeceleration); + result("naturalStrafeDeceleration", strafeDeceleration); + result("headingBrakingLinearCoefficient", headingLinear); + result("headingBrakingQuadraticCoefficient", headingQuadratic); + result("heading kP", heading); + result("forwardBrakingLinearCoefficient", forwardLinear); + result("forwardBrakingQuadraticCoefficient", forwardQuadratic); + result("strafeBrakingLinearCoefficient", strafeLinear); + result("strafeBrakingQuadraticCoefficient", strafeQuadratic); + result("forwardTranslational Primary kP", forwardTranslationalPrimary); + result("forwardTranslational Secondary kP", forwardTranslationalSecondary); + result("strafeTranslational Primary kP", strafeTranslationalPrimary); + result("strafeTranslational Secondary kP", strafeTranslationalSecondary); + result("coast kV", coast); + result("brake kV", brake); + + code(Language.JAVA, + "public static ForesightConfig foresightConfig = new ForesightConfig(\n" + + " c -> {\n" + + " Controller primaryTranslationalForward = Controller.proportional("+forwardTranslationalPrimary+");\n" + + " Controller secondaryTranslationalForward = Controller.proportional("+forwardTranslationalSecondary+");\n" + + " Controller primaryTranslationalLateral = Controller.proportional("+strafeTranslationalPrimary+");\n" + + " Controller secondaryTranslationalLateral = Controller.proportional("+strafeTranslationalSecondary+");\n" + + "\n" + + " c.forwardTranslational.set(Controller.piecewise(secondaryTranslationalForward).put(2.5, primaryTranslationalForward));\n" + + " c.strafeTranslational.set(Controller.piecewise(secondaryTranslationalLateral).put(2.5, primaryTranslationalLateral));\n" + + "\n" + + " c.coast.set(Controller.proportionalFeedforward("+coast+"));\n" + + " c.brake.set(Controller.proportionalFeedforward("+brake+"));\n" + + "\n" + + " c.headingFeedback.set(Controller.proportional("+heading+"));\n" + + " c.headingBrakeCoefficients.set(Vector2D.cartesian("+headingLinear+", "+headingQuadratic+"));\n" + + "\n" + + " c.linearBrakeCoefficients.set(Matrix.diag("+forwardLinear+", "+strafeLinear+"));\n" + + " c.quadraticBrakeCoefficients.set(Matrix.diag("+forwardQuadratic+", "+strafeQuadratic+"));\n" + + "\n" + + " c.maxAchievableForwardVelocity.set("+forwardVelocity+");\n" + + " c.maxAchievableStrafeVelocity.set("+strafeVelocity+");\n" + + " c.naturalForwardDeceleration.set("+forwardDeceleration+");\n" + + " c.naturalStrafeDeceleration.set("+strafeDeceleration+");\n" + + " }\n" + + " );"); + } +} + +class ForwardVelocity extends TuningOpMode { + Function localizerFunction; + Function drivetrainFunction; + double distance; + private final ArrayDeque velocities = new ArrayDeque<>(); + public static double RECORD_NUMBER = 10; + + public ForwardVelocity(Function localizerFunction, Function drivetrainFunction, double distance) { + super("Max Forward Velocity", "A tuner for finding the maximum achievable forward velocity. This will drive forward for " + distance + " inches and then likely drift past that position.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + this.distance = distance; + } + + @Override + protected Double runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + boolean end = false; + + localizer.setPose(Pose.zero()); + localizer.update(); + + DrivePowers power = new DrivePowers(1,0,0); + + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + + waitForStart(); + + while (!end) { + localizer.update(); + if (Math.abs(localizer.pose().x()) > distance) { + end = true; + drivetrain.stop(); + } else { + drivetrain.drive(power, true); + double currentVelocity = Math.abs(localizer.twist().toVector2D().x()); + velocities.addLast(currentVelocity); + velocities.removeFirst(); + } + } + + drivetrain.stop(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + return average; + } +} + +class StrafeVelocity extends TuningOpMode { + Function localizerFunction; + Function drivetrainFunction; + double distance; + private final ArrayDeque velocities = new ArrayDeque<>(); + public static double RECORD_NUMBER = 10; + + public StrafeVelocity(Function localizerFunction, Function drivetrainFunction, double distance) { + super("Max Strafe Velocity", "A tuner for finding the maximum achievable strafe velocity. This will drive left for " + distance + " inches and then likely drift past that position.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + this.distance = distance; + } + + @Override + protected Double runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + boolean end = false; + + localizer.setPose(Pose.zero()); + localizer.update(); + + DrivePowers power = new DrivePowers(0,1,0); + + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + + waitForStart(); + + while (!end) { + localizer.update(); + if (Math.abs(localizer.pose().y()) > distance) { + end = true; + drivetrain.stop(); + } else { + drivetrain.drive(power, false); + double currentVelocity = Math.abs(localizer.twist().toVector2D().y()); + velocities.addLast(currentVelocity); + velocities.removeFirst(); + } + } + + drivetrain.stop(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + return average; + } +} + +class ForwardDeceleration extends TuningOpMode { + Function localizerFunction; + Function drivetrainFunction; + double velocity; + + private final ArrayList accelerations = new ArrayList<>(); + + private double previousVelocity; + private long previousTimeNano; + private boolean stopping; + + public ForwardDeceleration(Function localizerFunction, Function drivetrainFunction, double velocity) { + super("Forward Deceleration", "A tuner for finding the deceleration of the robot when moving forward. This will move forward until it reaches " + velocity + " inches per second.", false); + + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + this.velocity = velocity; + } + + @Override + protected Double runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + accelerations.clear(); + previousVelocity = 0; + previousTimeNano = 0; + stopping = false; + + localizer.setPose(Pose.zero()); + localizer.update(); + + DrivePowers power = new DrivePowers(1, 0, 0); + waitForStart(); + + drivetrain.drive(power, false); + + while (!stopping) { + localizer.update(); + double currentVelocity = localizer.twist().toVector2D().x(); + if (currentVelocity > velocity) { + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + + stopping = true; + drivetrain.stop(false); + } + } + + boolean end = false; + + while (!end) { + localizer.update(); + double currentVelocity = localizer.twist().toVector2D().x(); + long currentTimeNano = System.nanoTime(); + double dt = (currentTimeNano - previousTimeNano) / 1e9; + + if (dt > 0) { + double acceleration = (currentVelocity - previousVelocity) / dt; + accelerations.add(acceleration); + } + + previousVelocity = currentVelocity; + previousTimeNano = currentTimeNano; + + if (Math.abs(currentVelocity) <= 1) { + end = true; + } + } + + drivetrain.stop(false); + + double average = 0; + + for (double acceleration : accelerations) { + average += acceleration; + } + + if (accelerations.isEmpty()) { + return 0.0; + } + + average /= accelerations.size(); + + return Math.abs(average); + } +} + +class StrafeDeceleration extends TuningOpMode { + Function localizerFunction; + Function drivetrainFunction; + double velocity; + + private final ArrayList accelerations = new ArrayList<>(); + + private double previousVelocity; + private long previousTimeNano; + private boolean stopping; + + public StrafeDeceleration(Function localizerFunction, Function drivetrainFunction, double velocity) { + super("Strafe Deceleration", "A tuner for finding the deceleration of the robot when moving laterally. This will drive left until it reaches " + velocity + " inches per second.", false); + + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + this.velocity = velocity; + } + + @Override + protected Double runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + accelerations.clear(); + previousVelocity = 0; + previousTimeNano = 0; + stopping = false; + + localizer.setPose(Pose.zero()); + localizer.update(); + + DrivePowers power = new DrivePowers(0, 1, 0); + waitForStart(); + + drivetrain.drive(power, false); + + while (!stopping) { + localizer.update(); + double currentVelocity = localizer.twist().toVector2D().y(); + if (currentVelocity > velocity) { + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + + stopping = true; + drivetrain.stop(false); + } + } + + boolean end = false; + + while (!end) { + localizer.update(); + double currentVelocity = localizer.twist().toVector2D().y(); + long currentTimeNano = System.nanoTime(); + double dt = (currentTimeNano - previousTimeNano) / 1e9; + + if (dt > 0) { + double acceleration = (currentVelocity - previousVelocity) / dt; + accelerations.add(acceleration); + } + + previousVelocity = currentVelocity; + previousTimeNano = currentTimeNano; + + if (Math.abs(currentVelocity) <= 1) { + end = true; + } + } + + drivetrain.stop(false); + + double average = 0; + + for (double acceleration : accelerations) { + average += acceleration; + } + + if (accelerations.isEmpty()) { + return 0.0; + } + + average /= accelerations.size(); + + return Math.abs(average); + } +} + +class HeadingBraking extends TuningOpMode> { + Function localizerFunction; + Function drivetrainFunction; + + private static double[] POWERS; + public static double MAX_BRAKE_TIME = 3; //seconds, the robot shouldn't take longer than this to brake + + public static int trials = 12; + public static double maxPower = 1; + public static double minPower = 0.2; + public static double bias = 1.5; // how much it favors doing trials with higher powers + public static double brakingPower = 0.001; + + private final ElapsedTime timer = new ElapsedTime(); + + private final List velocityToBrakingDistance = new ArrayList<>(); + private State state = State.DRIVE; + private int iteration = 0; + private int direction; + private double power; + + private double startHeading; + private double measuredVelocity; + private double totalHeading; + private double previousHeading; +// private VoltageSensor voltageSensor; + + public HeadingBraking(Function localizerFunction, Function drivetrainFunction) { + super("Heading Braking", "A tuner for finding the Heading Braking Coefficients. The robot will turn back at forth at various speed levels.", false); + + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + } + + @Override + protected List runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + localizer.update(); + + List coefficients = Collections.emptyList(); + + POWERS = biasedGradient(trials, maxPower, minPower, bias); + + waitForStart(); + timer.reset(); + + while (state != State.DONE && !isStopRequested()) { + localizer.update(); + double currentHeading = localizer.pose().heading(); + totalHeading += Angle.normalizeSigned(currentHeading - previousHeading); + previousHeading = currentHeading; + + direction = (iteration % 2 == 0) ? 1 : -1; + if (iteration < POWERS.length) { + power = POWERS[iteration]; + } + +// if (state != State.DONE) { +// double voltage = voltageSensor.getVoltage(); +// double duty = state == State.BRAKE ? -brakingPower * direction: power * direction; +// double appliedVoltage = voltage * duty; +// } + + switch (state) { + case DRIVE: { + if (timer.seconds() > 2) { + startHeading = totalHeading; + measuredVelocity = Math.abs(localizer.velocity().omega); + + drivetrain.drive(new DrivePowers(0.0, 0.0, -brakingPower * direction), false); + state = State.BRAKE; + timer.reset(); + break; + } + drivetrain.drive(new DrivePowers(0.0, 0.0, power * direction), false); + break; + } + case BRAKE: { + if (Math.abs(localizer.velocity().omega) > 0.001 && timer.seconds() < MAX_BRAKE_TIME) { + drivetrain.drive(new DrivePowers(0.0, 0.0, -brakingPower * direction), false); + break; + } + + double endHeading = totalHeading; + double brakingDistance = Math.abs(endHeading - startHeading); + + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); + + iteration++; + + if (iteration >= POWERS.length) { + drivetrain.stop(); + + double[] c = quadraticFit(velocityToBrakingDistance); + coefficients = List.of(c[0], c[1]); + + state = State.DONE; + } else { + timer.reset(); + state = State.DRIVE; + } + break; + } + case DONE: {} + } + } + + + return coefficients; + } + + private enum State { + DRIVE, + BRAKE, + DONE + } + + private static double[] biasedGradient( + int count, + double max, + double min, + double bias + ) { + if (count < 2) return new double[]{ max}; + + double[] values = new double[count]; + + for (int i = 0; i < count; i++) { + double t = (double) i / (count - 1); + + double curved = 1 - Math.pow(t, bias); + + values[i] = min + curved * (max - min); + } + + return values; + } +} + +class HeadingTuner extends TuningOpMode { + Function localizerFunction; + Function drivetrainFunction; + + private static final double POWER = 0.4; + private static final double RUNTIME = 1.2; + private static final int SAMPLES = 15; + public static double ALPHA = 18.25; + + private double tau; + private double K; + private double kV; + private double kA; + private double vMax = 0; + private final List times = new ArrayList<>(); + private final List velocities = new ArrayList<>(); + private final ElapsedTime timer = new ElapsedTime(); + private boolean done = false; + private double lastTime = 0.0; + + public HeadingTuner(Function localizerFunction, Function drivetrainFunction) { + super("Heading Tuner", "A tuner for finding the Heading Tuning Coefficients using system identification. This will spin the robot in place for a couple seconds.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + } + + @Override + protected Double runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + localizer.update(); + + times.clear(); + velocities.clear(); + done = false; + vMax = 0; + lastTime = 0.0; + + waitForStart(); + timer.reset(); + lastTime = timer.seconds(); + drivetrain.drive(new DrivePowers(0.0, 0.0, POWER), false); + + while (!done && !isStopRequested()) { + double now = timer.seconds(); + double dt = now - lastTime; + if (dt <= 0) dt = 1e-6; + lastTime = now; + + localizer.update(); + + if (!done) { + times.add(timer.seconds()); + + double turnVel = Math.abs(localizer.velocity().omega); + vMax = Math.max(vMax, turnVel / POWER); + + velocities.add(turnVel); + + if (timer.seconds() >= RUNTIME) { + done = true; + systemIdentification(); + drivetrain.drive(new DrivePowers(0.0, 0.0, 0.0), false); + } + } + } + + drivetrain.drive(new DrivePowers(0.0, 0.0, 0.0), true); + return calculatekP(ALPHA); + } + + private double calculatekP(double alpha) { + kV = 1 / K; + kA = tau / K; + return tau * alpha * alpha / K; + } + + private void systemIdentification() { + int N = times.size(); + if (N < 4) { + throw new IllegalArgumentException("Failed calibration."); + } + + int start = Math.max(0, N - SAMPLES); + double samples = N - start; + double sum = 0; + for (int i = start; i < N; i++) sum += velocities.get(i); + double A = sum / samples; + this.K = A / POWER; + + List y = new ArrayList<>(); + List x = new ArrayList<>(); + for (int i = 0; i < N; i++) { + double vel = velocities.get(i) / POWER; + if (vel > 0.8 * K) continue; + if (vel < 0.1 * K) continue; + y.add(Math.log(K - vel)); + x.add(times.get(i)); + } + double[] linReg = linearFit( + x.toArray(new Double[0]), + y.toArray(new Double[0]) + ); + if (linReg[1] == 0) throw new IllegalArgumentException("Failed calibration."); + this.tau = -1.0/linReg[1]; + } +} + +class ForwardBraking extends TuningOpMode> { + Function localizerFunction; + Function drivetrainFunction; + private final double headingLinear; + private final double headingQuadratic; + private final double headingKP; + + private double[] POWERS; + public double MAX_BRAKE_TIME = 7.0; + public int trials = 5; + public double maxPower = 0.7; + public double minPower = 0.3; + public double bias = 1.5; + public double brakingPower = 0.001; + public double distance; + public double IDLE_SECONDS = 1; + + private final ElapsedTime timer = new ElapsedTime(); + private final List velocityToBrakingDistance = new ArrayList<>(); + private State state = State.DRIVE; + private int iteration = 0; + private int direction; + private double power; + private Vector2D startPosition; + private double measuredVelocity; + + public ForwardBraking(Function localizerFunction, Function drivetrainFunction, + double headingLinear, double headingQuadratic, double headingKP, double distance) { + super("Forward Braking", "A tuner for finding the Forward Braking Coefficients by driving forward and backward at various speeds. Please ensure that you have plenty of room at least " + distance + " inches ahead of the robot, but also tile space behind and laterally around the robot.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + this.headingLinear = headingLinear; + this.headingQuadratic = headingQuadratic; + this.headingKP = headingKP; + this.distance = distance; + } + + @Override + protected List runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + localizer.update(); + + POWERS = biasedGradient(trials, maxPower, minPower, bias); + + List coefficients = Collections.emptyList(); + + waitForStart(); + timer.reset(); + + drivetrain.drive(new DrivePowers(maxPower,0,0), false); + + while (state != State.DONE && !isStopRequested()) { + localizer.update(); + direction = (iteration % 2 == 0) ? 1 : -1; + if (iteration < POWERS.length) { + power = POWERS[iteration]; + } + + switch (state) { + case DRIVE: { + if ((direction > 0 && localizer.pose().x() >= distance) || (direction < 0 && localizer.pose().x() <= 12)) { + startPosition = localizer.pose().toVector2D(); + measuredVelocity = localizer.velocity().toVector2D().magnitude(); + + brake(drivetrain, localizer); + state = State.BRAKE; + timer.reset(); + break; + } + drive(drivetrain, localizer); + break; + } + case BRAKE: { + if (localizer.velocity().toVector2D().magnitude() > 0.25 && timer.seconds() < MAX_BRAKE_TIME) { + brake(drivetrain, localizer); + break; + } + + collectTrialData(localizer, drivetrain); + break; + } + case WAIT: { + drivetrain.stop(); + if (timer.seconds() > IDLE_SECONDS) state = State.DRIVE; + break; + } + case DONE: {} + } + } + + if (state == State.DONE) { + double[] c = quadraticFit(velocityToBrakingDistance); + coefficients = List.of(c[0], c[1]); + } + + return coefficients; + } + + private double getHeadingPower(Localizer localizer) { + double angularVel = localizer.velocity().omega; + double brakeDist = headingLinear * angularVel + + headingQuadratic * angularVel * angularVel * Math.signum(angularVel); + double headingError = Angle.normalizeSigned(-localizer.pose().heading()); + double error = headingError - brakeDist; + return Utils.clamp(headingKP * error, -0.3, 1.0) / 2; + } + + + private void drive(Drivetrain drivetrain, Localizer localizer) { + drivetrain.drive(new DrivePowers(power * direction, 0.0, getHeadingPower(localizer)), false); + } + + private void brake(Drivetrain drivetrain, Localizer localizer) { + double headingPower = getHeadingPower(localizer); + double brake = -brakingPower * direction; + double minBrake = Math.abs(headingPower) + 0.001; + + if (direction > 0) { + brake = Math.min(brake, -minBrake); + } else { + brake = Math.max(brake, minBrake); + } + + drivetrain.drive(new DrivePowers(brake, 0, headingPower), false); + } + + private void collectTrialData(Localizer localizer, Drivetrain drivetrain) { + Vector2D endPosition = localizer.pose().toVector2D(); + double brakingDistance = endPosition.minus(startPosition).magnitude(); + + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); + + iteration++; + + if (iteration >= POWERS.length) { + drivetrain.stop(); + state = State.DONE; + } else { + state = State.WAIT; + timer.reset(); + } + } + + private enum State { + DRIVE, + BRAKE, + WAIT, + DONE + } + + private static double[] biasedGradient(int count, double max, double min, double bias) { + if (count < 2) return new double[]{max}; + double[] values = new double[count]; + for (int i = 0; i < count; i++) { + double t = (double) i / (count - 1); + double curved = 1 - Math.pow(t, bias); + values[i] = min + curved * (max - min); + } + return values; + } +} + +class StrafeBraking extends TuningOpMode> { + Function localizerFunction; + Function drivetrainFunction; + private final double headingLinear; + private final double headingQuadratic; + private final double headingKP; + + private double[] POWERS; + public double MAX_BRAKE_TIME = 7.0; + public int trials = 5; + public double maxPower = 1; + public double minPower = 0.3; + public double bias = 1.5; + public double brakingPower = 0.001; + public double distance; + public double IDLE_SECONDS = 1; + + private final ElapsedTime timer = new ElapsedTime(); + private final List velocityToBrakingDistance = new ArrayList<>(); + private State state = State.DRIVE; + private int iteration = 0; + private int direction; + private double power; + private Vector2D startPosition; + private double measuredVelocity; + + public StrafeBraking(Function localizerFunction, Function drivetrainFunction, + double headingLinear, double headingQuadratic, double headingKP, double distance) { + super("Strafe Braking", "A tuner for finding the Strafe Braking Coefficients by strafing left and right at various speeds. Please ensure that you have plenty of room at least " + distance + " inches to the left and right of the robot and space in front and behind the robot.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + this.headingLinear = headingLinear; + this.headingQuadratic = headingQuadratic; + this.headingKP = headingKP; + this.distance = distance; + } + + @Override + protected List runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + localizer.update(); + + POWERS = biasedGradient(trials, maxPower, minPower, bias); + + List coefficients = Collections.emptyList(); + + waitForStart(); + timer.reset(); + + drivetrain.drive(new DrivePowers(0,maxPower,0), false); + + while (state != State.DONE && !isStopRequested()) { + localizer.update(); + direction = (iteration % 2 == 0) ? 1 : -1; + if (iteration < POWERS.length) { + power = POWERS[iteration]; + } + + switch (state) { + case DRIVE: { + if ((direction > 0 && localizer.pose().y() > distance) || + (direction < 0 && localizer.pose().y() <= 6)) { + startPosition = localizer.pose().toVector2D(); + measuredVelocity = localizer.velocity().toVector2D().magnitude(); + + brake(drivetrain, localizer); + state = State.BRAKE; + timer.reset(); + break; + } + drive(drivetrain, localizer); + break; + } + case BRAKE: { + if (localizer.velocity().toVector2D().magnitude() > 0.25 && timer.seconds() < MAX_BRAKE_TIME) { + brake(drivetrain, localizer); + break; + } + + collectTrialData(localizer, drivetrain); + break; + } + case WAIT: { + drivetrain.stop(); + if (timer.seconds() > IDLE_SECONDS) state = State.DRIVE; + break; + } + case DONE: {} + } + } + + if (state == State.DONE) { + double[] c = quadraticFit(velocityToBrakingDistance); + coefficients = List.of(c[0], c[1]); + } + + return coefficients; + } + + private double getHeadingPower(Localizer localizer) { + double angularVel = localizer.velocity().omega; + double brakeDist = headingLinear * angularVel + + headingQuadratic * angularVel * angularVel * Math.signum(angularVel); + double headingError = Angle.normalizeSigned(-localizer.pose().heading()); + double error = headingError - brakeDist; + return Utils.clamp(headingKP * error, -0.3, 1.0) / 2; + } + + private void drive(Drivetrain drivetrain, Localizer localizer) { + drivetrain.drive(new DrivePowers(0.0, power * direction, getHeadingPower(localizer)), false); + } + + private void brake(Drivetrain drivetrain, Localizer localizer) { + double headingPower = getHeadingPower(localizer); + double brake = -brakingPower * direction; + double minBrake = Math.abs(headingPower) + 0.001; + + if (direction > 0) { + brake = Math.min(brake, -minBrake); + } else { + brake = Math.max(brake, minBrake); + } + + drivetrain.drive(new DrivePowers(0, brake, headingPower), false); + } + + private void collectTrialData(Localizer localizer, Drivetrain drivetrain) { + Vector2D endPosition = localizer.pose().toVector2D(); + double brakingDistance = endPosition.minus(startPosition).magnitude(); + + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); + + iteration++; + + if (iteration >= POWERS.length) { + drivetrain.stop(); + state = State.DONE; + } else { + state = State.WAIT; + timer.reset(); + } + } + + private enum State { + DRIVE, + BRAKE, + WAIT, + DONE + } + + private static double[] biasedGradient(int count, double max, double min, double bias) { + if (count < 2) return new double[]{max}; + double[] values = new double[count]; + for (int i = 0; i < count; i++) { + double t = (double) i / (count - 1); + double curved = 1 - Math.pow(t, bias); + values[i] = min + curved * (max - min); + } + return values; + } +} + +class ForwardTranslational extends TuningOpMode> { + Function localizerFunction; + Function drivetrainFunction; + + public static double ALPHA_LARGE = 10.2; + public static double ALPHA_SMALL = 6.2; + private final double VEL_AGGRESSIVENESS = 0.85; + private final double POWER = 0.4; + private final double RUNTIME = 1.2; + private final int SAMPLES = 15; + + private double tau; + private double K; + private double kV; + private double kA; + private double vMax = 0; + private final List times = new ArrayList<>(); + private final List velocities = new ArrayList<>(); + private final ElapsedTime timer = new ElapsedTime(); + private boolean done = false; + private double lastTime = 0.0; + + public ForwardTranslational(Function localizerFunction, Function drivetrainFunction) { + super("Forward Translational", "A tuner for finding the Forward Translational kP coefficients using system identification. This will move around 12-24 inches in front of the robot and then stop.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + } + + @Override + protected List runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + localizer.update(); + + times.clear(); + velocities.clear(); + done = false; + vMax = 0; + lastTime = 0.0; + + waitForStart(); + timer.reset(); + lastTime = timer.seconds(); + drivetrain.drive(new DrivePowers(POWER, 0.0, 0.0), false); + + while (!done && !isStopRequested()) { + double now = timer.seconds(); + double dt = now - lastTime; + if (dt <= 0) dt = 1e-6; + lastTime = now; + + localizer.update(); + + if (!done) { + times.add(timer.seconds()); + + double forwardVelocity = Math.abs(localizer.twist().toVector2D().x()); + vMax = Math.max(vMax, forwardVelocity / POWER); + + velocities.add(forwardVelocity); + + if (timer.seconds() >= RUNTIME) { + done = true; + systemIdentification(); + drivetrain.drive(new DrivePowers(0.0, 0.0, 0.0), false); + } + } + } + + drivetrain.drive(new DrivePowers(0.0, 0.0, 0.0), true); + + double kP_large = calculatekP(ALPHA_LARGE); + double kP_small = calculatekP(ALPHA_SMALL); + + // kP_large, kP_small, coast kV, and brake kV (scaled by aggressiveness factor) + return List.of(kP_large, kP_small, kV, kV * VEL_AGGRESSIVENESS); + } + + private double calculatekP(double alpha) { + kV = 1 / K; + kA = tau / K; + return tau * alpha * alpha / K; + } + + private void systemIdentification() { + int N = times.size(); + if (N < 4) { + throw new IllegalArgumentException("Failed calibration."); + } + + int start = Math.max(0, N - SAMPLES); + double samples = N - start; + double sum = 0; + for (int i = start; i < N; i++) sum += velocities.get(i); + double A = sum / samples; + this.K = A / POWER; + + List y = new ArrayList<>(); + List x = new ArrayList<>(); + for (int i = 0; i < N; i++) { + double vel = velocities.get(i) / POWER; + if (vel > 0.8 * K) continue; + if (vel < 0.1 * K) continue; + y.add(Math.log(K - vel)); + x.add(times.get(i)); + } + double[] linReg = linearFit( + x.toArray(new Double[0]), + y.toArray(new Double[0]) + ); + if (linReg[1] == 0) throw new IllegalArgumentException("Failed calibration."); + this.tau = -1.0/linReg[1]; + } +} + +class StrafeTranslational extends TuningOpMode> { + Function localizerFunction; + Function drivetrainFunction; + public static double ALPHA_LARGE = 10.2; + public static double ALPHA_SMALL = 6.2; + private final double POWER = 0.4; + private final double RUNTIME = 1.2; + private final int SAMPLES = 15; + + private double tau; + private double K; + private double kV; + private double kA; + private double vMax = 0; + private final List times = new ArrayList<>(); + private final List velocities = new ArrayList<>(); + private final ElapsedTime timer = new ElapsedTime(); + private boolean done = false; + private double lastTime = 0.0; + + public StrafeTranslational(Function localizerFunction, Function drivetrainFunction) { + super("Strafe Translational", "A tuner for finding the Strafe Translational kP coefficients using system identification. This will move around 12-24 inches to the left and right of the robot and then stop.", false); + this.localizerFunction = localizerFunction; + this.drivetrainFunction = drivetrainFunction; + } + + @Override + protected List runTuningOpMode() { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + localizer.update(); + + times.clear(); + velocities.clear(); + done = false; + vMax = 0; + lastTime = 0.0; + + waitForStart(); + timer.reset(); + lastTime = timer.seconds(); + drivetrain.drive(new DrivePowers(0.0, POWER, 0.0), false); + + while (!done && !isStopRequested()) { + double now = timer.seconds(); + double dt = now - lastTime; + if (dt <= 0) dt = 1e-6; + lastTime = now; + + localizer.update(); + + if (!done) { + times.add(timer.seconds()); + + double lateralVelocity = Math.abs(localizer.twist().toVector2D().y()); + vMax = Math.max(vMax, lateralVelocity / POWER); + + velocities.add(lateralVelocity); + + if (timer.seconds() >= RUNTIME) { + done = true; + systemIdentification(); + drivetrain.drive(new DrivePowers(0.0, 0.0, 0.0), false); + } + } + } + + drivetrain.drive(new DrivePowers(0.0, 0.0, 0.0), true); + + double kP_large = calculatekP(ALPHA_LARGE); + double kP_small = calculatekP(ALPHA_SMALL); + + return List.of(kP_large, kP_small); + } + + private double calculatekP(double alpha) { + kV = 1 / K; + kA = tau / K; + return tau * alpha * alpha / K; + } + + private void systemIdentification() { + int N = times.size(); + if (N < 4) { + throw new IllegalArgumentException("Failed calibration."); + } + + int start = Math.max(0, N - SAMPLES); + double samples = N - start; + double sum = 0; + for (int i = start; i < N; i++) sum += velocities.get(i); + double A = sum / samples; + this.K = A / POWER; + + List y = new ArrayList<>(); + List x = new ArrayList<>(); + for (int i = 0; i < N; i++) { + double vel = velocities.get(i) / POWER; + if (vel > 0.8 * K) continue; + if (vel < 0.1 * K) continue; + y.add(Math.log(K - vel)); + x.add(times.get(i)); + } + double[] linReg = linearFit( + x.toArray(new Double[0]), + y.toArray(new Double[0]) + ); + if (linReg[1] == 0) throw new IllegalArgumentException("Failed calibration."); + this.tau = -1.0/linReg[1]; + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/MecanumTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/MecanumTuner.java new file mode 100644 index 0000000..b4c0ff7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/MecanumTuner.java @@ -0,0 +1,96 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.tuning.autotune.*; +import com.pedropathing.tuning.autotune.Display.FourWheelBot.Wheel; +import com.qualcomm.robotcore.hardware.DcMotor; + +enum Direction { + @DisplayName("Forward") FORWARD, + @DisplayName("Reversed") REVERSE +} + +public class MecanumTuner extends Procedure { + public MecanumTuner() { + super("Mecanum Tuner", "A procedure to find the directions of mecanum wheels."); + } + + @Override + public void run() throws InterruptedException { + Inputs motorNames = inputs("Mecanum Motor Names", "Enter the names in HardwareMap of your drivetrain motors."); + Inputs.Field frontLeftName = motorNames.s("Front Left Name"); + Inputs.Field frontRightName = motorNames.s("Front Right Name"); + Inputs.Field backLeftName = motorNames.s("Back Left Name"); + Inputs.Field backRightName = motorNames.s("Back Right Name"); + awaitInputs(motorNames); + + confirmation("Motor Directions", "Each drivetrain motor will spin, one at a time. After each one, you will enter whether it spun forward or reversed. You may use the interactive diagram to see which wheel should be spinning and which direction is forward."); + + Direction frontLeftDirection = testMotor(Wheel.FRONT_LEFT, "Front Left", frontLeftName.get()); + Direction frontRightDirection = testMotor(Wheel.FRONT_RIGHT, "Front Right", frontRightName.get()); + Direction backLeftDirection = testMotor(Wheel.BACK_LEFT, "Back Left", backLeftName.get()); + Direction backRightDirection = testMotor(Wheel.BACK_RIGHT, "Back Right", backRightName.get()); + + result("frontLeftName", frontLeftName.get()); + result("frontRightName", frontRightName.get()); + result("backLeftName", backLeftName.get()); + result("backRightName", backRightName.get()); + result("frontLeftDirection", frontLeftDirection); + result("frontRightDirection", frontRightDirection); + result("backLeftDirection", backLeftDirection); + result("backRightDirection", backRightDirection); + + code(Language.JAVA, "public static MecanumConfig drivetrainConfig = new MecanumConfig(c -> {\n" + + " c.frontLeftName.set(\"" + frontLeftName.get() + "\");\n" + + " c.frontRightName.set(\"" + frontRightName.get() + "\");\n" + + " c.backLeftName.set(\"" + backLeftName.get() + "\");\n" + + " c.backRightName.set(\"" + backRightName.get() + "\");\n" + + " c.frontLeftDirection.set(DcMotorSimple.Direction." + frontLeftDirection + ");\n" + + " c.frontRightDirection.set(DcMotorSimple.Direction." + frontRightDirection + ");\n" + + " c.backLeftDirection.set(DcMotorSimple.Direction." + backLeftDirection + ");\n" + + " c.backRightDirection.set(DcMotorSimple.Direction." + backRightDirection + ");\n" + + "});"); + } + + private Direction testMotor(Wheel wheel, String displayName, String hardwareName) throws InterruptedException { + final boolean[] correctMotor = new boolean[1]; + final Direction[] direction = new Direction[1]; + + withDisplay(Display.fourWheelBot(wheel, false), () -> { + runOpMode(new SpinMotor(displayName, hardwareName)); + + Inputs inputs = inputs(displayName, "Determine the " + displayName.toLowerCase() + " motor direction."); + Inputs.Field correctMotorField = inputs.b("Did the " + displayName.toLowerCase() + " motor spin?").withDefault(true); + Inputs.Field directionField = inputs.e("Which way did the motor spin?", Direction.class); + awaitInputs(inputs); + + correctMotor[0] = correctMotorField.get(); + direction[0] = directionField.get(); + }); + + if (!correctMotor[0]) + abort("The wrong motor spun. Check that your motors are plugged into the correct ports, and that they are configured correctly. Then, try again."); + + return direction[0]; + } +} + +class SpinMotor extends TuningOpMode { + private final String name; + + public SpinMotor(String displayName, String hardwareName) { + super(displayName, "The " + displayName.toLowerCase() + " motor will spin. The interactive diagram shows which way is forward. Click stop when you know if it is spinning forward or reversed.", true); + this.name = hardwareName; + } + + @SuppressWarnings("StatementWithEmptyBody") + @Override + protected Void runTuningOpMode() { + DcMotor motor = hardwareMap.dcMotor.get(name); + waitForStart(); + motor.setPower(0.5); + while (opModeIsActive()) { + } + motor.setPower(0); + return null; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OTOSTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OTOSTuner.java new file mode 100644 index 0000000..08fcc35 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OTOSTuner.java @@ -0,0 +1,210 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.math.Pose; +import com.pedropathing.revhub.localizers.OTOSConfig; +import com.pedropathing.revhub.localizers.OTOSLocalizer; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.pedropathing.utils.Angle; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import java.util.List; + +public class OTOSTuner extends Procedure { + public OTOSTuner() { + super("OTOS Tuner", "A procedure for tuning the OTOS localizer."); + } + + @Override + public void run() throws InterruptedException { + Inputs inputs = inputs("Setup", "Set OTOS HardwareMap Name"); + Inputs.Field name = inputs.s("HardwareMap Name").withDefault("otos"); + awaitInputs(inputs); + + Inputs scalar = inputs( + "Scalar Identification", + "Set the distance you will push your robot forward in inches and the number of full rotations for the angular test" + ); + Inputs.Field distance = scalar.d("Distance to push robot").withDefault(48.0); + Inputs.Field turns = scalar.i("Full rotations").withDefault(10); + awaitInputs(scalar); + + if (!(distance.get() > 0.0)) { + abort("Enter a positive push distance in inches."); + return; + } + if (turns.get() <= 0) { + abort("Enter a positive number of full rotations."); + return; + } + + Double angularScalar = runOpMode(new OTOSAngularScalar(name.get(), turns.get())); + Double linearScalar = runOpMode(new OTOSLinearScalar(name.get(), distance.get())); + + List offsets = runOpMode(new OTOSOffsets(name.get(), linearScalar, angularScalar)); + if (offsets == null) { + abort("Offset stage ended without a saved pose. Rotate the robot 180 degrees about the robot center, then press Stop."); + return; + } + + result("name", name.get()); + result("linearScalar", linearScalar); + result("angularScalar", angularScalar); + result("xOffset", offsets.get(0)); + result("yOffset", offsets.get(1)); + + code(Language.JAVA,"public static OTOSConfig localizerConfig = new OTOSConfig(c -> {\n" + + " c.name.set(\"" + name.get() + "\");\n" + + " c.linearScalar.set(" + linearScalar + ");\n" + + " c.angularScalar.set(" + angularScalar + ");\n" + + " c.offset.set(new Pose(" + offsets.get(0) + ", " + offsets.get(1) + "));\n" + + " c.linearUnit.set(DistanceUnit.INCH);\n" + + "});"); + } + +} + +class OTOSLinearScalar extends TuningOpMode { + String name; + double distance; + + public OTOSLinearScalar(String name, double distance) { + super("Linear Scalar Identification", + "Determines the linear scalar for the OTOS localizer. \n" + + "Push your robot forward " + distance + " inches, stop moving, then press Stop", + true); + this.name = name; + this.distance = distance; + } + + @Override + protected Double runTuningOpMode() { + OTOSConfig config = new OTOSConfig(c -> { + c.name.set(name); + c.linearUnit.set(DistanceUnit.INCH); + c.linearScalar.set(1.0); + c.angularScalar.set(1.0); + c.offset.set(Pose.zero()); + }); + OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config); + localizer.setPose(Pose.zero()); + localizer.update(); + + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + + + } + + if (position == null || Math.abs(position.x()) <= 1e-9) { + return null; + } + + return Math.abs(distance / position.x()); + } +} + +class OTOSAngularScalar extends TuningOpMode { + String name; + int turns; + double targetRadians; + + public OTOSAngularScalar(String name, int turns) { + super("Angular Scalar Identification", + "Determines the angular scalar for the OTOS localizer. \n" + + "Spin your robot " + turns + " full rotations, stop moving, then press Stop", + true); + this.name = name; + this.turns = turns; + this.targetRadians = turns * 2.0 * Math.PI; + } + + @Override + protected Double runTuningOpMode() { + OTOSConfig config = new OTOSConfig(c -> { + c.name.set(name); + c.linearUnit.set(DistanceUnit.INCH); + c.linearScalar.set(1.0); + c.angularScalar.set(1.0); + c.offset.set(Pose.zero()); + }); + OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config); + localizer.setPose(Pose.zero()); + localizer.update(); + + waitForStart(); + + localizer.update(); + double prevHeading = localizer.pose().heading(); + double totalHeading = 0.0; + + while (!isStopRequested()) { + localizer.update(); + + Pose position = localizer.pose(); + double currentHeading = position.heading(); + totalHeading += Angle.normalizeSigned(currentHeading - prevHeading); + prevHeading = currentHeading; + + + } + + if (Math.abs(totalHeading) <= 1e-9) { + return null; + } + + return Math.abs(targetRadians / totalHeading); + } +} + +class OTOSOffsets extends TuningOpMode> { + String name; + double linearScalar, angularScalar; + + public OTOSOffsets(String name, double linearScalar, double angularScalar) { + super("OTOS Offset Identification", + "Automatically identifies the X/Y offset for your OTOS localizer. \n" + + "Rotate the robot 180 degrees counterclockwise about the robot center without translating it, stop moving, then press Stop", + true); + this.name = name; + this.linearScalar = linearScalar; + this.angularScalar = angularScalar; + } + + @Override + protected List runTuningOpMode() { + OTOSConfig config = new OTOSConfig(c -> { + c.name.set(name); + c.linearUnit.set(DistanceUnit.INCH); + c.linearScalar.set(linearScalar); + c.angularScalar.set(angularScalar); + c.offset.set(Pose.zero()); + }); + OTOSLocalizer localizer = new OTOSLocalizer(hardwareMap, config); + localizer.setPose(Pose.zero()); + localizer.update(); + + Pose position = null; + + waitForStart(); + + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + telemetry.addData("heading", localizer.pose().heading()); + telemetry.update(); + } + + if (position == null) { + return null; + } + + return List.of(-position.x() / 2.0, -position.y() / 2.0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OctoQuadTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OctoQuadTuner.java new file mode 100644 index 0000000..1031da2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/OctoQuadTuner.java @@ -0,0 +1,350 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.math.Pose; +import com.pedropathing.revhub.localizers.OctoQuadConfig; +import com.pedropathing.revhub.localizers.OctoQuadLocalizer; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.pedropathing.utils.Angle; +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import java.util.List; + +public class OctoQuadTuner extends Procedure { + enum PodType { + SWING_ARM, + FOUR_BAR, + CUSTOM + } + + public static double SWING_ARM = 336.877962768; + public static double FOUR_BAR = 505.316944406; + + public OctoQuadTuner() { + super("OctoQuad Tuner", "A procedure for tuning the OctoQuad localizer."); + } + + @Override + public void run() throws InterruptedException { + Inputs inputs = inputs("Setup", "Set OctoQuad HardwareMap Name and Odometry Pod Type"); + Inputs.Field octoquadName = inputs.s("HardwareMap Name").withDefault("octoquad"); + Inputs.Field xPort = inputs.i("Forward Pod Port").withDefault(0); + Inputs.Field yPort = inputs.i("Strafe Pod Port").withDefault(1); + Inputs.Field podType = inputs.e("Odometry Pod Type", OctoQuadTuner.PodType.class).withDefault(OctoQuadTuner.PodType.FOUR_BAR); + Inputs.Field recoveryMode = inputs.e("Recovery Mode", OctoQuad.I2cRecoveryMode.class).withDefault(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR); + awaitInputs(inputs); + + double customPodScalar = 0; + + Inputs inputsHeadingScalar = inputs("Custom Scalar Identification Turns", "Set the number of times you will turn your robot."); + Inputs.Field turns = inputsHeadingScalar.i("Turns").withDefault(10); + awaitInputs(inputsHeadingScalar); + double headingScalar = runOpMode(new OctoQuadHeadingScalar(octoquadName.get(), turns.get(), xPort.get(), yPort.get())); + + if (podType.get() == PodType.CUSTOM) { + Inputs inputsCustom = inputs("Custom Scalar Identification Push Distance", "Set the distance you will push your robot forward in inches"); + Inputs.Field distance = inputsCustom.d("Distance").withDefault(48.0); + awaitInputs(inputsCustom); + customPodScalar = runOpMode(new OctoQuadCustomPodScalar(distance.get(), octoquadName.get(), xPort.get(), yPort.get())); + } + + boolean forwardPodReversed = runOpMode(new OctoQuadForwardDirection(octoquadName.get(), podType.get(), customPodScalar, headingScalar, xPort.get(), yPort.get())); + boolean strafePodReversed = runOpMode(new OctoQuadStrafeDirection(octoquadName.get(), podType.get(), customPodScalar, headingScalar, xPort.get(), yPort.get())); + + List offsets = runOpMode(new OctoQuadOffsets(octoquadName.get(), podType.get(), customPodScalar, forwardPodReversed, strafePodReversed, headingScalar, xPort.get(), yPort.get())); + + result("name", octoquadName.get()); + result("headingScalar", headingScalar); + + if (podType.get() == PodType.CUSTOM) { + result("podType", "Custom"); + result("ticksPerUnit", customPodScalar); + } else { + result("podType", podType.get() == PodType.SWING_ARM ? SWING_ARM : FOUR_BAR); + } + + result("xPodDirection", forwardPodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + result("yPodDirection", strafePodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + result("xPodOffset", offsets.get(0)); + result("yPodOffset", offsets.get(1)); + + code(Language.JAVA,"public static OctoQuadConfig localizerConfig = new OctoQuadConfig(c -> {\n" + + " c.name.set(\"" + octoquadName.get() + "\");\n" + + " c.xPodPort.set(" + xPort.get() + ");\n" + + " c.yPodPort.set(" + yPort.get() + ");\n" + + (podType.get() == PodType.CUSTOM ? " c.ticksPerUnit.set(" + customPodScalar + ");\n" : " c.ticksPerUnit.set(" + (podType.get() == OctoQuadTuner.PodType.SWING_ARM ? SWING_ARM : FOUR_BAR) + ");\n") + + " c.xPodOffset.set(" + offsets.get(0) + ");\n" + + " c.yPodOffset.set(" + offsets.get(1) + ");\n" + + " c.xPodDirection.set(" + (forwardPodReversed ? "OctoQuad.EncoderDirection.REVERSE" : "OctoQuad.EncoderDirection.FORWARD") + ");\n" + + " c.yPodDirection.set(" + (strafePodReversed ? "OctoQuad.EncoderDirection.REVERSE" : "OctoQuad.EncoderDirection.FORWARD") + ");\n" + + " c.globalDistanceUnit.set(DistanceUnit.INCH);\n" + + " c.offsetUnits.set(DistanceUnit.INCH);\n" + + " c.i2cRecoveryMode.set(OctoQuad.I2cRecoveryMode." + recoveryMode.get() + ");\n" + + " c.headingScalar.set(" + headingScalar + ");\n" + + "});"); + } +} + +class OctoQuadHeadingScalar extends TuningOpMode { + String name; + int turns; + double totalHeading = 0; + double prevHeading = 0; + int xPodPort; + int yPodPort; + + public OctoQuadHeadingScalar(String name, int turns, int xPodPort, int yPodPort) { + super("Heading Scalar Identification", + "Determines the scalar for the custom pods of the OctoQuad localizer. \n" + + "Turn your robot " + turns * 360 + " degrees exactly ("+ turns + " times) exactly and then stop the OpMode.", + true); + this.name = name; + this.turns = turns; + this.xPodPort = xPodPort; + this.yPodPort = yPodPort; + } + + @Override + protected Double runTuningOpMode() throws InterruptedException { + OctoQuadConfig config = new OctoQuadConfig(c -> { + c.name.set(name); + c.xPodPort.set(xPodPort); + c.yPodPort.set(yPodPort); + c.ticksPerUnit.set(1.0); + c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + }); + + OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + localizer.update(); + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + + if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y() || localizer.pose().heading() != Pose.zero().heading()) { + double currentHeading = localizer.pose().heading(); + totalHeading += Angle.normalizeSigned(currentHeading - prevHeading); + prevHeading = currentHeading; + } + } + return Math.abs((turns * Math.PI * 2 / totalHeading)); + } +} + +class OctoQuadCustomPodScalar extends TuningOpMode { + String name; + double distance; + int xPodPort, yPodPort; + + public OctoQuadCustomPodScalar(Double distance, String name, int xPodPort, int yPodPort) { + super("Custom Scalar Identification", + "Determines the scalar for the custom pods of the OctoQuad localizer. \n" + + "Push your robot forward " + distance + " inches exactly and then stop the Opmode", + true); + this.name = name; + this.distance = distance; + this.xPodPort = xPodPort; + this.yPodPort = yPodPort; + } + + @Override + protected Double runTuningOpMode() { + OctoQuadConfig config = new OctoQuadConfig(c -> { + c.name.set(name); + c.xPodPort.set(xPodPort); + c.yPodPort.set(yPodPort); + c.ticksPerUnit.set(1.0); + c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + }); + OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + waitForStart(); + + double startTicks = localizer.octoQuad.readAllEncoderData().positions[xPodPort]; + double lastLastTicksPerInch = 0; + double lastTicksPerInch = 0; + + while (!isStopRequested()) { + localizer.update(); + + double pos = localizer.octoQuad.readAllEncoderData().positions[xPodPort]; + lastLastTicksPerInch = lastTicksPerInch; + lastTicksPerInch = Math.abs(pos - startTicks) / distance; + } + + return lastLastTicksPerInch; + } +} + +class OctoQuadForwardDirection extends TuningOpMode { + String name; + OctoQuadTuner.PodType podType; + double customPodScalar; + double headingScalar; + int xPodPort, yPodPort; + + public OctoQuadForwardDirection(String name, OctoQuadTuner.PodType podType, double customPodScalar, double headingScalar, + int xPodPort, int yPodPort) { + super("Forward Direction Identification", + "Determines if your forward pod needs to be reversed. \n" + + "Push your robot forward and then stop the Opmode", + true); + this.name = name; + this.podType = podType; + this.customPodScalar = customPodScalar; + this.headingScalar = headingScalar; + this.xPodPort = xPodPort; + this.yPodPort = yPodPort; + } + + @Override + protected Boolean runTuningOpMode() { + OctoQuadConfig config = new OctoQuadConfig(c -> { + c.name.set(name); + c.xPodPort.set(xPodPort); + c.yPodPort.set(yPodPort); + c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + c.headingScalar.set(headingScalar); + if (podType == OctoQuadTuner.PodType.CUSTOM) { + c.ticksPerUnit.set(customPodScalar); + } else { + c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR); + } + }); + OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + } + + return localizer.pose().x() < 0; + } +} + +class OctoQuadStrafeDirection extends TuningOpMode { + String name; + OctoQuadTuner.PodType podType; + double customPodScalar; + double headingScalar; + int xPodPort, yPodPort; + + public OctoQuadStrafeDirection(String name, OctoQuadTuner.PodType podType, double customPodScalar, double headingScalar, + int xPodPort, int yPodPort) { + super("Strafe Direction Identification", + "Determines if your strafe pod needs to be reversed. \n" + + "Push your robot to the left and then stop the Opmode", + true); + this.name = name; + this.podType = podType; + this.customPodScalar = customPodScalar; + this.headingScalar = headingScalar; + this.xPodPort = xPodPort; + this.yPodPort = yPodPort; + } + + @Override + protected Boolean runTuningOpMode() { + OctoQuadConfig config = new OctoQuadConfig(c -> { + c.name.set(name); + c.xPodPort.set(xPodPort); + c.yPodPort.set(yPodPort); + c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.yPodDirection.set(OctoQuad.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + c.headingScalar.set(headingScalar); + + if (podType == OctoQuadTuner.PodType.CUSTOM) { + c.ticksPerUnit.set(customPodScalar); + } else { + c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR); + } + }); + OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + } + + return localizer.pose().y() < 0; + } +} + +class OctoQuadOffsets extends TuningOpMode> { + String name; + OctoQuadTuner.PodType podType; + double customPodScalar; + boolean forwardPodReversed, strafePodReversed; + double headingScalar; + Pose previous; + int xPodPort, yPodPort; + + public OctoQuadOffsets(String name, OctoQuadTuner.PodType podType, double customPodScalar, Boolean forwardPodReversed, Boolean strafePodReversed, double headingScalar, + int xPodPort, int yPodPort) { + super("OctoQuadOffsets Identification", + "Automatically identifies the offsets for your OctoQuad localizer. \n" + + "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode", + true); + this.name = name; + this.podType = podType; + this.customPodScalar = customPodScalar; + this.forwardPodReversed = forwardPodReversed; + this.strafePodReversed = strafePodReversed; + this.headingScalar = headingScalar; + this.xPodPort = xPodPort; + this.yPodPort = yPodPort; + } + + @Override + protected List runTuningOpMode() { + OctoQuadConfig config = new OctoQuadConfig(c -> { + c.name.set(name); + c.xPodPort.set(xPodPort); + c.yPodPort.set(yPodPort); + c.xPodDirection.set(forwardPodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + c.yPodDirection.set(strafePodReversed ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + if (podType.equals(OctoQuadTuner.PodType.CUSTOM)) { + c.encoderResolutionUnit.set(DistanceUnit.INCH); + c.ticksPerUnit.set(customPodScalar); + } else { + c.encoderResolutionUnit.set(DistanceUnit.INCH); + c.ticksPerUnit.set(podType == OctoQuadTuner.PodType.SWING_ARM ? OctoQuadTuner.SWING_ARM : OctoQuadTuner.FOUR_BAR); + } + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + c.headingScalar.set(headingScalar); + }); + OctoQuadLocalizer localizer = new OctoQuadLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + localizer.update(); + + waitForStart(); + + while (!isStopRequested()) { + previous = localizer.pose(); + localizer.update(); + telemetry.addData("heading", localizer.pose().heading()); + telemetry.update(); + } + + if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) { + previous = localizer.pose(); + } + + return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/PinpointTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/PinpointTuner.java new file mode 100644 index 0000000..ad9e7fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/PinpointTuner.java @@ -0,0 +1,256 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.math.Pose; +import com.pedropathing.revhub.localizers.PinpointConfig; +import com.pedropathing.revhub.localizers.PinpointLocalizer; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import java.util.*; + +public class PinpointTuner extends Procedure { + enum PodType { + SWING_ARM, + FOUR_BAR, + CUSTOM + } + public PinpointTuner() { + super("Pinpoint Tuner", "A procedure for tuning the Pinpoint localizer."); + } + + @Override + public void run() throws InterruptedException { + Inputs inputs = inputs("Setup", "Set Pinpoint HardwareMap Name and Odometry Pod Type"); + Inputs.Field pinpointName = inputs.s("HardwareMap Name").withDefault("pinpoint"); + Inputs.Field podType = inputs.e("Odometry Pod Type", PodType.class).withDefault(PodType.FOUR_BAR); + awaitInputs(inputs); + + OptionalDouble customPodScalar = OptionalDouble.empty(); + + if (podType.get() == PodType.CUSTOM) { + Inputs inputsCustom = inputs("Custom Scalar Identification Push Distance", "Set the distance you will push your robot forward in inches"); + Inputs.Field distance = inputsCustom.d("Distance").withDefault(48.0); + awaitInputs(inputsCustom); + customPodScalar = OptionalDouble.of(runOpMode(new PinpointCustomPodScalar(distance.get(), pinpointName.get()))); + } + + boolean forwardPodReversed = runOpMode(new PinpointForwardDirection(pinpointName.get(), podType.get(), customPodScalar)); + boolean strafePodReversed = runOpMode(new PinpointStrafeDirection(pinpointName.get(), podType.get(), customPodScalar)); + + List offsets = runOpMode(new PinpointOffsets(pinpointName.get(), podType.get(), customPodScalar, forwardPodReversed, strafePodReversed)); + + result("name", pinpointName.get()); + + if (customPodScalar.isPresent()) { + result("podType", "Custom"); + result("ticksPerUnit", customPodScalar.getAsDouble()); + } else { + result("podType", podType.get() == PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + } + + result("xPodDirection", forwardPodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD); + result("yPodDirection", strafePodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD); + result("xPodOffset", offsets.get(0)); + result("yPodOffset", offsets.get(1)); + + code(Language.JAVA,"public static PinpointConfig localizerConfig = new PinpointConfig(c -> {\n" + + " c.name.set(\"" + pinpointName.get() + "\");\n" + + (customPodScalar.isPresent() ? " c.ticksPerUnit.set(OptionalDouble.of(" + customPodScalar.getAsDouble() + "));\n" : " c.podType.set(" + (podType.get() == PodType.SWING_ARM ? "GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD" : "GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD") + ");\n") + + " c.xPodOffset.set(" + offsets.get(0) + ");\n" + + " c.yPodOffset.set(" + offsets.get(1) + ");\n" + + " c.xPodDirection.set(" + (forwardPodReversed ? "GoBildaPinpointDriver.EncoderDirection.REVERSED" : "GoBildaPinpointDriver.EncoderDirection.FORWARD") + ");\n" + + " c.yPodDirection.set(" + (strafePodReversed ? "GoBildaPinpointDriver.EncoderDirection.REVERSED" : "GoBildaPinpointDriver.EncoderDirection.FORWARD") + ");\n" + + " c.globalDistanceUnit.set(DistanceUnit.INCH);\n" + + " c.offsetUnits.set(DistanceUnit.INCH);\n" + + "});"); + } +} + +class PinpointCustomPodScalar extends TuningOpMode { + + String name; + double distance; + + public PinpointCustomPodScalar(Double distance, String name) { + super("Custom Scalar Identification", + "Determines the scalar for the custom pods of the Pinpoint localizer. \n" + + "Push your robot forward " + distance + " inches exactly and then stop the Opmode", + true); + this.name = name; + this.distance = distance; + } + + @Override + protected Double runTuningOpMode() { + PinpointConfig config = new PinpointConfig(c -> { + c.name.set(name); + c.ticksPerUnit.set(OptionalDouble.of(1.0)); + c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + }); + PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + } + return Math.abs((localizer.pose().x() / distance)); + } +} + +class PinpointForwardDirection extends TuningOpMode { + String name; + PinpointTuner.PodType podType; + OptionalDouble customPodScalar; + + public PinpointForwardDirection(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar) { + super("Forward Direction Identification", + "Determines if your forward pod needs to be reversed. \n" + + "Push your robot forward and then stop the Opmode", + true); + this.name = name; + this.podType = podType; + this.customPodScalar = customPodScalar; + } + + @Override + protected Boolean runTuningOpMode() { + PinpointConfig config = new PinpointConfig(c -> { + c.name.set(name); + c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + if (customPodScalar.isPresent()) { + c.encoderResolutionUnit.set(DistanceUnit.INCH); + c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble())); + } else { + c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + } + }); + PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + waitForStart(); + + while (!isStopRequested()) { + localizer.update(); + } + + return localizer.pose().x() < 0; + } +} + +class PinpointStrafeDirection extends TuningOpMode { + String name; + PinpointTuner.PodType podType; + OptionalDouble customPodScalar; + + public PinpointStrafeDirection(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar) { + super("Strafe Direction Identification", + "Determines if your strafe pod needs to be reversed. \n" + + "Push your robot to the left and then stop the Opmode", + true); + this.name = name; + this.podType = podType; + this.customPodScalar = customPodScalar; + } + + @Override + protected Boolean runTuningOpMode() { + PinpointConfig config = new PinpointConfig(c -> { + c.name.set(name); + c.xPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.yPodDirection.set(GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + if (customPodScalar.isPresent()) { + c.encoderResolutionUnit.set(DistanceUnit.INCH); + c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble())); + } else { + c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + } + }); + PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + } + + return localizer.pose().y() < 0; + } +} + +class PinpointOffsets extends TuningOpMode> { + String name; + PinpointTuner.PodType podType; + OptionalDouble customPodScalar = OptionalDouble.empty(); + boolean forwardPodReversed, strafePodReversed; + private Pose previous = Pose.zero(); + + public PinpointOffsets(String name, PinpointTuner.PodType podType, OptionalDouble customPodScalar, Boolean forwardPodReversed, Boolean strafePodReversed) { + super("Offsets Identification", + "Automatically identifies the offsets for your Pinpoint localizer. \n" + + "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode", + true); + this.name = name; + this.podType = podType; + if (customPodScalar.isPresent()) { + this.customPodScalar = customPodScalar; + } + this.forwardPodReversed = forwardPodReversed; + this.strafePodReversed = strafePodReversed; + } + + @Override + protected List runTuningOpMode() { + PinpointConfig config = new PinpointConfig(c -> { + c.name.set(name); + c.xPodDirection.set(forwardPodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD); + c.yPodDirection.set(strafePodReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD); + if (customPodScalar.isPresent()) { + c.encoderResolutionUnit.set(DistanceUnit.INCH); + c.ticksPerUnit.set(OptionalDouble.of(customPodScalar.getAsDouble())); + c.resetMode.set(PinpointLocalizer.ResetMode.RESET_AND_RECALIBRATE_IMU); + } else { + c.podType.set(podType == PinpointTuner.PodType.SWING_ARM ? GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_SWINGARM_POD : GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + } + c.xPodOffset.set(0.0); + c.yPodOffset.set(0.0); + c.globalDistanceUnit.set(DistanceUnit.INCH); + c.offsetUnits.set(DistanceUnit.INCH); + }); + PinpointLocalizer localizer = new PinpointLocalizer(hardwareMap, config); + if (customPodScalar.isPresent()) { + localizer.reset(); + } + localizer.setPose(Pose.zero()); + localizer.update(); + + + waitForStart(); + + localizer.setPose(Pose.zero()); + + while (!isStopRequested()) { + previous = localizer.pose(); + localizer.update(); + + telemetry.addData("heading", localizer.pose().heading()); + telemetry.addData("pose", localizer.pose()); + telemetry.addData("previous", previous); + telemetry.update(); + } + + if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) { + previous = localizer.pose(); + } + + return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0)); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/Tests.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/Tests.java new file mode 100644 index 0000000..8499ff3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/Tests.java @@ -0,0 +1,327 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.algorithm.Algorithm; +import com.pedropathing.drivetrain.DrivePowers; +import com.pedropathing.drivetrain.Drivetrain; +import com.pedropathing.follower.Follower; +import com.pedropathing.localization.Localizer; +import com.pedropathing.math.Pose; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.interpolator.Interpolator; +import com.pedropathing.tuning.autotune.DisplayName; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.function.Function; +import java.util.function.Supplier; + +import static com.pedropathing.api.Paths.curve; +import static com.pedropathing.api.Paths.line; + +public class Tests extends Procedure { + enum Test { + @DisplayName("Hold Test") + HOLD, + @DisplayName("Line Test") + LINE, + @DisplayName("Curve Test") + CURVED, + @DisplayName("Interpolation Test") + INTERPOLATION_CURVED, + @DisplayName("Localization Test") + LOCALIZATION, + @DisplayName("Driving Test") + DRIVING, + @DisplayName("Pose Test") + POSE + + } + Function drivetrainFunction; + Function localizerFunction; + Supplier algorithmSupplier; + Function followerFunction; + + public Tests(Function drivetrainFunction, Function localizerFunction, Supplier algorithmSupplier) { + super("Tests", "A procedure for testing the Follower."); + this.drivetrainFunction = drivetrainFunction; + this.localizerFunction = localizerFunction; + this.algorithmSupplier = algorithmSupplier; + } + + @Override + public void run() throws InterruptedException { + boolean completed = false; + boolean algorithm = true, localizer = true, drivetrain = true; + + if (algorithmSupplier == null) + algorithm = false; + + if (localizerFunction == null) + localizer = false; + + if (drivetrainFunction == null) + drivetrain = false; + + if (algorithm && localizer && drivetrain) + followerFunction = (hardwareMap) -> new Follower(localizerFunction.apply(hardwareMap), drivetrainFunction.apply(hardwareMap), algorithmSupplier.get()); + + Inputs inputs = inputs("Select", "Select"); + Inputs.Field selectedTest = inputs.e("Test", Test.class).withDefault(Test.LINE); + Inputs.Field distance = inputs.d("Distance").withDefault(48.0); + + awaitInputs(inputs); + + switch (selectedTest.get()) { + case HOLD: + if (!algorithm) + throw new IllegalArgumentException("Algorithm is required for Hold Test."); + completed = runOpMode(new TestsHold(followerFunction)); + break; + case LINE: + if (!algorithm) + throw new IllegalArgumentException("Algorithm is required for Hold Test."); + completed = runOpMode(new TestsLine(followerFunction, distance.get())); + break; + case CURVED: + if (!algorithm) + throw new IllegalArgumentException("Algorithm is required for Hold Test."); + completed = runOpMode(new TestsCurve(followerFunction, distance.get())); + break; + case INTERPOLATION_CURVED: + if (!algorithm) + throw new IllegalArgumentException("Algorithm is required for Hold Test."); + completed = runOpMode(new TestsInterpolation(followerFunction, distance.get())); + break; + case LOCALIZATION: + if (!drivetrain) + throw new IllegalArgumentException("Drivetrain is required for Localization Test."); + if (!localizer) + throw new IllegalArgumentException("Localizer is required for Localization Test."); + completed = runOpMode(new TestsLocalization(drivetrainFunction, localizerFunction)); + break; + case POSE: + if (!localizer) + throw new IllegalArgumentException("Localizer is required for Pose Test."); + completed = runOpMode(new TestsPose(localizerFunction)); + break; + case DRIVING: + if (!drivetrain) + throw new IllegalArgumentException("Drivetrain is required for Driving Test."); + completed = runOpMode(new TestsDriving(drivetrainFunction)); + break; + } + + result("Completed", completed); + } +} + +class TestsHold extends TuningOpMode { + Function followerFunction; + + public TestsHold(Function followerFunction) { + super("Hold Test", "Tests the Follower's ability to hold a position.", true); + this.followerFunction = followerFunction; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Follower follower = followerFunction.apply(hardwareMap); + follower.setPose(Pose.zero()); + waitForStart(); + follower.hold(Pose.zero()); + while (opModeIsActive()) { + follower.update(); + } + return true; + } +} + +class TestsLine extends TuningOpMode { + Function followerFunction; + double distance; + + public TestsLine(Function followerFunction, double distance) { + super("Line Test", "Tests the Follower's ability to follow a line.", true); + this.followerFunction = followerFunction; + this.distance = distance; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Follower follower = followerFunction.apply(hardwareMap); + follower.setPose(Pose.zero()); + + double distance = 48; + boolean forward = true; + + Path path1 = line(Pose.zero(), new Pose(distance,0, 0)).constant(0); + Path path2 = line(new Pose(distance,0, 0), Pose.zero()).constant(0); + + waitForStart(); + follower.follow(path1); + + while (opModeIsActive()) { + follower.update(); + if (follower.atParametricEnd()) { + if (forward) { + follower.follow(path2); + } else { + follower.follow(path1); + } + forward = !forward; + } + } + return true; + } +} + +class TestsCurve extends TuningOpMode { + Function followerFunction; + double distance; + + public TestsCurve(Function followerFunction, double distance) { + super("Curve Test", "Tests the Follower's ability to follow a curve.", true); + this.followerFunction = followerFunction; + this.distance = distance; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Follower follower = followerFunction.apply(hardwareMap); + follower.setPose(Pose.zero()); + + double distance = 48; + boolean forward = true; + + Path path1 = curve(Pose.zero(), new Pose(distance + 0,0), new Pose(distance,distance)).tangent(); + Path path2 = curve(new Pose(distance,distance), new Pose(distance,0), Pose.zero()).tangent(); + + waitForStart(); + follower.follow(path1); + + while (opModeIsActive()) { + follower.update(); + if (follower.atParametricEnd()) { + if (forward) { + follower.follow(path2); + } else { + follower.follow(path1); + } + forward = !forward; + } + } + return true; + } +} + +class TestsInterpolation extends TuningOpMode { + Function followerFunction; + double distance; + + public TestsInterpolation(Function followerFunction, double distance) { + super("Interpolation Curve Test", "Tests the Follower's ability to follow a curve with several interpolations.", true); + this.followerFunction = followerFunction; + this.distance = distance; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Follower follower = followerFunction.apply(hardwareMap); + follower.setPose(Pose.zero()); + + double distance = 48; + boolean forward = true; + + Path path1 = curve(Pose.zero(), new Pose(distance + 0,0), new Pose(distance,distance)).heading((curve, t) -> Math.PI); + Path path2 = curve(new Pose(distance,distance), new Pose(distance,0), Pose.zero()).heading(Interpolator.piecewise().until(0.5, Interpolator.tangent).until(1.0, Interpolator.constant(0))); + + waitForStart(); + follower.follow(path1); + + while (opModeIsActive()) { + follower.update(); + if (follower.atParametricEnd()) { + if (forward) { + follower.follow(path2); + } else { + follower.follow(path1); + } + forward = !forward; + } + } + return true; + } +} + +class TestsLocalization extends TuningOpMode { + Function drivetrainFunction; + Function localizerFunction; + + public TestsLocalization(Function drivetrainFunction, Function localizerFunction) { + super("Localization Test", "Verifies localization and manual control.", true); + this.drivetrainFunction = drivetrainFunction; + this.localizerFunction = localizerFunction; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Localizer localizer = localizerFunction.apply(hardwareMap); + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + + localizer.setPose(Pose.zero()); + + waitForStart(); + + while (opModeIsActive()) { + drivetrain.drive(new DrivePowers(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x), false); + localizer.update(); + telemetry.addData("Pose", localizer.pose()); + telemetry.update(); + } + return true; + } +} + +class TestsDriving extends TuningOpMode { + Function drivetrainFunction; + + public TestsDriving(Function drivetrainFunction) { + super("Driving Test", "Tests raw drivetrain control without localization.", true); + this.drivetrainFunction = drivetrainFunction; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Drivetrain drivetrain = drivetrainFunction.apply(hardwareMap); + waitForStart(); + while (opModeIsActive()) { + drivetrain.drive(new DrivePowers(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x), true); + } + return true; + } +} + +class TestsPose extends TuningOpMode { + Function localizerFunction; + + public TestsPose(Function localizerFunction) { + super("Pose Test", "Verifies localizer output without a drivetrain.", true); + this.localizerFunction = localizerFunction; + } + + @Override + public Boolean runTuningOpMode() throws InterruptedException { + Localizer localizer = localizerFunction.apply(hardwareMap); + waitForStart(); + while (opModeIsActive()) { + localizer.update(); + telemetry.addData("Pose", localizer.pose()); + telemetry.update(); + } + return true; + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelIMUTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelIMUTuner.java new file mode 100644 index 0000000..7513192 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelIMUTuner.java @@ -0,0 +1,328 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.math.Pose; +import com.pedropathing.revhub.localizers.Encoder; +import com.pedropathing.revhub.localizers.RevHubIMU; +import com.pedropathing.revhub.localizers.ThreeWheelIMUConfig; +import com.pedropathing.revhub.localizers.ThreeWheelIMULocalizer; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.List; + +public class ThreeWheelIMUTuner extends Procedure { + + private static String leftEncoderName = "lf"; + private static String rightEncoderName = "rr"; + private static String strafeEncoderName = "lr"; + private static String imuName = "imu"; + private static RevHubOrientationOnRobot.LogoFacingDirection logoDirection = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + private static RevHubOrientationOnRobot.UsbFacingDirection usbDirection = + RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD; + + public ThreeWheelIMUTuner() { + super("Three Wheel + IMU Tuner", "Tune three odometry pods with an IMU"); + } + + @Override + public void run() throws InterruptedException { + Inputs setup = inputs("Encoder + IMU Setup", + "Set encoder motor ports, IMU HardwareMap name, and Control Hub orientation."); + Inputs.Field leftEncoder = setup.s("Left Encoder Motor Name").withDefault("lf"); + Inputs.Field rightEncoder = setup.s("Right Encoder Motor Name").withDefault("rr"); + Inputs.Field strafeEncoder = setup.s("Strafe Encoder Motor Name").withDefault("lr"); + Inputs.Field imu = setup.s("IMU HardwareMap Name").withDefault("imu"); + Inputs.Field logo = + setup.e("Logo Facing Direction", RevHubOrientationOnRobot.LogoFacingDirection.class) + .withDefault(RevHubOrientationOnRobot.LogoFacingDirection.UP); + Inputs.Field usb = + setup.e("USB Facing Direction", RevHubOrientationOnRobot.UsbFacingDirection.class) + .withDefault(RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD); + awaitInputs(setup); + leftEncoderName = leftEncoder.get(); + rightEncoderName = rightEncoder.get(); + strafeEncoderName = strafeEncoder.get(); + imuName = imu.get(); + logoDirection = logo.get(); + usbDirection = usb.get(); + + Inputs resolution = inputs("Encoder Resolution Identification", + "Set a positive push distance in inches. Keep the robot straight during each push."); + Inputs.Field distance = resolution.d("Distance").withDefault(48.0); + awaitInputs(resolution); + if (!(distance.get() > 0.0)) { + abort("Enter a positive distance in inches."); + return; + } + + List left = measure("Left", distance.get()); + if (left == null) { + return; + } + List right = measure("Right", distance.get()); + if (right == null) { + return; + } + List strafe = measure("Strafe", distance.get()); + if (strafe == null) { + return; + } + + double leftTicksPerInch = left.get(0); + double rightTicksPerInch = right.get(0); + double strafeTicksPerInch = strafe.get(0); + double forwardTicksPerInch = 2.0 / (1.0 / leftTicksPerInch + 1.0 / rightTicksPerInch); + + double forward = 1.0 / forwardTicksPerInch; + double lateral = 1.0 / strafeTicksPerInch; + + List leftOffsets = runOpMode(new ThreeWheelIMUOffsets( + true, forward, lateral, left.get(1), right.get(1), strafe.get(1))); + if (leftOffsets == null) { + abort("Left stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop."); + return; + } + List rightOffsets = runOpMode(new ThreeWheelIMUOffsets( + false, forward, lateral, left.get(1), right.get(1), strafe.get(1))); + + if (rightOffsets == null) { + abort("Right stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop."); + return; + } + ThreeWheelIMUConfig config = config(true, forward, lateral, + left.get(1), right.get(1), strafe.get(1)); + config.leftPodY.set(leftOffsets.get(0)); + config.rightPodY.set(rightOffsets.get(0)); + config.turnTicksToRadians.set(forward); + + Double turn = runOpMode(new ThreeWheelIMUTurn(config)); + if (turn == null) { + abort("Turn stage ended without positive rotation. Rotate 360 degrees CCW, then press Stop."); + return; + } + double strafeX = (leftOffsets.get(1) + rightOffsets.get(1)) / 2.0 * turn / lateral; + + result("leftEncoderName", leftEncoderName); + result("rightEncoderName", rightEncoderName); + result("strafeEncoderName", strafeEncoderName); + result("imuName", imuName); + result("logoDirection", logoDirection); + result("usbDirection", usbDirection); + result("leftPodY", leftOffsets.get(0)); + result("rightPodY", rightOffsets.get(0)); + result("strafePodX", strafeX); + result("leftTicksPerInch", leftTicksPerInch); + result("rightTicksPerInch", rightTicksPerInch); + result("forwardTicksPerInch", forwardTicksPerInch); + result("strafeTicksPerInch", strafeTicksPerInch); + result("forwardTicksToInches", forward); + result("strafeTicksToInches", lateral); + result("turnTicksToRadians", turn); + result("leftEncoderDirection", direction(left.get(1))); + result("rightEncoderDirection", direction(right.get(1))); + result("strafeEncoderDirection", direction(strafe.get(1))); + + code(Language.JAVA, + "public static ThreeWheelIMUConfig localizerConfig = new ThreeWheelIMUConfig(c -> {\n" + + " c.leftEncoderName.set(\"" + leftEncoderName + "\");\n" + + " c.rightEncoderName.set(\"" + rightEncoderName + "\");\n" + + " c.strafeEncoderName.set(\"" + strafeEncoderName + "\");\n" + + " c.imuName.set(\"" + imuName + "\");\n" + + " c.imuOrientation.set(new RevHubOrientationOnRobot(\n" + + " RevHubOrientationOnRobot.LogoFacingDirection." + logoDirection.name() + ",\n" + + " RevHubOrientationOnRobot.UsbFacingDirection." + usbDirection.name() + "\n" + + " ));\n" + + " c.leftPodY.set(" + leftOffsets.get(0) + ");\n" + + " c.rightPodY.set(" + rightOffsets.get(0) + ");\n" + + " c.strafePodX.set(" + strafeX + ");\n" + + " c.forwardTicksToInches.set(" + forward + ");\n" + + " c.strafeTicksToInches.set(" + lateral + ");\n" + + " c.turnTicksToRadians.set(" + turn + ");\n" + + " c.leftEncoderDirection.set(" + direction(left.get(1)) + ");\n" + + " c.rightEncoderDirection.set(" + direction(right.get(1)) + ");\n" + + " c.strafeEncoderDirection.set(" + direction(strafe.get(1)) + ");\n" + + "});"); + } + + private List measure(String pod, double distance) throws InterruptedException { + List measured = runOpMode(new ThreeWheelIMUResolution(pod, distance)); + if (measured == null) { + abort(pod + " stage ended without a nonzero measurement. Check the displayed ticks, complete the push, then press Stop."); + return null; + } + return measured; + } + + static String direction(double direction) { + return direction == Encoder.REVERSE ? "Encoder.REVERSE" : "Encoder.FORWARD"; + } + + static ThreeWheelIMUConfig config(boolean left, double forward, double strafe, + double leftDirection, double rightDirection, double strafeDirection) { + return new ThreeWheelIMUConfig(c -> { + c.leftEncoderName.set(leftEncoderName); + c.rightEncoderName.set(rightEncoderName); + c.strafeEncoderName.set(strafeEncoderName); + c.imuName.set(imuName); + c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(logoDirection, usbDirection))); c.leftPodY.set(left ? 0.0 : 1.0); + c.rightPodY.set(left ? -1.0 : 0.0); + c.strafePodX.set(0.0); + c.forwardTicksToInches.set(forward); + c.strafeTicksToInches.set(strafe); + c.turnTicksToRadians.set(0.0); + c.leftEncoderDirection.set(leftDirection); + c.rightEncoderDirection.set(rightDirection); + c.strafeEncoderDirection.set(strafeDirection); + }); + } + + static ThreeWheelIMULocalizer localizer(HardwareMap map, ThreeWheelIMUConfig config) { + for (LynxModule hub : map.getAll(LynxModule.class)) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + for (String name : new String[]{"lf", "lr", "rf", "rr"}) { + DcMotorEx motor = map.get(DcMotorEx.class, name); + motor.setPower(0); + motor.setDirection(name.equals("lf") || name.equals("lr") + ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + return new ThreeWheelIMULocalizer(map, config); + } +} + +class ThreeWheelIMUResolution extends TuningOpMode> { + + String pod; + double distance; + + ThreeWheelIMUResolution(String pod, double distance) { + super(pod + " Encoder Resolution and Direction", + "After Start, push the robot " + (pod.equals("Strafe") ? "left " : "forward ") + + distance + " inches exactly without turning. Stop moving, press Stop to save this measurement.", true); + this.pod = pod; + this.distance = distance; + } + + @Override + protected List runTuningOpMode() { + ThreeWheelIMUConfig config = ThreeWheelIMUTuner.config(!pod.equals("Right"), 1.0, 1.0, + Encoder.FORWARD, Encoder.FORWARD, Encoder.FORWARD); + ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + } + + if (position == null) { + return null; + } + double movement = pod.equals("Strafe") ? position.y() : position.x(); + if (movement == 0.0) { + return null; + } + return List.of(Math.abs(movement / distance), movement < 0 ? Encoder.REVERSE : Encoder.FORWARD); + } +} + +class ThreeWheelIMUOffsets extends TuningOpMode> { + + boolean left; + double forward; + double strafe; + double leftDirection; + double rightDirection; + double strafeDirection; + + ThreeWheelIMUOffsets(boolean left, double forward, double strafe, + double leftDirection, double rightDirection, double strafeDirection) { + super((left ? "Left" : "Right") + " Pod Offset Identification", + "After Start, rotate exactly 180 degrees counterclockwise about the robot center. " + + "Keep that center fixed. Stop moving, press Stop to save this measurement.", true); + this.left = left; + this.forward = forward; + this.strafe = strafe; + this.leftDirection = leftDirection; + this.rightDirection = rightDirection; + this.strafeDirection = strafeDirection; + } + + @Override + protected List runTuningOpMode() { + ThreeWheelIMUConfig config = ThreeWheelIMUTuner.config(left, forward, strafe, + leftDirection, rightDirection, strafeDirection); + boolean previousUseIMU = ThreeWheelIMULocalizer.useIMU; + ThreeWheelIMULocalizer.useIMU = false; + try { + ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + localizer.update(); + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + } + + if (position == null) { + return null; + } + return List.of(-position.x() / Math.PI, position.y() / Math.PI); + } finally { + ThreeWheelIMULocalizer.useIMU = previousUseIMU; + } + } +} + +class ThreeWheelIMUTurn extends TuningOpMode { + + ThreeWheelIMUConfig config; + + ThreeWheelIMUTurn(ThreeWheelIMUConfig config) { + super("Turn Multiplier Identification", + "After Start, rotate exactly 360 degrees counterclockwise. " + + "Stop moving, press Stop to save this measurement.", true); + this.config = config; + } + + @Override + protected Double runTuningOpMode() { + boolean previousUseIMU = ThreeWheelIMULocalizer.useIMU; + ThreeWheelIMULocalizer.useIMU = false; + try { + ThreeWheelIMULocalizer localizer = ThreeWheelIMUTuner.localizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + localizer.update(); + double startHeading = localizer.getTotalHeading(); + Double heading = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + heading = localizer.getTotalHeading(); + } + + if (heading == null || heading <= startHeading) { + return null; + } + return config.turnTicksToRadians.get() * (2.0 * Math.PI) / (heading - startHeading); + } finally { + ThreeWheelIMULocalizer.useIMU = previousUseIMU; + } + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelTuner.java new file mode 100644 index 0000000..ff64260 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ThreeWheelTuner.java @@ -0,0 +1,290 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.math.Pose; +import com.pedropathing.revhub.localizers.Encoder; +import com.pedropathing.revhub.localizers.ThreeWheelConfig; +import com.pedropathing.revhub.localizers.ThreeWheelLocalizer; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.List; + +public class ThreeWheelTuner extends Procedure { + + private static String leftEncoderName = "lf"; + private static String rightEncoderName = "rr"; + private static String strafeEncoderName = "lr"; + + public ThreeWheelTuner() { + super("Three Wheel Tuner", "Tune three odometry pods"); + } + + @Override + public void run() throws InterruptedException { + Inputs setup = inputs("Encoder Setup", + "Set the motor ports that the three odometry encoders are plugged into."); + Inputs.Field leftEncoder = setup.s("Left Encoder Motor Name").withDefault("lf"); + Inputs.Field rightEncoder = setup.s("Right Encoder Motor Name").withDefault("rr"); + Inputs.Field strafeEncoder = setup.s("Strafe Encoder Motor Name").withDefault("lr"); + awaitInputs(setup); + leftEncoderName = leftEncoder.get(); + rightEncoderName = rightEncoder.get(); + strafeEncoderName = strafeEncoder.get(); + + Inputs resolution = inputs("Encoder Resolution Identification", + "Set a positive push distance in inches. Keep the robot straight during each push."); + Inputs.Field distance = resolution.d("Distance").withDefault(48.0); + awaitInputs(resolution); + if (!(distance.get() > 0.0)) { + abort("Enter a positive distance in inches."); + return; + } + + List left = measure("Left", distance.get()); + if (left == null) { + return; + } + List right = measure("Right", distance.get()); + if (right == null) { + return; + } + List strafe = measure("Strafe", distance.get()); + if (strafe == null) { + return; + } + + double leftTicksPerInch = left.get(0); + double rightTicksPerInch = right.get(0); + double strafeTicksPerInch = strafe.get(0); + double forwardTicksPerInch = 2.0 / (1.0 / leftTicksPerInch + 1.0 / rightTicksPerInch); + + double forward = 1.0 / forwardTicksPerInch; + double lateral = 1.0 / strafeTicksPerInch; + + List leftOffsets = runOpMode(new ThreeWheelOffsets( + true, forward, lateral, left.get(1), right.get(1), strafe.get(1))); + if (leftOffsets == null) { + abort("Left stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop."); + return; + } + List rightOffsets = runOpMode(new ThreeWheelOffsets( + false, forward, lateral, left.get(1), right.get(1), strafe.get(1))); + + if (rightOffsets == null) { + abort("Right stage ended without parallel pod travel. Rotate 180 degrees CCW, then press Stop."); + return; + } + ThreeWheelConfig config = config(true, forward, lateral, + left.get(1), right.get(1), strafe.get(1)); + config.leftPodY.set(leftOffsets.get(0)); + config.rightPodY.set(rightOffsets.get(0)); + config.turnTicksToRadians.set(forward); + + Double turn = runOpMode(new ThreeWheelTurn(config)); + if (turn == null) { + abort("Turn stage ended without positive rotation. Rotate 360 degrees CCW, then press Stop."); + return; + } + double strafeX = (leftOffsets.get(1) + rightOffsets.get(1)) / 2.0 * turn / lateral; + + result("leftEncoderName", leftEncoderName); + result("rightEncoderName", rightEncoderName); + result("strafeEncoderName", strafeEncoderName); + result("leftPodY", leftOffsets.get(0)); + result("rightPodY", rightOffsets.get(0)); + result("strafePodX", strafeX); + result("leftTicksPerInch", leftTicksPerInch); + result("rightTicksPerInch", rightTicksPerInch); + result("forwardTicksPerInch", forwardTicksPerInch); + result("strafeTicksPerInch", strafeTicksPerInch); + result("forwardTicksToInches", forward); + result("strafeTicksToInches", lateral); + result("turnTicksToRadians", turn); + result("leftEncoderDirection", direction(left.get(1))); + result("rightEncoderDirection", direction(right.get(1))); + result("strafeEncoderDirection", direction(strafe.get(1))); + + code(Language.JAVA, + "public static ThreeWheelConfig localizerConfig = new ThreeWheelConfig(c -> {\n" + + " c.leftEncoderName.set(\"" + leftEncoderName + "\");\n" + + " c.rightEncoderName.set(\"" + rightEncoderName + "\");\n" + + " c.strafeEncoderName.set(\"" + strafeEncoderName + "\");\n" + + " c.leftPodY.set(" + leftOffsets.get(0) + ");\n" + + " c.rightPodY.set(" + rightOffsets.get(0) + ");\n" + + " c.strafePodX.set(" + strafeX + ");\n" + + " c.forwardTicksToInches.set(" + forward + ");\n" + + " c.strafeTicksToInches.set(" + lateral + ");\n" + + " c.turnTicksToRadians.set(" + turn + ");\n" + + " c.leftEncoderDirection.set(" + direction(left.get(1)) + ");\n" + + " c.rightEncoderDirection.set(" + direction(right.get(1)) + ");\n" + + " c.strafeEncoderDirection.set(" + direction(strafe.get(1)) + ");\n" + + "});"); + } + + private List measure(String pod, double distance) throws InterruptedException { + List measured = runOpMode(new ThreeWheelResolution(pod, distance)); + if (measured == null) { + abort(pod + " stage ended without a nonzero measurement. Check the displayed ticks, complete the push, then press Stop."); + return null; + } + return measured; + } + + static String direction(double direction) { + return direction == Encoder.REVERSE ? "Encoder.REVERSE" : "Encoder.FORWARD"; + } + + static ThreeWheelConfig config(boolean left, double forward, double strafe, + double leftDirection, double rightDirection, double strafeDirection) { + return new ThreeWheelConfig(c -> { + c.leftEncoderName.set(leftEncoderName); + c.rightEncoderName.set(rightEncoderName); + c.strafeEncoderName.set(strafeEncoderName); + c.leftPodY.set(left ? 0.0 : 1.0); + c.rightPodY.set(left ? -1.0 : 0.0); + c.strafePodX.set(0.0); + c.forwardTicksToInches.set(forward); + c.strafeTicksToInches.set(strafe); + c.turnTicksToRadians.set(0.0); + c.leftEncoderDirection.set(leftDirection); + c.rightEncoderDirection.set(rightDirection); + c.strafeEncoderDirection.set(strafeDirection); + }); + } + + static ThreeWheelLocalizer localizer(HardwareMap map, ThreeWheelConfig config) { + for (LynxModule hub : map.getAll(LynxModule.class)) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + for (String name : new String[]{"lf", "lr", "rf", "rr"}) { + DcMotorEx motor = map.get(DcMotorEx.class, name); + motor.setPower(0); + motor.setDirection(name.equals("lf") || name.equals("lr") + ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + return new ThreeWheelLocalizer(map, config); + } +} + +class ThreeWheelResolution extends TuningOpMode> { + + String pod; + double distance; + + ThreeWheelResolution(String pod, double distance) { + super(pod + " Encoder Resolution and Direction", + "After Start, push the robot " + (pod.equals("Strafe") ? "left " : "forward ") + + distance + " inches exactly without turning. Stop moving, press Stop to save this measurement.", true); + this.pod = pod; + this.distance = distance; + } + + @Override + protected List runTuningOpMode() { + ThreeWheelConfig config = ThreeWheelTuner.config(!pod.equals("Right"), 1.0, 1.0, + Encoder.FORWARD, Encoder.FORWARD, Encoder.FORWARD); + ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + } + + if (position == null) { + return null; + } + double movement = pod.equals("Strafe") ? position.y() : position.x(); + if (movement == 0.0) { + return null; + } + return List.of(Math.abs(movement / distance), movement < 0 ? Encoder.REVERSE : Encoder.FORWARD); + } +} + +class ThreeWheelOffsets extends TuningOpMode> { + + boolean left; + double forward; + double strafe; + double leftDirection; + double rightDirection; + double strafeDirection; + + ThreeWheelOffsets(boolean left, double forward, double strafe, + double leftDirection, double rightDirection, double strafeDirection) { + super((left ? "Left" : "Right") + " Pod Offset Identification", + "After Start, rotate exactly 180 degrees counterclockwise about the robot center. " + + "Keep that center fixed. Stop moving, press Stop to save this measurement.", true); + this.left = left; + this.forward = forward; + this.strafe = strafe; + this.leftDirection = leftDirection; + this.rightDirection = rightDirection; + this.strafeDirection = strafeDirection; + } + + @Override + protected List runTuningOpMode() { + ThreeWheelConfig config = ThreeWheelTuner.config(left, forward, strafe, + leftDirection, rightDirection, strafeDirection); + ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + localizer.update(); + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + } + + if (position == null) { + return null; + } + return List.of(-position.x() / Math.PI, position.y() / Math.PI); + } +} + +class ThreeWheelTurn extends TuningOpMode { + + ThreeWheelConfig config; + + ThreeWheelTurn(ThreeWheelConfig config) { + super("Turn Multiplier Identification", + "After Start, rotate exactly 360 degrees counterclockwise. " + + "Stop moving, press Stop to save this measurement.", true); + this.config = config; + } + + @Override + protected Double runTuningOpMode() { + ThreeWheelLocalizer localizer = ThreeWheelTuner.localizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + localizer.update(); + double startHeading = localizer.getTotalHeading(); + Double heading = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + heading = localizer.getTotalHeading(); + } + + if (heading == null || heading <= startHeading) { + return null; + } + return config.turnTicksToRadians.get() * (2.0 * Math.PI) / (heading - startHeading); + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/TwoWheelTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/TwoWheelTuner.java new file mode 100644 index 0000000..f539a9c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/TwoWheelTuner.java @@ -0,0 +1,404 @@ +package org.firstinspires.ftc.teamcode.pedro.procedures; + +import com.pedropathing.math.Pose; +import com.pedropathing.revhub.localizers.Encoder; +import com.pedropathing.revhub.localizers.RevHubIMU; +import com.pedropathing.revhub.localizers.TwoWheelConfig; +import com.pedropathing.revhub.localizers.TwoWheelLocalizer; +import com.pedropathing.tuning.autotune.Inputs; +import com.pedropathing.tuning.autotune.Procedure; +import com.pedropathing.tuning.autotune.TuningOpMode; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; + +import java.util.List; + +public class TwoWheelTuner extends Procedure { + + public TwoWheelTuner() { + super("Two Wheel Tuner", "A procedure for tuning the Two Wheel localizer."); + } + + @Override + public void run() throws InterruptedException { + Inputs setup = inputs("Setup", "Set encoder, IMU, and Control Hub orientation"); + Inputs.Field forwardPodName = setup.s("Forward Encoder Motor Name").withDefault("lf"); + Inputs.Field strafePodName = setup.s("Strafe Encoder Motor Name").withDefault("rr"); + Inputs.Field imuName = setup.s("IMU HardwareMap Name").withDefault("imu"); + Inputs.Field logoDirection = + setup.e("Logo Facing Direction", RevHubOrientationOnRobot.LogoFacingDirection.class) + .withDefault(RevHubOrientationOnRobot.LogoFacingDirection.UP); + Inputs.Field usbDirection = + setup.e("USB Facing Direction", RevHubOrientationOnRobot.UsbFacingDirection.class) + .withDefault(RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD); + awaitInputs(setup); + + Inputs resolution = inputs("Encoder Resolution Identification", "Set the exact distance you will push the robot in inches"); + Inputs.Field distance = resolution.d("Distance").withDefault(48.0); + awaitInputs(resolution); + + TwoWheelSetup values = new TwoWheelSetup( + forwardPodName.get(), + strafePodName.get(), + imuName.get(), + logoDirection.get(), + usbDirection.get() + ); + + Double forwardTicksPerInchResult = runOpMode(new TwoWheelForwardResolution(values, distance.get())); + Double strafeTicksPerInchResult = runOpMode(new TwoWheelStrafeResolution(values, distance.get())); + + if (forwardTicksPerInchResult == null || strafeTicksPerInchResult == null + || forwardTicksPerInchResult == 0.0 || strafeTicksPerInchResult == 0.0) { + abort("Encoder resolution measurement was zero. Complete both pushes before pressing Stop."); + return; + } + + double forwardTicksPerInch = forwardTicksPerInchResult; + double strafeTicksPerInch = strafeTicksPerInchResult; + + double forwardTicksToInches = 1.0 / forwardTicksPerInch; + double strafeTicksToInches = 1.0 / strafeTicksPerInch; + + boolean forwardPodReversed = runOpMode( + new TwoWheelForwardDirection(values, forwardTicksToInches, strafeTicksToInches) + ); + + boolean strafePodReversed = runOpMode( + new TwoWheelStrafeDirection(values, forwardTicksToInches, strafeTicksToInches) + ); + + List offsets = runOpMode( + new TwoWheelOffsets( + values, + forwardTicksToInches, + strafeTicksToInches, + forwardPodReversed, + strafePodReversed + ) + ); + + result("xPodName", values.forwardPodName); + result("yPodName", values.strafePodName); + result("imuName", values.imuName); + result("logoDirection", values.logoDirection); + result("usbDirection", values.usbDirection); + result("forwardTicksPerInch", forwardTicksPerInch); + result("strafeTicksPerInch", strafeTicksPerInch); + result("forwardTicksToInches", forwardTicksToInches); + result("strafeTicksToInches", strafeTicksToInches); + result("xPodDirection", forwardPodReversed ? "REVERSED" : "FORWARD"); + result("yPodDirection", strafePodReversed ? "REVERSED" : "FORWARD"); + result("xPodOffset", offsets.get(0)); + result("yPodOffset", offsets.get(1)); + + code(Language.JAVA, + "public static TwoWheelConfig localizerConfig = new TwoWheelConfig(c -> {\n" + + " c.xPodName.set(\"" + values.forwardPodName + "\");\n" + + " c.yPodName.set(\"" + values.strafePodName + "\");\n" + + " c.imuName.set(\"" + values.imuName + "\");\n" + + " c.xPodOffset.set(" + offsets.get(0) + ");\n" + + " c.yPodOffset.set(" + offsets.get(1) + ");\n" + + " c.forwardTicksToInches.set(" + forwardTicksToInches + ");\n" + + " c.strafeTicksToInches.set(" + strafeTicksToInches + ");\n" + + " c.xPodDirection.set(" + + (forwardPodReversed ? "Encoder.REVERSE" : "Encoder.FORWARD") + + ");\n" + + " c.yPodDirection.set(" + + (strafePodReversed ? "Encoder.REVERSE" : "Encoder.FORWARD") + + ");\n" + + " c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(\n" + + " RevHubOrientationOnRobot.LogoFacingDirection." + values.logoDirection.name() + ",\n" + + " RevHubOrientationOnRobot.UsbFacingDirection." + values.usbDirection.name() + "\n" + + " )));\n" + + "});" + ); + } + + static TwoWheelConfig config( + TwoWheelSetup values, + double forwardTicksToInches, + double strafeTicksToInches, + double xPodDirection, + double yPodDirection, + double xPodOffset, + double yPodOffset + ) { + return new TwoWheelConfig(c -> { + c.xPodName.set(values.forwardPodName); + c.yPodName.set(values.strafePodName); + c.imuName.set(values.imuName); + c.xPodOffset.set(xPodOffset); + c.yPodOffset.set(yPodOffset); + c.forwardTicksToInches.set(forwardTicksToInches); + c.strafeTicksToInches.set(strafeTicksToInches); + c.xPodDirection.set(xPodDirection); + c.yPodDirection.set(yPodDirection); + c.imu.set(new RevHubIMU(new RevHubOrientationOnRobot(values.logoDirection, values.usbDirection))); + }); + } +} + +class TwoWheelSetup { + + String forwardPodName; + String strafePodName; + String imuName; + RevHubOrientationOnRobot.LogoFacingDirection logoDirection; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection; + + TwoWheelSetup( + String forwardPodName, + String strafePodName, + String imuName, + RevHubOrientationOnRobot.LogoFacingDirection logoDirection, + RevHubOrientationOnRobot.UsbFacingDirection usbDirection + ) { + this.forwardPodName = forwardPodName; + this.strafePodName = strafePodName; + this.imuName = imuName; + this.logoDirection = logoDirection; + this.usbDirection = usbDirection; + } +} + +class TwoWheelForwardResolution extends TuningOpMode { + + TwoWheelSetup values; + double distance; + + TwoWheelForwardResolution(TwoWheelSetup values, double distance) { + super( + "Forward Encoder Resolution Identification", + "Push your robot forward " + distance + " inches exactly and then stop the Opmode", + true + ); + this.values = values; + this.distance = distance; + } + + @Override + protected Double runTuningOpMode() { + TwoWheelConfig config = TwoWheelTuner.config( + values, + 1.0, + 1.0, + Encoder.FORWARD, + Encoder.FORWARD, + 0.0, + 0.0 + ); + + TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + telemetry.addData("heading", localizer.pose().heading()); + telemetry.addData("pose", localizer.pose()); + telemetry.update(); + } + + if (position == null || position.x() == 0.0) { + return null; + } + return Math.abs(position.x() / distance); + } +} + +class TwoWheelStrafeResolution extends TuningOpMode { + + TwoWheelSetup values; + double distance; + + TwoWheelStrafeResolution(TwoWheelSetup values, double distance) { + super( + "Strafe Encoder Resolution Identification", + "Push your robot left " + distance + " inches exactly and then stop the Opmode", + true + ); + this.values = values; + this.distance = distance; + } + + @Override + protected Double runTuningOpMode() { + TwoWheelConfig config = TwoWheelTuner.config( + values, + 1.0, + 1.0, + Encoder.FORWARD, + Encoder.FORWARD, + 0.0, + 0.0 + ); + + TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + Pose position = null; + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + position = localizer.pose(); + } + + if (position == null || position.y() == 0.0) { + return null; + } + return Math.abs(position.y() / distance); + } +} + +class TwoWheelForwardDirection extends TuningOpMode { + + TwoWheelSetup values; + double forwardTicksToInches; + double strafeTicksToInches; + + TwoWheelForwardDirection(TwoWheelSetup values, double forwardTicksToInches, double strafeTicksToInches) { + super( + "Forward Direction Identification", + "Determines if your forward pod needs to be reversed.\n" + + "Push your robot forward and then stop the Opmode", + true + ); + this.values = values; + this.forwardTicksToInches = forwardTicksToInches; + this.strafeTicksToInches = strafeTicksToInches; + } + + @Override + protected Boolean runTuningOpMode() { + TwoWheelConfig config = TwoWheelTuner.config( + values, + forwardTicksToInches, + strafeTicksToInches, + Encoder.FORWARD, + Encoder.FORWARD, + 0.0, + 0.0 + ); + + TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + } + + return localizer.pose().x() < 0; + } +} + +class TwoWheelStrafeDirection extends TuningOpMode { + + TwoWheelSetup values; + double forwardTicksToInches; + double strafeTicksToInches; + + TwoWheelStrafeDirection(TwoWheelSetup values, double forwardTicksToInches, double strafeTicksToInches) { + super( + "Strafe Direction Identification", + "Determines if your strafe pod needs to be reversed.\n" + + "Push your robot left and then stop the Opmode", + true + ); + this.values = values; + this.forwardTicksToInches = forwardTicksToInches; + this.strafeTicksToInches = strafeTicksToInches; + } + + @Override + protected Boolean runTuningOpMode() { + TwoWheelConfig config = TwoWheelTuner.config( + values, + forwardTicksToInches, + strafeTicksToInches, + Encoder.FORWARD, + Encoder.FORWARD, + 0.0, + 0.0 + ); + + TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config); + localizer.setPose(new Pose(0, 0)); + + waitForStart(); + while (!isStopRequested()) { + localizer.update(); + } + + return localizer.pose().y() < 0; + } +} + +class TwoWheelOffsets extends TuningOpMode> { + + TwoWheelSetup values; + double forwardTicksToInches; + double strafeTicksToInches; + boolean forwardPodReversed; + boolean strafePodReversed; + Pose previous = Pose.zero(); + + TwoWheelOffsets( + TwoWheelSetup values, + double forwardTicksToInches, + double strafeTicksToInches, + boolean forwardPodReversed, + boolean strafePodReversed + ) { + super( + "Two Wheel Offset Identification", + "Automatically identifies the offsets for your Two Wheel localizer.\n" + + "Spin your robot in place 180 degrees counterclockwise and then stop the Opmode", + true + ); + this.values = values; + this.forwardTicksToInches = forwardTicksToInches; + this.strafeTicksToInches = strafeTicksToInches; + this.forwardPodReversed = forwardPodReversed; + this.strafePodReversed = strafePodReversed; + } + + @Override + protected List runTuningOpMode() { + TwoWheelConfig config = TwoWheelTuner.config( + values, + forwardTicksToInches, + strafeTicksToInches, + forwardPodReversed ? Encoder.REVERSE : Encoder.FORWARD, + strafePodReversed ? Encoder.REVERSE : Encoder.FORWARD, + 0.0, + 0.0 + ); + + TwoWheelLocalizer localizer = new TwoWheelLocalizer(hardwareMap, config); + localizer.setPose(Pose.zero()); + localizer.update(); + + waitForStart(); + + localizer.setPose(Pose.zero()); + + while (!isStopRequested()) { + previous = localizer.pose(); + localizer.update(); + + telemetry.addData("heading", localizer.pose().heading()); + telemetry.addData("pose", localizer.pose()); + telemetry.addData("previous", previous); + telemetry.update(); + } + + if (localizer.pose().x() != Pose.zero().x() || localizer.pose().y() != Pose.zero().y()) { + previous = localizer.pose(); + } + + return List.of(((-previous.y()) / 2.0), ((-previous.x()) / 2.0)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java deleted file mode 100644 index 06b1fce..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing; - -import com.pedropathing.follower.Follower; -import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; -import com.pedropathing.paths.PathConstraints; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); - - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); - - public static Follower createFollower(HardwareMap hardwareMap) { - return new FollowerBuilder(followerConstants, hardwareMap) - .pathConstraints(pathConstraints) - .build(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java deleted file mode 100644 index 195749a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ /dev/null @@ -1,1793 +0,0 @@ -package org.firstinspires.ftc.teamcode.pedroPathing; - -import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; -import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrent; -import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawCurrentAndHistory; -import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower; -import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot; -import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM; - -import com.bylazar.configurables.PanelsConfigurables; -import com.bylazar.configurables.annotations.Configurable; -import com.bylazar.configurables.annotations.IgnoreConfigurable; -import com.bylazar.field.FieldManager; -import com.bylazar.field.PanelsField; -import com.bylazar.field.Style; -import com.bylazar.telemetry.PanelsTelemetry; -import com.bylazar.telemetry.TelemetryManager; -import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.*; -import com.pedropathing.math.*; -import com.pedropathing.paths.*; -import com.pedropathing.telemetry.SelectableOpMode; -import com.pedropathing.util.*; -import static com.pedropathing.math.MathFunctions.quadraticFit; - -import android.annotation.SuppressLint; - -import com.qualcomm.hardware.lynx.LynxModule; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.util.ElapsedTime; - -import java.util.ArrayList; -import java.util.List; - -/** - * This is the Tuning class. It contains a selection menu for various tuning OpModes. - * - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 6/26/2025 - */ -@Configurable -@TeleOp(name = "Tuning", group = "Pedro Pathing") -public class Tuning extends SelectableOpMode { - public static Follower follower; - - @IgnoreConfigurable - static PoseHistory poseHistory; - - @IgnoreConfigurable - static TelemetryManager telemetryM; - - @IgnoreConfigurable - static ArrayList changes = new ArrayList<>(); - - public Tuning() { - super("Select a Tuning OpMode", s -> { - s.folder("Localization", l -> { - l.add("Localization Test", LocalizationTest::new); - l.add("Offsets Tuner", OffsetsTuner::new); - l.add("Forward Tuner", ForwardTuner::new); - l.add("Lateral Tuner", LateralTuner::new); - l.add("Turn Tuner", TurnTuner::new); - }); - s.folder("Automatic", a -> { - a.add("Forward Velocity Tuner", ForwardVelocityTuner::new); - a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); - a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); - a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); - a.add("Predictive Braking Tuner", PredictiveBrakingTuner::new); - }); - s.folder("Manual", p -> { - p.add("Translational Tuner", TranslationalTuner::new); - p.add("Heading Tuner", HeadingTuner::new); - p.add("Drive Tuner", DriveTuner::new); - p.add("Centripetal Tuner", CentripetalTuner::new); - }); - s.folder("Tests", p -> { - p.add("Line", Line::new); - p.add("Triangle", Triangle::new); - p.add("Circle", Circle::new); - }); - s.folder("Swerve", p-> { - p.add("Analog Min / Max Tuner", AnalogMinMaxTuner::new); - p.add("Swerve Offsets Test", SwerveOffsetsTest::new); - p.add("Swerve Turn Test", SwerveTurnTest::new); - }); - }); - } - - @Override - public void onSelect() { - if (follower == null) { - follower = Constants.createFollower(hardwareMap); - PanelsConfigurables.INSTANCE.refreshClass(this); - } else { - follower = Constants.createFollower(hardwareMap); - } - - follower.setStartingPose(new Pose()); - - poseHistory = follower.getPoseHistory(); - - telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); - } - - @Override - public void onLog(List lines) {} - - public static void drawCurrent() { - try { - Drawing.drawRobot(follower.getPose()); - Drawing.sendPacket(); - } catch (Exception e) { - throw new RuntimeException("Drawing failed " + e); - } - } - - public static void drawCurrentAndHistory() { - Drawing.drawPoseHistory(poseHistory); - drawCurrent(); - } - - /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ - public static void stopRobot() { - follower.startTeleopDrive(true); - follower.setTeleOpDrive(0,0,0,true); - } -} - -/** - * This is the LocalizationTest OpMode. This is basically just a simple drive attached to a - * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. - * You should use this to check the robot's localization. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @author Kabir Goyal - * @version 1.0, 5/6/2024 - */ -class LocalizationTest extends OpMode { - boolean debugStringEnabled = false; - - @Override - public void init() { - follower.setStartingPose(new Pose(72,72)); - } - - /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ - @Override - public void init_loop() { - if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { - debugStringEnabled = !debugStringEnabled; - } - - - telemetryM.debug("This will print your robot's position to telemetry while " - + "allowing robot control through a basic drive on gamepad 1."); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + - " (press gamepad a to toggle)"); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.startTeleopDrive(); - follower.update(); - } - - /** - * This updates the robot's pose estimate, the simple drive, and updates the - * Panels telemetry with the robot's position as well as draws the robot's position. - */ - @Override - public void loop() { - if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { - debugStringEnabled = !debugStringEnabled; - } - - follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); - follower.update(); - - telemetryM.debug("x:" + follower.getPose().getX()); - telemetryM.debug("y:" + follower.getPose().getY()); - telemetryM.debug("heading:" + follower.getPose().getHeading()); - telemetryM.debug("total heading:" + follower.getTotalHeading()); - if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + - follower.getDrivetrain().debugString()); - } - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - -/** - * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the - * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the - * robot's current distance in ticks to the specified distance in inches. So, to use this, run the - * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're - * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials - * and average the results. Then, input the multiplier into the forward ticks to inches in your - * localizer of choice. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 5/6/2024 - */ -class ForwardTuner extends OpMode { - public static double DISTANCE = 48; - - @Override - public void init() { - follower.setStartingPose(new Pose(72,72)); - follower.update(); - drawCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); - drawCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated multiplier and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Distance Moved: " + follower.getPose().getX()); - telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getX() / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - -/** - * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the - * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the - * robot's current distance in ticks to the specified distance in inches. So, to use this, run the - * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're - * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials - * and average the results. Then, input the multiplier into the strafe ticks to inches in your - * localizer of choice. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 2.0, 6/26/2025 - */ -class LateralTuner extends OpMode { - public static double DISTANCE = 48; - - @Override - public void init() { - follower.setStartingPose(new Pose(72,72)); - follower.update(); - drawCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); - drawCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated multiplier and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Distance Moved: " + follower.getPose().getY()); - telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); - telemetryM.debug("Multiplier: " + (DISTANCE / (follower.getPose().getY() / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - -/** - * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the - * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the - * robot's current angle in ticks to the specified angle in radians. So, to use this, run the - * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. - * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run - * multiple trials and average the results. Then, input the multiplier into the turning ticks to - * radians in your localizer of choice. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 5/6/2024 - */ -class TurnTuner extends OpMode { - public static double ANGLE = 2 * Math.PI; - - @Override - public void init() { - follower.setStartingPose(new Pose(72,72)); - follower.update(); - drawCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); - telemetryM.update(telemetry); - - drawCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated multiplier and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Total Angle: " + follower.getTotalHeading()); - telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); - telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - -/** - * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max - * power until it reaches some specified distance. It records the most recent velocities, and on - * reaching the end of the distance, it averages them and prints out the velocity obtained. It is - * recommended to run this multiple times on a full battery to get the best results. What this does - * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that - * empirically represents the direction your wheels actually prefer to go in, allowing for - * more accurate following. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 3/13/2024 - */ -class ForwardVelocityTuner extends OpMode { - private final ArrayList velocities = new ArrayList<>(); - public static double DISTANCE = 48; - public static double RECORD_NUMBER = 10; - - private boolean end; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.debug("pose", follower.getPose()); - telemetryM.update(telemetry); - - follower.update(); - drawCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run forward at full power. */ - @Override - public void start() { - for (int i = 0; i < RECORD_NUMBER; i++) { - velocities.add(0.0); - } - follower.startTeleopDrive(true); - follower.update(); - end = false; - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B on - * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent - * velocities, and when the robot has run forward enough, these last velocities recorded are - * averaged and printed. - */ - @Override - public void loop() { - if (gamepad1.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - drawCurrentAndHistory(); - - - if (!end) { - if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { - end = true; - stopRobot(); - } else { - follower.setTeleOpDrive(1,0,0,true); - //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); - double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX()); - velocities.add(currentVelocity); - velocities.remove(0); - } - } else { - stopRobot(); - double average = 0; - for (double velocity : velocities) { - average += velocity; - } - average /= velocities.size(); - telemetryM.debug("Forward Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); - - for (int i = 0; i < velocities.size(); i++) { - telemetry.addData(String.valueOf(i), velocities.get(i)); - } - - telemetryM.update(telemetry); - telemetry.update(); - - if (gamepad1.aWasPressed()) { - follower.setXVelocity(average); - String message = "XMovement: " + average; - changes.add(message); - } - } - } -} - -/** - * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot right at max - * power until it reaches some specified distance. It records the most recent velocities, and on - * reaching the end of the distance, it averages them and prints out the velocity obtained. It is - * recommended to run this multiple times on a full battery to get the best results. What this does - * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that - * empirically represents the direction your wheels actually prefer to go in, allowing for - * more accurate following. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 3/13/2024 - */ -class LateralVelocityTuner extends OpMode { - private final ArrayList velocities = new ArrayList<>(); - - public static double DISTANCE = 48; - public static double RECORD_NUMBER = 10; - - private boolean end; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** - * This initializes the drive motors as well as the cache of velocities and the Panels - * telemetryM. - */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the left."); - telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); - telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); - - follower.update(); - drawCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run left at full power. */ - @Override - public void start() { - for (int i = 0; i < RECORD_NUMBER; i++) { - velocities.add(0.0); - } - follower.startTeleopDrive(true); - follower.update(); - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B on - * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent - * velocities, and when the robot has run sideways enough, these last velocities recorded are - * averaged and printed. - */ - @Override - public void loop() { - if (gamepad1.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - drawCurrentAndHistory(); - - if (!end) { - if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { - end = true; - stopRobot(); - } else { - follower.setTeleOpDrive(0,1,0,true); - double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2))); - velocities.add(currentVelocity); - velocities.remove(0); - } - } else { - stopRobot(); - double average = 0; - for (double velocity : velocities) { - average += velocity; - } - average /= velocities.size(); - - telemetryM.debug("Strafe Velocity: " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); - telemetryM.update(telemetry); - - if (gamepad1.aWasPressed()) { - follower.setYVelocity(average); - String message = "YMovement: " + average; - changes.add(message); - } - } - } -} - -/** - * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot - * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting - * them to zero power. The deceleration, or negative acceleration, is then measured until the robot - * stops. The accelerations across the entire time the robot is slowing down is then averaged and - * that number is then printed. This is used to determine how the robot will decelerate in the - * forward direction when power is cut, making the estimations used in the calculations for the - * drive Vector more accurate and giving better braking at the end of Paths. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/13/2024 - */ -class ForwardZeroPowerAccelerationTuner extends OpMode { - private final ArrayList accelerations = new ArrayList<>(); - public static double VELOCITY = 30; - - private double previousVelocity; - private long previousTimeNano; - - private boolean stopping; - private boolean end; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** This initializes the drive motors as well as the Panels telemetryM. */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on Gamepad 1 to stop."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run forward at full power. */ - @Override - public void start() { - follower.startTeleopDrive(false); - follower.update(); - follower.setTeleOpDrive(1,0,0,true); - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B on - * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will - * record its deceleration / negative acceleration until it stops. Then, it will average all the - * recorded deceleration / negative acceleration and print that value. - */ - @Override - public void loop() { - if (gamepad1.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - drawCurrentAndHistory(); - - Vector heading = new Vector(1.0, follower.getPose().getHeading()); - if (!end) { - if (!stopping) { - if (follower.getVelocity().dot(heading) > VELOCITY) { - previousVelocity = follower.getVelocity().dot(heading); - previousTimeNano = System.nanoTime(); - stopping = true; - follower.setTeleOpDrive(0,0,0,true); - } - } else { - double currentVelocity = follower.getVelocity().dot(heading); - accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); - previousVelocity = currentVelocity; - previousTimeNano = System.nanoTime(); - if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { - end = true; - } - } - } else { - double average = 0; - for (double acceleration : accelerations) { - average += acceleration; - } - average /= accelerations.size(); - - telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); - - if (gamepad1.aWasPressed()) { - follower.getConstants().setForwardZeroPowerAcceleration(average); - String message = "Forward Zero Power Acceleration: " + average; - changes.add(message); - } - } - } -} - -/** - * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot - * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting - * them to zero power. The deceleration, or negative acceleration, is then measured until the robot - * stops. The accelerations across the entire time the robot is slowing down is then averaged and - * that number is then printed. This is used to determine how the robot will decelerate in the - * forward direction when power is cut, making the estimations used in the calculations for the - * drive Vector more accurate and giving better braking at the end of Paths. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @author Baron Henderson - 20077 The Indubitables - * @version 1.0, 3/13/2024 - */ -class LateralZeroPowerAccelerationTuner extends OpMode { - private final ArrayList accelerations = new ArrayList<>(); - public static double VELOCITY = 30; - private double previousVelocity; - private long previousTimeNano; - private boolean stopping; - private boolean end; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** This initializes the drive motors as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second."); - telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - /** This starts the OpMode by setting the drive motors to run forward at full power. */ - @Override - public void start() { - follower.startTeleopDrive(false); - follower.update(); - follower.setTeleOpDrive(0,1,0,true); - } - - /** - * This runs the OpMode. At any point during the running of the OpMode, pressing B on - * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will - * record its deceleration / negative acceleration until it stops. Then, it will average all the - * recorded deceleration / negative acceleration and print that value. - */ - @Override - public void loop() { - if (gamepad1.bWasPressed()) { - stopRobot(); - requestOpModeStop(); - } - - follower.update(); - drawCurrentAndHistory(); - - Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2); - if (!end) { - if (!stopping) { - if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) { - previousVelocity = Math.abs(follower.getVelocity().dot(heading)); - previousTimeNano = System.nanoTime(); - stopping = true; - follower.setTeleOpDrive(0,0,0,true); - } - } else { - double currentVelocity = Math.abs(follower.getVelocity().dot(heading)); - accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); - previousVelocity = currentVelocity; - previousTimeNano = System.nanoTime(); - if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { - end = true; - } - } - } else { - double average = 0; - for (double acceleration : accelerations) { - average += acceleration; - } - average /= accelerations.size(); - - telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); - telemetryM.debug("\n"); - telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); - telemetryM.update(telemetry); - - if (gamepad1.aWasPressed()) { - follower.getConstants().setLateralZeroPowerAcceleration(average); - String message = "Lateral Zero Power Acceleration: " + average; - changes.add(message); - } - } - } -} - -/** - * This is the Predictive Braking Tuner. It runs the robot forward and backward at various power - * levels, recording the robot’s velocity and position immediately before braking. The motors are - * then set to a reverse power, which represents the fastest theoretical braking the robot - * can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance. - * Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a - * quadratic curve to model the braking behavior. - * - * @author Ashay Sarda - 19745 Turtle Walkers - * @author Jacob Ophoven - 18535 Frozen Code - * @version 1.0, 12/26/2025 - */ -class PredictiveBrakingTuner extends OpMode { - private static final double[] TEST_POWERS = - {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; - private static final double BRAKING_POWER = -0.2; - - private static final int DRIVE_TIME_MS = 1000; - - private enum State { - START_MOVE, - WAIT_DRIVE_TIME, - APPLY_BRAKE, - WAIT_BRAKE_TIME, - RECORD, - DONE - } - - private static class BrakeRecord { - double timeMs; - Pose pose; - double velocity; - - BrakeRecord(double timeMs, Pose pose, double velocity) { - this.timeMs = timeMs; - this.pose = pose; - this.velocity = velocity; - } - } - - private State state = State.START_MOVE; - - private final ElapsedTime timer = new ElapsedTime(); - - private int iteration = 0; - - private Vector startPosition; - private double measuredVelocity; - - private final List velocityToBrakingDistance = new ArrayList<>(); - private final List brakeData = new ArrayList<>(); - - @Override - public void init() {} - - @Override - public void init_loop() { - telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down."); - telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet."); - telemetryM.debug("After stopping, kFriction and kBraking will be displayed."); - telemetryM.debug("Make sure to turn the timer off."); - telemetryM.debug("Press B on game pad 1 to stop."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - timer.reset(); - follower.update(); - follower.startTeleOpDrive(true); - } - - @SuppressLint("DefaultLocale") - @Override - public void loop() { - follower.update(); - - if (gamepad1.b) { - stopRobot(); - requestOpModeStop(); - return; - } - - double direction = (iteration % 2 == 0) ? 1 : -1; - - switch (state) { - case START_MOVE: { - if (iteration >= TEST_POWERS.length) { - state = State.DONE; - break; - } - - double currentPower = TEST_POWERS[iteration]; - follower.setMaxPower(currentPower); - follower.setTeleOpDrive(direction, 0, 0, true); - - timer.reset(); - state = State.WAIT_DRIVE_TIME; - break; - } - - case WAIT_DRIVE_TIME: { - if (timer.milliseconds() >= DRIVE_TIME_MS) { - measuredVelocity = follower.getVelocity().getMagnitude(); - startPosition = follower.getPose().getAsVector(); - state = State.APPLY_BRAKE; - } - break; - } - - case APPLY_BRAKE: { - follower.setTeleOpDrive(BRAKING_POWER * direction, 0, 0, true); - - timer.reset(); - state = State.WAIT_BRAKE_TIME; - break; - } - - case WAIT_BRAKE_TIME: { - double t = timer.milliseconds(); - Pose currentPose = follower.getPose(); - double currentVelocity = follower.getVelocity().getMagnitude(); - - brakeData.add(new BrakeRecord(t, currentPose, currentVelocity)); - - if (follower.getVelocity().dot(new Vector(direction, - follower.getHeading())) <= 0) { - state = State.RECORD; - } - break; - } - - case RECORD: { - Vector endPosition = follower.getPose().getAsVector(); - double brakingDistance = endPosition.minus(startPosition).getMagnitude(); - - velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); - - telemetryM.debug("Test " + iteration, - String.format("v=%.3f d=%.3f", measuredVelocity, - brakingDistance)); - telemetryM.update(telemetry); - - iteration++; - state = State.START_MOVE; - - break; - } - - case DONE: { - stopRobot(); - - double[] coefficients = quadraticFit(velocityToBrakingDistance); - - telemetryM.debug("Tuning Complete"); - telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadratic", coefficients[1]); - telemetryM.debug("kLinear", coefficients[0]); - telemetryM.update(telemetry); - telemetryM.debug("Tuning Complete"); - telemetryM.debug("Braking Profile:"); - telemetryM.debug("kQuadraticFriction", coefficients[1]); - telemetryM.debug("kLinearBraking", coefficients[0]); - for (BrakeRecord record : brakeData) { - Pose p = record.pose; - telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", - record.timeMs, p.getX(), p.getY(), - p.getHeading(), - record.velocity)); - } - telemetryM.update(); - break; - } - } - } -} - -/** - * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. - * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/12/2024 - */ -class TranslationalTuner extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** This initializes the Follower and creates the forward and backward Paths. */ - @Override - public void init_loop() { - telemetryM.debug("This will activate the translational PIDF(s)"); - telemetryM.debug("The robot will try to stay in place while you push it laterally."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.deactivateAllPIDFs(); - follower.activateTranslational(); - forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); - forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); - backwards.setConstantHeadingInterpolation(0); - follower.followPath(forwards); - } - - /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); - telemetryM.addData("Zero Line", 0); - telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); - telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); - telemetryM.update(telemetry); - } -} - -/** - * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. - * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. - * It will try to keep the robot at a constant heading while the user tries to turn it. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/12/2024 - */ -class HeadingTuner extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** - * This initializes the Follower and creates the forward and backward Paths. Additionally, this - * initializes the Panels telemetry. - */ - @Override - public void init_loop() { - telemetryM.debug("This will activate the heading PIDF(s)."); - telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.deactivateAllPIDFs(); - follower.activateHeading(); - forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); - forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); - backwards.setConstantHeadingInterpolation(0); - follower.followPath(forwards); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); - telemetryM.addData("Zero Line", 0); - telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); - telemetryM.update(telemetry); - } -} - -/** - * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/12/2024 - */ -class DriveTuner extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private PathChain forwards; - private PathChain backwards; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** - * This initializes the Follower and creates the forward and backward Paths. Additionally, this - * initializes the Panels telemetry. - */ - @Override - public void init_loop() { - telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); - telemetryM.debug("The robot will go forward and backward continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.deactivateAllPIDFs(); - follower.activateDrive(); - - forwards = follower.pathBuilder() - .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) - .setConstantHeadingInterpolation(0) - .build(); - - backwards = follower.pathBuilder() - .setGlobalDeceleration() - .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) - .setConstantHeadingInterpolation(0) - .build(); - - follower.followPath(forwards); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Driving forward?: " + forward); - telemetryM.addData("Zero Line", 0); - telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); - telemetryM.update(telemetry); - } -} - -/** - * This is the Line Test Tuner OpMode. It will drive the robot forward and back - * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/12/2024 - */ -class Line extends OpMode { - public static double DISTANCE = 40; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** This initializes the Follower and creates the forward and backward Paths. */ - @Override - public void init_loop() { - telemetryM.debug("This will activate all the PIDF(s)"); - telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); - telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.activateAllPIDFs(); - forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); - forwards.setConstantHeadingInterpolation(0); - backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); - backwards.setConstantHeadingInterpolation(0); - follower.followPath(forwards); - } - - /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Driving Forward?: " + forward); - telemetryM.update(telemetry); - } -} - -/** - * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance - * forward and to the left. On reaching the end of the forward Path, the robot runs the backward - * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety - * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the - * centripetal Vector. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/13/2024 - */ -class CentripetalTuner extends OpMode { - public static double DISTANCE = 20; - private boolean forward = true; - - private Path forwards; - private Path backwards; - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** - * This initializes the Follower and creates the forward and backward Paths. - * Additionally, this initializes the Panels telemetry. - */ - @Override - public void init_loop() { - telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); - telemetryM.debug("The robot will go continuously along the path."); - telemetryM.debug("Make sure you have enough room."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.activateAllPIDFs(); - forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); - backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); - - backwards.setTangentHeadingInterpolation(); - backwards.reverseHeadingInterpolation(); - - follower.followPath(forwards); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - if (!follower.isBusy()) { - if (forward) { - forward = false; - follower.followPath(backwards); - } else { - forward = true; - follower.followPath(forwards); - } - } - - telemetryM.debug("Driving away from the origin along the curve?: " + forward); - telemetryM.update(telemetry); - } -} - -/** - * This is the Triangle autonomous OpMode. - * It runs the robot in a triangle, with the starting point being the bottom-middle point. - * - * @author Baron Henderson - 20077 The Indubitables - * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge - * @version 1.0, 12/30/2024 - */ -class Triangle extends OpMode { - - private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); - private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); - private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); - - private PathChain triangle; - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the Panels. - */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - - if (follower.atParametricEnd()) { - follower.followPath(triangle, true); - } - } - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - @Override - public void init_loop() { - telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); - telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - /** Creates the PathChain for the "triangle".*/ - @Override - public void start() { - follower.setStartingPose(startPose); - - triangle = follower.pathBuilder() - .addPath(new BezierLine(startPose, interPose)) - .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading()) - .addPath(new BezierLine(interPose, endPose)) - .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading()) - .addPath(new BezierLine(endPose, startPose)) - .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading()) - .build(); - - follower.followPath(triangle); - } -} - -/** - * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite - * a circle, but some Bezier curves that have control points set essentially in a square. However, - * it turns enough to tune your centripetal force correction and some of your heading. Some lag in - * heading is to be expected. - * - * @author Anyi Lin - 10158 Scott's Bots - * @author Aaron Yang - 10158 Scott's Bots - * @author Harrison Womack - 10158 Scott's Bots - * @version 1.0, 3/12/2024 - */ -class Circle extends OpMode { - public static double RADIUS = 10; - private PathChain circle; - - public void start() { - circle = follower.pathBuilder() - .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) - .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) - .build(); - follower.followPath(circle); - } - - @Override - public void init_loop() { - telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); - telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); - telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void init() { - follower.setStartingPose(new Pose(72, 72)); - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the FTC Dashboard. - */ - @Override - public void loop() { - follower.update(); - drawCurrentAndHistory(); - - if (follower.atParametricEnd()) { - follower.followPath(circle); - } - } -} - -/** - * Tuning OpMode to get the min and max encoder values for swerve pods - * @author Kabir Goyal - */ -class AnalogMinMaxTuner extends OpMode { - //populate the below with your names for the servos and encoders - public String[] encoderNames = {"leftFrontEncoder", "rightFrontEncoder", "leftBackEncoder", "rightBackEncoder"}; - public AnalogInput[] encoders = new AnalogInput[encoderNames.length]; - public double[] minVoltages = new double[encoderNames.length]; - public double[] maxVoltages = new double[encoderNames.length]; - - public List lynxModules; //js to improve loop times a bit yk - - public void start() { - } - - @Override - public void init_loop() { - telemetryM.debug("Press START. Then, Spin each pod slowly for 4 to 5 full rotations.\n" + - "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry."); - telemetryM.update(telemetry); - } - - @Override - public void init() { - lynxModules = hardwareMap.getAll(LynxModule.class); - for (LynxModule hub : lynxModules) { - hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); - } - - for (int i = 0; i < encoders.length; i++) { - encoders[i] = hardwareMap.get(AnalogInput.class, encoderNames[i]); - minVoltages[i] = 5; //bigger value than should ever be read - } - } - - /** - * This runs the OpMode, updating the Follower as well as printing out the debug statements to - * the Telemetry, as well as the FTC Dashboard. - */ - @Override - public void loop() { - for (LynxModule hub : lynxModules) { - hub.clearBulkCache(); - } - - telemetryM.debug("Spin each pod slowly for 4 to 5 full rotations.\n" + - "The OpMode will keep track of the min and max voltages seen so far and print them to telemetry.\n\n"); - - for (int i = 0; i < encoders.length; i++) { - double currentVoltage = encoders[i].getVoltage(); - minVoltages[i] = Math.min(minVoltages[i], currentVoltage); - maxVoltages[i] = Math.max(maxVoltages[i], currentVoltage); - telemetryM.addData(encoderNames[i] + "min value:", minVoltages[i]); - telemetryM.addData(encoderNames[i] + "max value:", maxVoltages[i]); - telemetryM.addLine(""); - } - - telemetryM.update(); - } -} - -/** - * This is the SwerveOffsetsTest - * You should use this to check how good your swerve angle offsets are and if your motor directions are correct - * @author Kabir Goyal - * - */ -class SwerveOffsetsTest extends OpMode { - boolean debugStringEnabled = false; - - @Override - public void init() {} - - /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ - @Override - public void init_loop() { - if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { - debugStringEnabled = !debugStringEnabled; - } - - - telemetryM.debug("This OpMode will run all four swerve pods in the direction they think is forward" - + "\nensure your bot is not on the ground while running"); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + - " (press gamepad a to toggle)"); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.startTeleopDrive(); - follower.update(); - } - - /** - * This updates the robot's pose estimate, the simple drive, and updates the - * Panels telemetry with the robot's position as well as draws the robot's position. - */ - @Override - public void loop() { - if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { - debugStringEnabled = !debugStringEnabled; - } - - follower.setTeleOpDrive(0.25, 0, 0, true); - follower.update(); - - if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + - follower.getDrivetrain().debugString()); - } - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - -/** - * This is the SwerveTurnTest - * You should use this to check your encoder directions and x/y pod offsets - * @author Kabir Goyal - * - */ -class SwerveTurnTest extends OpMode { - boolean debugStringEnabled = false; - - @Override - public void init() {} - - /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ - @Override - public void init_loop() { - if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { - debugStringEnabled = !debugStringEnabled; - } - - - telemetryM.debug("This OpMode will run all four swerve pods in their turning direction (perpendicular to the center of the robot) " - + "\nrun this once off the ground to check servo directions and motor directions before testing on the ground"); - telemetryM.debug("Drivetrain debug string " + (((debugStringEnabled) ? "enabled" : "disabled")) + - " (press gamepad a to toggle)"); - telemetryM.update(telemetry); - follower.update(); - drawCurrent(); - } - - @Override - public void start() { - follower.startTeleopDrive(); - follower.update(); - } - - /** - * This updates the robot's pose estimate, the simple drive, and updates the - * Panels telemetry with the robot's position as well as draws the robot's position. - */ - @Override - public void loop() { - if (gamepad1.aWasPressed() || gamepad2.aWasPressed()) { - debugStringEnabled = !debugStringEnabled; - } - - follower.setTeleOpDrive(0, 0, 0.25, true); - follower.update(); - - if (debugStringEnabled) { - telemetryM.debug("Drivetrain Debug String:\n" + - follower.getDrivetrain().debugString()); - } - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - -/** - * This is the OffsetsTuner OpMode. This tracks the movement of the robot as it turns 180 degrees, - * and calculates what the robot's strafeX and forwardY offsets should be. Ensure that your strafeX and forwardY offsets - * are set to 0 before running this OpMode. After running, input the displayed offsets into your localizer constants. - * - * @author Havish Sripada - 12808 RevAmped Robotics - * @author Baron Henderson - */ -class OffsetsTuner extends OpMode { - @Override - public void init() { - follower.setStartingPose(new Pose(72,72)); - follower.update(); - drawCurrent(); - } - - /** This initializes the PoseUpdater as well as the Panels telemetry. */ - @Override - public void init_loop() { - telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); - telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); - telemetryM.update(telemetry); - - drawCurrent(); - } - - /** - * This updates the robot's pose estimate, and updates the Panels telemetry with the - * calculated offsets and draws the robot. - */ - @Override - public void loop() { - follower.update(); - - telemetryM.debug("Total Angle: " + follower.getTotalHeading()); - - telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer."); - telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); - telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); - telemetryM.update(telemetry); - - drawCurrentAndHistory(); - } -} - - -/** - * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. - * - * @author Lazar - 19234 - * @version 1.1, 5/19/2025 - */ -class Drawing { - public static final double ROBOT_RADIUS = 9; // woah - private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); - - private static final Style robotLook = new Style( - "", "#3F51B5", 0.75 - ); - private static final Style historyLook = new Style( - "", "#4CAF50", 0.75 - ); - - /** - * This prepares Panels Field for using Pedro Offsets - */ - public static void init() { - panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); - } - - /** - * This draws everything that will be used in the Follower's telemetryDebug() method. This takes - * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. - * - * @param follower Pedro Follower instance. - */ - public static void drawDebug(Follower follower) { - if (follower.getCurrentPath() != null) { - drawPath(follower.getCurrentPath(), robotLook); - Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); - drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); - } - drawPoseHistory(follower.getPoseHistory(), historyLook); - drawRobot(follower.getPose(), historyLook); - - sendPacket(); - } - - /** - * This draws a robot at a specified Pose with a specified - * look. The heading is represented as a line. - * - * @param pose the Pose to draw the robot at - * @param style the parameters used to draw the robot with - */ - public static void drawRobot(Pose pose, Style style) { - if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { - return; - } - - panelsField.setStyle(style); - panelsField.moveCursor(pose.getX(), pose.getY()); - panelsField.circle(ROBOT_RADIUS); - - Vector v = pose.getHeadingAsUnitVector(); - v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); - double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; - double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); - - panelsField.setStyle(style); - panelsField.moveCursor(x1, y1); - panelsField.line(x2, y2); - } - - /** - * This draws a robot at a specified Pose. The heading is represented as a line. - * - * @param pose the Pose to draw the robot at - */ - public static void drawRobot(Pose pose) { - drawRobot(pose, robotLook); - } - - /** - * This draws a Path with a specified look. - * - * @param path the Path to draw - * @param style the parameters used to draw the Path with - */ - public static void drawPath(Path path, Style style) { - double[][] points = path.getPanelsDrawingPoints(); - - for (int i = 0; i < points[0].length; i++) { - for (int j = 0; j < points.length; j++) { - if (Double.isNaN(points[j][i])) { - points[j][i] = 0; - } - } - } - - panelsField.setStyle(style); - panelsField.moveCursor(points[0][0], points[0][1]); - panelsField.line(points[1][0], points[1][1]); - } - - /** - * This draws all the Paths in a PathChain with a - * specified look. - * - * @param pathChain the PathChain to draw - * @param style the parameters used to draw the PathChain with - */ - public static void drawPath(PathChain pathChain, Style style) { - for (int i = 0; i < pathChain.size(); i++) { - drawPath(pathChain.getPath(i), style); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - * @param style the parameters used to draw the pose history with - */ - public static void drawPoseHistory(PoseHistory poseTracker, Style style) { - panelsField.setStyle(style); - - int size = poseTracker.getXPositionsArray().length; - for (int i = 0; i < size - 1; i++) { - - panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); - panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); - } - } - - /** - * This draws the pose history of the robot. - * - * @param poseTracker the PoseHistory to get the pose history from - */ - public static void drawPoseHistory(PoseHistory poseTracker) { - drawPoseHistory(poseTracker, historyLook); - } - - /** - * This tries to send the current packet to FTControl Panels. - */ - public static void sendPacket() { - panelsField.update(); - } -} \ No newline at end of file diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 931502f..3b08fca 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,7 @@ repositories { mavenCentral() google() // Needed for androidx + maven { url 'https://repo.dairy.foundation/releases/'} maven { url = "https://mymaven.bylazar.com/releases" } } @@ -16,8 +17,9 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:11.2.1' //noinspection GradleDependency implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.pedropathing:ftc:2.1.2' - implementation 'com.pedropathing:telemetry:1.0.0' + implementation 'com.bylazar:fullpanels:1.0.12' + implementation 'com.pedropathing:revhub:3.0.0' + implementation 'com.pedropathing:tuning:1.0.0' }