From 3319e934c108be3cc4a9a8d732fdd4225e23f297 Mon Sep 17 00:00:00 2001 From: Anirudh Narayanan <109127469+ripark2@users.noreply.github.com> Date: Thu, 19 Feb 2026 19:52:09 -0600 Subject: [PATCH] a --- .../ftc/teamcode/Auton/redDepot.java | 303 ++++++++++++++---- ...trolsV1.java => DriverControlsV1Blue.java} | 29 +- .../ftc/teamcode/DriverControlsV1Red.java | 54 +++- 3 files changed, 308 insertions(+), 78 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{DriverControlsV1.java => DriverControlsV1Blue.java} (90%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java index 04b293a..1159cfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java @@ -1,110 +1,285 @@ -package org.firstinspires.ftc.teamcode.Auton; +package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.util.PersonalPID; @Config -@Autonomous(name = "Clean Red Depot") +@Autonomous(name = "Blue Depot Cycle") public class redDepot extends LinearOpMode { Hware robot = new Hware(); public static double TICKS_PER_DEGREE = 9.738888 * 0.29969; - // --- Action Helpers --- - public void setTurret(int degrees) { - robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE)); + //turret + public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; + public static double target = 0; + + public static double ticksperdegree = 9.738888 * 0.29969; + + public int minTicks = (int) (ticksperdegree * (-90)); + public int maxTicks = (int) (ticksperdegree * (75)); + PersonalPID pid; + Limelight3A limelight; + LLResult result; + + boolean lock = true; + + public void turretPower(double power) { + robot.turret.setPower(power); + } + + public void turretPos(int position) { + turretPower(0); + robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.turret.setPower(1.0); + turretPower(1); } - public void startShooter(double velocity) { - robot.topFly.setVelocity(velocity); - robot.bottomFly.setVelocity(velocity); - robot.launchHood.setPosition(0.54); + public void spintakeStart(){ + robot.intake.set(1); } - public void stopEverything() { -// robot.intake.set(0); -// robot.topFly.setVelocity(0); -// robot.bottomFly.setVelocity(0); -// robot.activeTransfer.setPosition(0.8); + public void spintakeEnd(){ + robot.intake.set(0); } - @Override + public void shoot(){ + robot.topFly.setVelocity(1800); + robot.bottomFly.setVelocity(1800); + robot.launchHood.setPosition(0.55); + } + + public void reset(){ + robot.intake.set(0); +// robot.topFly.setVelocity(1200); +// robot.bottomFly.setVelocity(1200); +// robot.topFly.set(0); +// robot.bottomFly.set(0); +// robot.launchHood.setPosition(0.6); + robot.activeTransfer.setPosition(1); + } + + public void runOpMode() throws InterruptedException { + + //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); robot.initialize(hardwareMap); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + pid = new PersonalPID(p, i, d, f); + limelight.pipelineSwitch(1); + limelight.start(); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + pid = new PersonalPID(p, i, d, f); + + Pose2d startPose = new Pose2d(54, -46, Math.toRadians(-52)); - Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205)); drive.setPoseEstimate(startPose); - // --- TRAJECTORY 1: Initial Score --- - TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose) - .addDisplacementMarker(() -> { - setTurret(-5); - startShooter(1660); - robot.intake.set(.8); + TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); +// turretPos(5); }) - .strafeRight(35) - .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0)) + .UNSTABLE_addTemporalMarkerOffset(.3, () -> { + shoot(); + }) + .back(47) + .waitSeconds(0.5) + .UNSTABLE_addTemporalMarkerOffset(0.3, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.85); + }) + +// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +// robot.launchHood.setPosition(0.5); +// }) .waitSeconds(1.5) - .addDisplacementMarker(this::stopEverything) - .build(); + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) - // --- TRAJECTORY 2: Intake 6-Ball --- - TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end()) - .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265))) - .addDisplacementMarker(() -> robot.intake.set(1.0)) - .forward(35) - .addDisplacementMarker(() -> robot.intake.set(0)) - .build(); + //6ball + .lineToLinearHeading(new Pose2d(8,-28, Math.toRadians(-90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + shoot(); + }) + .lineToLinearHeading(new Pose2d(24,-14, Math.toRadians(-52))) + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.85); + }) - // --- TRAJECTORY 3: Score 6-Ball --- - TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end()) - .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205))) - .addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through - .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0)) +// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +// robot.launchHood.setPosition(0.5); +// }) .waitSeconds(1.5) - .addDisplacementMarker(this::stopEverything) - .build(); - // --- TRAJECTORY 4: Intake 9-Ball --- - TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end()) - .lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265))) - .addDisplacementMarker(() -> robot.intake.set(1.0)) - .forward(35) - .addDisplacementMarker(() -> robot.intake.set(0)) - .build(); - - // --- TRAJECTORY 5: Score 9-Ball & Park --- - TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end()) - .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205))) - .addDisplacementMarker(() -> { - startShooter(1660); - robot.activeTransfer.setPosition(1.0); + //9ball + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .lineToLinearHeading(new Pose2d(-19,-27, Math.toRadians(-90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + }) + .lineToLinearHeading(new Pose2d(-12,-54, Math.toRadians(-90))) + .lineToLinearHeading(new Pose2d(26,-14, Math.toRadians(-52))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.85); }) .waitSeconds(1.5) - .addDisplacementMarker(this::stopEverything) - .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90))) + + //12ball + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .lineToLinearHeading(new Pose2d(-40,-22, Math.toRadians(-90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + }) + .lineToLinearHeading(new Pose2d(35,-14, Math.toRadians(-52))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + shoot(); + }) + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(0.8); + robot.activeTransfer.setPosition(0.85); + }) + .waitSeconds(1.5) + // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45))) + +// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> { +// turretPos(84); +// }) + .waitSeconds(1) .build(); +// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose) +// .strafeLeft(25) +// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// shoot(); +// }) +// .waitSeconds(1) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// shoot(); +// }) +// .waitSeconds(1) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .forward(10) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .strafeLeft(10) +// .forward(3) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// shoot(); +// }) +// .waitSeconds(1) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeStart(); +// }) +// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), +// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// spintakeEnd(); +// }) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// shoot(); +// }) +// .waitSeconds(1) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); +// }) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .build(); + waitForStart(); + if (opModeIsActive() && !isStopRequested()) { +// while (opModeIsActive()) { + drive.followTrajectorySequenceAsync(threeBall); + PoseStorage.currentPose = drive.getPoseEstimate(); - if (isStopRequested()) return; + while(opModeIsActive()){ + robot.leftPTO.setPosition(0.2); + robot.rightPTO.setPosition(0.14); + drive.update(); + result = limelight.getLatestResult(); + double vision = result.getTx() + 3; - // Execute sequentially - drive.followTrajectorySequence(traj1); - drive.followTrajectorySequence(traj2); - drive.followTrajectorySequence(traj3); - drive.followTrajectorySequence(traj4); - drive.followTrajectorySequence(traj5); + double target = ticksperdegree * vision; + double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target); + pid.setPIDF(p, i, d, f); + + if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){ + targetTicks = targetTicks; + } else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){ + targetTicks = targetTicks; + + } else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){ + targetTicks = 0; + } + PoseStorage.currentPose = drive.getPoseEstimate(); + robot.turret.setPower(- targetTicks); + } +// } + } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Blue.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Blue.java index 368c3f3..c5301d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Blue.java @@ -18,7 +18,7 @@ import org.firstinspires.ftc.teamcode.util.PersonalPID; @Config @TeleOp(name = "TeleOp BLUE - Final Optimized") -public class DriverControlsV1 extends LinearOpMode { +public class DriverControlsV1Blue extends LinearOpMode { // --- DASHBOARD TUNING --- public static double flywheelTuner = 1.0; @@ -34,7 +34,7 @@ public class DriverControlsV1 extends LinearOpMode { private double lastTuner = -1.0; private double lastPTOPosition = -1.0; - enum ShootState { IDLE, START_SHOOT, RESET } + enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR } //turret public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; @@ -44,6 +44,7 @@ public class DriverControlsV1 extends LinearOpMode { public int minTicks = (int) (ticksperdegree * (-90)); public int maxTicks = (int) (ticksperdegree * (75)); + public int offsetCam = 3; PersonalPID pid; Limelight3A limelight; LLResult result; @@ -81,7 +82,7 @@ public class DriverControlsV1 extends LinearOpMode { while (opModeIsActive()) { result = limelight.getLatestResult(); - double vision = result.getTx()-3; + double vision = result.getTx()-offsetCam; // 1. INPUTS & TURRET // if (gamepad2.yWasPressed()) { @@ -178,6 +179,12 @@ public class DriverControlsV1 extends LinearOpMode { shootTimer.reset(); } + if (gamepad2.dpadLeftWasPressed()) { + shooting = true; + shootStatus = ShootState.START_SHOOT_FAR; + shootTimer.reset(); + } + if (gamepad2.yWasPressed()) { flywheelTuner+=.01; } @@ -186,6 +193,12 @@ public class DriverControlsV1 extends LinearOpMode { flywheelTuner-=.01; } + if (gamepad2.xWasPressed()) { + offsetCam = 0; + } else if (gamepad2.bWasPressed()) { + offsetCam = 3; + } + switch (shootStatus) { case START_SHOOT: robot.activeTransfer.setPosition(0.85); @@ -195,6 +208,16 @@ public class DriverControlsV1 extends LinearOpMode { shootStatus = ShootState.RESET; } break; + + case START_SHOOT_FAR: + robot.activeTransfer.setPosition(0.85); + robot.intake.set(.6); + targetTicks = 0; + if (shootTimer.milliseconds() > 2000) { + shootStatus = ShootState.RESET; + } + break; + case RESET: if (shootTimer.milliseconds() > 2500) { shootStatus = ShootState.IDLE; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java index 118271c..32a0f3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java @@ -17,7 +17,7 @@ import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; import org.firstinspires.ftc.teamcode.util.PersonalPID; @Config -@TeleOp(name = "TeleOp RED - Final Optimized") +@TeleOp(name = "TeleOp BLUE - Final Optimized") public class DriverControlsV1Red extends LinearOpMode { // --- DASHBOARD TUNING --- @@ -27,14 +27,14 @@ public class DriverControlsV1Red extends LinearOpMode { public static double HOOD_MIN = 0.3; public static double HOOD_MAX = 1.0; - public static double HOOD_CORRECTION_SENSITIVITY = 0.04; + public static double HOOD_CORRECTION_SENSITIVITY = 0.01; // ------------------------- // State tracking to prevent redundant hardware writes (The Fix) private double lastTuner = -1.0; private double lastPTOPosition = -1.0; - enum ShootState { IDLE, START_SHOOT, RESET } + enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR } //turret public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; @@ -44,6 +44,7 @@ public class DriverControlsV1Red extends LinearOpMode { public int minTicks = (int) (ticksperdegree * (-90)); public int maxTicks = (int) (ticksperdegree * (75)); + public int offsetCam = 3; PersonalPID pid; Limelight3A limelight; LLResult result; @@ -58,7 +59,7 @@ public class DriverControlsV1Red extends LinearOpMode { SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose); - ShooterSubsystemRed shooterMap = new ShooterSubsystemRed(); + ShooterSubsystem shooterMap = new ShooterSubsystem(); ShootState shootStatus = ShootState.IDLE; ElapsedTime shootTimer = new ElapsedTime(); @@ -81,7 +82,7 @@ public class DriverControlsV1Red extends LinearOpMode { while (opModeIsActive()) { result = limelight.getLatestResult(); - double vision = result.getTx(); + double vision = result.getTx()+offsetCam; // 1. INPUTS & TURRET // if (gamepad2.yWasPressed()) { @@ -104,7 +105,7 @@ public class DriverControlsV1Red extends LinearOpMode { // 2. PTO OPTIMIZATION (Only write if value is new) - double targetPTO = 0.2; + double targetPTO = 0.15; if (targetPTO != lastPTOPosition) { robot.leftPTO.setPosition(targetPTO); robot.rightPTO.setPosition(targetPTO); @@ -156,7 +157,7 @@ public class DriverControlsV1Red extends LinearOpMode { double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0; double velocityError = targetVelocity - currentVel; - double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY; + double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY; double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX); if (gamepad2.dpad_up) adjust = true; @@ -167,8 +168,8 @@ public class DriverControlsV1Red extends LinearOpMode { robot.topFly.setVelocity(targetVelocity); robot.launchHood.setPosition(correctedHood); } else { - robot.bottomFly.setVelocity(0); - robot.topFly.setVelocity(0); + robot.bottomFly.setVelocity(1700); + robot.topFly.setVelocity(1700); } // 7. SHOOTING STATE MACHINE @@ -178,15 +179,45 @@ public class DriverControlsV1Red extends LinearOpMode { shootTimer.reset(); } + if (gamepad2.dpadLeftWasPressed()) { + shooting = true; + shootStatus = ShootState.START_SHOOT_FAR; + shootTimer.reset(); + } + + if (gamepad2.yWasPressed()) { + flywheelTuner+=.01; + } + + if (gamepad2.aWasPressed()) { + flywheelTuner-=.01; + } + + if (gamepad2.xWasPressed()) { + offsetCam = 0; + } else if (gamepad2.bWasPressed()) { + offsetCam = 3; + } + switch (shootStatus) { case START_SHOOT: - robot.activeTransfer.setPosition(0.81); - robot.intake.set(1.0); + robot.activeTransfer.setPosition(0.85); + robot.intake.set(1); targetTicks = 0; if (shootTimer.milliseconds() > 2000) { shootStatus = ShootState.RESET; } break; + + case START_SHOOT_FAR: + robot.activeTransfer.setPosition(0.85); + robot.intake.set(.6); + targetTicks = 0; + if (shootTimer.milliseconds() > 2000) { + shootStatus = ShootState.RESET; + } + break; + case RESET: if (shootTimer.milliseconds() > 2500) { shootStatus = ShootState.IDLE; @@ -219,6 +250,7 @@ public class DriverControlsV1Red extends LinearOpMode { telemetry.addData("Status", "Running"); telemetry.addData("Vel Error", (int)velocityError); telemetry.addData("Hood", "%.2f", correctedHood); + telemetry.addData("TunerMult", flywheelTuner); telemetry.update(); } }