From 707fa0b68f2ea9922a40aa4f04a3bbac5243eac7 Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Mon, 12 Jan 2026 17:47:13 -0500 Subject: [PATCH 01/16] Tightened TeleOp, PedroPathing progress. --- .../TeleOp/TeleOpMecanumField2Drivers.java | 140 +- .../ftc/teamcode/pedroPathing/Constants.java | 77 + .../ftc/teamcode/pedroPathing/Tuning.java | 1362 +++++++++++++++++ .../ftc/teamcode/util/Shooter.java | 9 +- .../{Constants.java => TeamConstants.java} | 37 +- build.dependencies.gradle | 6 +- 6 files changed, 1540 insertions(+), 91 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/{Constants.java => TeamConstants.java} (69%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java index 81177be001f0..c2edef856973 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java @@ -12,52 +12,52 @@ import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.util.Constants; +import org.firstinspires.ftc.teamcode.util.TeamConstants; import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; import java.io.IOException; -import java.util.List; import java.util.Locale; @TeleOp(group = "Field Centric") public class TeleOpMecanumField2Drivers extends LinearOpMode { - private final static int CPR = Constants.CPR; - private final static double LOCK_ON_DENOMINATOR = Constants.LOCK_ON_DENOMINATOR; - private static IMU.Parameters parameters; + // Required for the DiagnosticLogger at the bottom of this file: + private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); + // --- One-time-only constants (non-@Config) --- + // These only update after a restart of the bot. private final static FtcDashboard DASH = FtcDashboard.getInstance(); - private final static TelemetryPacket packet = new TelemetryPacket(); + Telemetry telemetryD = DASH.getTelemetry(); + Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); + @Override public void runOpMode() { - Telemetry telemetryD = DASH.getTelemetry(); - Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); - + // These methods are only initialized when inside of runOpMode: telemetryD.setDisplayFormat(Telemetry.DisplayFormat.HTML); telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); - // Note to self: only use hardwareMap in or after the initialization! - DcMotorEx[] base = Constants.getDriveMotors(hardwareMap); - DcMotorEx[] scoring = Constants.getScoringMotors(hardwareMap); - IMU imu = Constants.getIMU(hardwareMap); - Limelight3A limelight = Constants.getLimelight(hardwareMap); + // --- @Config constants --- + final int CPR = TeamConstants.CPR; + final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; + final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; - parameters = Constants.getIMUParms(); - imu.initialize(parameters); + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + final IMU imu = TeamConstants.getIMU(hardwareMap); + final Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + // Final setup before control starts: + imu.initialize(parameters); limelight.start(); - boolean lockOn = false; - telemetryM.addLine("> Ready."); telemetryM.update(); @@ -75,20 +75,15 @@ public void runOpMode() logger.resumeRun(); loggerRuntimeThread.start(); - boolean pastLeftBumper = false; + boolean lockOn = false; + boolean pastLeftBumper1 = false; while(opModeIsActive()) { LLResult rawResult = limelight.getLatestResult(); - List fiducial = rawResult.getFiducialResults(); boolean resultIsValid = rawResult.isValid(); - boolean leftBumper = gamepad1.left_bumper; - double targetX; - - double y = -gamepad1.left_stick_y; - double x = gamepad1.left_stick_x*1.1; - double rx; + double rx, targetX; if(lockOn && resultIsValid) { targetX = rawResult.getTx(); @@ -97,37 +92,54 @@ public void runOpMode() // getTx returns a value in the range of -27.25 to 27.25. // We want to get a value from -1 to 1, so we can just divide // by 27.25. (27.25/27.25 = 1) - rx = targetX/LOCK_ON_DENOMINATOR; + rx = (targetX+LOCK_ON_OFFSET)/LOCK_ON_DENOMINATOR; } else { rx = gamepad1.right_stick_x; } + double driveBreakPower = 1-gamepad1.right_trigger; + + // Gamepad variables: + boolean a1 = gamepad1.a; + boolean a2 = gamepad2.a; + boolean b1 = gamepad1.b; + boolean x1 = gamepad1.x; + boolean x2 = gamepad2.x; + boolean y1 = gamepad1.y; + boolean y2 = gamepad2.y; + boolean leftBumper1 = gamepad1.left_bumper; + boolean rightBumper2 = gamepad2.right_bumper; + boolean start1 = gamepad1.start; + + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; - double brakePower = 1-gamepad1.right_trigger; // Max target RPM: 4900 - double bigHooperPower = 2500 + (gamepad2.left_trigger * 2400); + double hooperPower = 3000 + (gamepad2.left_trigger * 1900); - // --- Trigonometry Warning !!! --- (Continues for 14 lines) double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double botHeadingDegrees = botHeading * (180.0/Math.PI); - // Rotate the movement direction counter to the bot's rotation - double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); - double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); - - rotX = rotX * 1.1; + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; // Denominator is the largest motor power (absolute value) or 1 // This ensures all the powers maintain the same ratio, // but only if at least one is out of the range [-1, 1] - double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); double frontLeftPower = (rotY + rotX + rx) / denominator; double backLeftPower = (rotY - rotX + rx) / denominator; double frontRightPower = (rotY - rotX - rx) / denominator; double backRightPower = (rotY + rotX - rx) / denominator; // -- Runtime Telemetry -- - double bigHooperSpeed = (scoring[0].getVelocity()*60)/CPR; + double hooperSpeed = (scoring[0].getVelocity()*60)/CPR; String runtimeSeconds = String.format( Locale.ENGLISH, "%.4f", getRuntime() ); @@ -143,8 +155,8 @@ public void runOpMode() // runtimeSeconds shouldn't be graphable,so we make it a line // instead of data. (FtcDashboard graph already adds time as X) DASH.sendTelemetryPacket(packet); - telemetryD.addData("shooting-input-rpm", bigHooperPower); - telemetryD.addData("shooting-speed-rpm", bigHooperSpeed); + telemetryD.addData("shooting-input-rpm", hooperPower); + telemetryD.addData("shooting-speed-rpm", hooperSpeed); telemetryD.update(); telemetryD.addData("
", ""); telemetryD.update(); @@ -157,8 +169,8 @@ public void runOpMode() telemetry.addData("Runtime (Seconds)", runtimeSeconds); telemetry.addLine(); telemetry.addLine("== Shooting Motor (scoring[0]) =="); - telemetry.addData("shooting-input-rpm", bigHooperPower); - telemetry.addData("shooting-speed-rpm", bigHooperSpeed); + telemetry.addData("shooting-input-rpm", hooperPower); + telemetry.addData("shooting-speed-rpm", hooperSpeed); telemetry.addLine(); telemetry.addData("right-stick-x", rx); telemetry.addData("bot-heading", botHeadingDegrees); @@ -166,49 +178,59 @@ public void runOpMode() telemetry.addData("valid-target", resultIsValid); telemetry.update(); - if (gamepad1.a && gamepad1.b && gamepad1.x && gamepad1.y) + if (a1 && b1 && x1 && y1) { logger.stopRun(); } - if (gamepad1.start) + if (start1) { imu.resetYaw(); } // pastLeftBumper initializes to false - if(leftBumper && ! pastLeftBumper) + if(leftBumper1 && ! pastLeftBumper1) { // LockOn toggle lockOn = !lockOn; } - if(gamepad2.y) + if(y2) { scoring[0].setPower(-0.9); } else { - scoring[0].setVelocity((bigHooperPower / 60)*CPR); + scoring[0].setVelocity((hooperPower / 60)*CPR); } - if (gamepad2.a) - { - scoring[1].setPower(0.9); - } else if (gamepad2.x) + if (rightBumper2 && a2) { scoring[1].setPower(-0.9); + } else if (a2) + { + scoring[1].setPower(0.9); } else { scoring[1].setPower(0); } + if(rightBumper2 && x2) + { + scoring[2].setPower(-0.9); + } else if(x2) + { + scoring[2].setPower(0.9); + } else + { + scoring[2].setPower(0); + } - base[0].setPower(frontLeftPower*brakePower); - base[1].setPower(backLeftPower*brakePower); - base[2].setPower(backRightPower*brakePower); - base[3].setPower(frontRightPower*brakePower); + base[0].setPower(frontLeftPower*driveBreakPower); + base[1].setPower(backLeftPower*driveBreakPower); + base[2].setPower(backRightPower*driveBreakPower); + base[3].setPower(frontRightPower*driveBreakPower); - pastLeftBumper = leftBumper; + pastLeftBumper1 = leftBumper1; } } @@ -223,9 +245,9 @@ private DiagnosticLogger getLogger() null, new String[] { - "frontLeftMotor", "backLeftMotor", - "frontRightMotor", "backRightMotor", - "bigHooperMotor", "smallHooperMotor" + "fl", "rl", + "fr", "rr", + "hooper", "intake1", "intake2" }, null, "imu", parameters ); 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 new file mode 100644 index 000000000000..5fb1c65d1f2b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -0,0 +1,77 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.acmerobotics.dashboard.config.Config; +import com.pedropathing.ftc.localization.constants.DriveEncoderConstants; +import com.pedropathing.ftc.localization.Encoder; + +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; + +import com.pedropathing.paths.PathConstraints; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +@Config +public class Constants { + public static double multiplierForward = 0.08294676287147268; + public static double multiplierStrafe = 0.008038662919454933; + public static double multiplierTurn = 0.02731472448271; + + + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(8.9); + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(1) + .leftFrontMotorName("fl") + .leftRearMotorName("rl") + .rightRearMotorName("rr") + .rightFrontMotorName("fr") + .leftFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .leftRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD); + public static DriveEncoderConstants localizerConstants = new DriveEncoderConstants() + .leftFrontMotorName("fl") + .leftRearMotorName("rl") + .rightRearMotorName("rr") + .rightFrontMotorName("fr") + .robotLength(17.6) + .forwardTicksToInches(multiplierForward) + .strafeTicksToInches(multiplierStrafe) + .turnTicksToInches(multiplierTurn) + .robotWidth(16.6) + .leftFrontEncoderDirection(Encoder.FORWARD) + .leftRearEncoderDirection(Encoder.FORWARD) + .rightRearEncoderDirection(Encoder.REVERSE) + .rightFrontEncoderDirection(Encoder.FORWARD); + /** + These are the PathConstraints in order: + tValueConstraint, velocityConstraint, translationalConstraint, headingConstraint, timeoutConstraint, + brakingStrength, BEZIER_CURVE_SEARCH_LIMIT, brakingStart + + The BEZIER_CURVE_SEARCH_LIMIT should typically be left at 10 and shouldn't be changed. + */ + + public static PathConstraints pathConstraints = new PathConstraints( 1.0, // tValueConstraint + 50.0, // velocityConstraint + 50.0, // translationalConstraint + Math.toRadians(180), // headingConstraint + 3.0, // timeoutConstraint + 1.0, // brakingStrength + 10, // BEZIER_CURVE_SEARCH_LIMIT (leave at 10) + 0.3 // brakingStart + ); + + //Add custom localizers or drivetrains here + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .driveEncoderLocalizer(localizerConstants) + .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 new file mode 100644 index 000000000000..7c0496d4212d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -0,0 +1,1362 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw; +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.ErrorCalculator; +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 com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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("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); + }); + s.folder("Manual", p -> { + p.add("Translational Tuner", TranslationalTuner::new); + p.add("Heading Tuner", HeadingTuner::new); + p.add("Drive Tuner", DriveTuner::new); + p.add("Line Tuner", Line::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); + }); + }); + } + + @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(); + + Drawing.init(); + } + + @Override + public void onLog(List lines) {} + + public static void drawOnlyCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void draw() { + Drawing.drawDebug(follower); + } + + /** 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 mecanum 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 + * @version 1.0, 5/6/2024 + */ +class LocalizationTest extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + } + + /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + 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()); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * 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() { + System.out.println("Hello, World!"); + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** 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); + drawOnlyCurrent(); + } + + /** + * 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() - 72)); + 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() - 72) / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * 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(); + drawOnlyCurrent(); + } + + /** 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); + drawOnlyCurrent(); + } + + /** + * 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() - 72)); + 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() - 72) / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * 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(); + drawOnlyCurrent(); + } + + /** 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); + + drawOnlyCurrent(); + } + + /** + * 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); + + draw(); + } +} + +/** + * 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 mecanum 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(); + drawOnlyCurrent(); + } + + /** 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(); + draw(); + + + 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 left 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 mecanum 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(); + drawOnlyCurrent(); + } + + /** 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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + /** 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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + /** 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(); + draw(); + + 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 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(); + drawOnlyCurrent(); + } + + @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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + @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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + @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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + @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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + @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(); + draw(); + 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(); + draw(); + + 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(); + drawOnlyCurrent(); + } + + /** 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(); + drawOnlyCurrent(); + } + + @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(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + } +} + +/** + * 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(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java index 0f2a4d58048a..118368ea34fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java @@ -7,17 +7,14 @@ import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @TeleOp(group = "Testing") public class Shooter extends LinearOpMode { - private final int CPR = Constants.CPR; + private final int CPR = TeamConstants.CPR; private final static FtcDashboard DASH = FtcDashboard.getInstance(); @@ -36,9 +33,9 @@ public void runOpMode() ); telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); - DcMotorEx[] scoring = Constants.getScoringMotors(hardwareMap); + DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); - Limelight3A limelight = Constants.getLimelight(hardwareMap); + Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); limelight.start(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java similarity index 69% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java index baa0bc2355d5..cce8aa25309c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java @@ -10,26 +10,22 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; -import com.pedropathing.follower.Follower; -import com.pedropathing.follower.FollowerConstants; -import com.pedropathing.ftc.FollowerBuilder; -import com.pedropathing.paths.PathConstraints; - @Config -public class Constants +public class TeamConstants { // Counts-Per-Revolution of a REV HD Hex Motor. public static int CPR = 28; public static double LOCK_ON_DENOMINATOR = 27.25; + public static double LOCK_ON_OFFSET = 0.0; public static DcMotorEx[] getDriveMotors(HardwareMap hardwareMap) { // --- Get Motor Objects --- DcMotorEx[] motors = { - hardwareMap.get(DcMotorEx.class, "frontLeftMotor"), - hardwareMap.get(DcMotorEx.class, "backLeftMotor"), - hardwareMap.get(DcMotorEx.class, "backRightMotor"), - hardwareMap.get(DcMotorEx.class, "frontRightMotor") + hardwareMap.get(DcMotorEx.class, "fl"), + hardwareMap.get(DcMotorEx.class, "rl"), + hardwareMap.get(DcMotorEx.class, "rr"), + hardwareMap.get(DcMotorEx.class, "fr") }; @@ -59,20 +55,22 @@ public static DcMotorEx[] getScoringMotors(HardwareMap hardwareMap) { // --- Get Motor Objects --- DcMotorEx[] motors = { - hardwareMap.get(DcMotorEx.class, "bigHooperMotor"), - hardwareMap.get(DcMotorEx.class, "smallHooperMotor") + hardwareMap.get(DcMotorEx.class, "hooper"), + hardwareMap.get(DcMotorEx.class, "intake1"), + hardwareMap.get(DcMotorEx.class, "intake2") }; // --- Motor Placement --- // "F": Front; "B": Back // + --F-- + // | 1 | - // | 0 | + // | 20 | // + --B-- + // --- Set Motor Directions --- motors[0].setDirection(DcMotorSimple.Direction.REVERSE); - motors[1].setDirection(DcMotorSimple.Direction.FORWARD); + motors[1].setDirection(DcMotorSimple.Direction.REVERSE); + motors[2].setDirection(DcMotorSimple.Direction.REVERSE); return motors; } @@ -102,16 +100,5 @@ public static Limelight3A getLimelight(HardwareMap hardwareMap) return limelight3a; } - // --- PedroPathing 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(); - } } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 778dc043f31e..70c240ea555f 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -2,6 +2,7 @@ repositories { mavenCentral() google() // Needed for androidx maven { url = "https://maven.brott.dev/"} + maven {url = "https://mymaven.bylazar.com/releases"} } dependencies { @@ -15,6 +16,9 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.pedropathing:ftc:2.0.4' - implementation 'com.pedropathing:telemetry:1.0.0' implementation 'com.acmerobotics.dashboard:dashboard:0.5.1' + + implementation 'com.pedropathing:ftc:2.0.5' + implementation 'com.pedropathing:telemetry:1.0.0' + implementation 'com.bylazar:fullpanels:1.0.12' } \ No newline at end of file From 99e5b2c5c7341a600306c60a66fda5d5e0773383 Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Mon, 12 Jan 2026 17:54:31 -0500 Subject: [PATCH 02/16] Deleting to limit conflicts with options-dev branch. --- .../ftc/teamcode/util/Options.java | 41 ------------------- 1 file changed, 41 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java deleted file mode 100644 index 955a8dbbed9e..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Options.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.teamcode.util; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -@Autonomous(group = "Management") -public class Options extends LinearOpMode -{ - private final String[] prompts = { - "Begin?", - "What alliance are you playing for?", - "Where do you plan to start from? (North is where the goals are.)", - "How many drivers?", - "Preferred driving style?" - }; - - private String[] modes = {"", "", "", "", "", ""}; - private final boolean[] inputs = { - gamepad1.a, gamepad1.b, gamepad1.x, gamepad1.y, - gamepad1.left_bumper, gamepad1.right_bumper - }; - - @Override - public void runOpMode() throws InterruptedException - { - String currentPrompt = prompts[0]; - - telemetry.addData("Prompt", currentPrompt); - telemetry.addData(" A", modes[0]); - telemetry.addData(" B", modes[1]); - telemetry.addData(" X", modes[2]); - telemetry.addData(" Y", modes[3]); - telemetry.addData("LB", modes[4]); - telemetry.addData("RB", modes[5]); - - for(String prompt : prompts) - { - System.out.println(prompt); - } - } -} From 4d05ff8b0d891f555db3d4f4645282bb853384cd Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Mon, 12 Jan 2026 22:56:49 -0500 Subject: [PATCH 03/16] Testing: Artemis Take the Flywheel --- .../TeleOp/TeleOpMecanumField2Drivers.java | 56 +++++++++++++++++-- .../ftc/teamcode/util/Shooter.java | 10 ++-- .../ftc/teamcode/util/TeamConstants.java | 4 ++ 3 files changed, 60 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java index c2edef856973..1abac6b44e62 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java @@ -48,6 +48,9 @@ public void runOpMode() final int CPR = TeamConstants.CPR; final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; + final double LL_MOUNT_ANGLE = TeamConstants.LL_MOUNT_ANGLE; + final double LL_LENS_HEIGHT_INCHES = TeamConstants.LL_LENS_HEIGHT_INCHES; + final double GOAL_HEIGHT_INCHES = TeamConstants.GOAL_HEIGHT_INCHES; final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); @@ -76,17 +79,20 @@ public void runOpMode() loggerRuntimeThread.start(); boolean lockOn = false; + boolean autoShot = true; boolean pastLeftBumper1 = false; + boolean pastB2 = false; while(opModeIsActive()) { LLResult rawResult = limelight.getLatestResult(); boolean resultIsValid = rawResult.isValid(); - double rx, targetX; + // LockOn logic + double rx; if(lockOn && resultIsValid) { - targetX = rawResult.getTx(); + double targetX = rawResult.getTx(); // A simple proportional input; May use PI (proportional-integral) // control in the future. // getTx returns a value in the range of -27.25 to 27.25. @@ -97,25 +103,55 @@ public void runOpMode() { rx = gamepad1.right_stick_x; } + + // "Artemis Take the Flywheel" logic + double hooperPower; + if(autoShot) + { + double targetY = rawResult.getTy(); + double distance = + (GOAL_HEIGHT_INCHES - LL_LENS_HEIGHT_INCHES) / Math.tan( + ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) + ) + ; + + if(distance >= 65.7) + { + hooperPower = (28.6*distance)+2623; + } else if(distance >= 47.5) + { + hooperPower = (16.5*distance)+3416; + } else if(distance >= 38.8) + { + hooperPower = (8.62*distance)+3791; + } else + { + hooperPower = 3000; + } + } else + { + // Max target RPM: 4900 + hooperPower = 3000 + (gamepad2.left_trigger * 1900); + } double driveBreakPower = 1-gamepad1.right_trigger; // Gamepad variables: boolean a1 = gamepad1.a; boolean a2 = gamepad2.a; boolean b1 = gamepad1.b; + boolean b2 = gamepad2.b; boolean x1 = gamepad1.x; boolean x2 = gamepad2.x; boolean y1 = gamepad1.y; boolean y2 = gamepad2.y; boolean leftBumper1 = gamepad1.left_bumper; boolean rightBumper2 = gamepad2.right_bumper; + double rightTrigger2 = gamepad2.right_trigger; boolean start1 = gamepad1.start; double lsY1 = -gamepad1.left_stick_y; double lsX1 = gamepad1.left_stick_x*1.1; - // Max target RPM: 4900 - double hooperPower = 3000 + (gamepad2.left_trigger * 1900); double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); double botHeadingDegrees = botHeading * (180.0/Math.PI); @@ -162,6 +198,7 @@ public void runOpMode() telemetryD.update(); telemetryD.addData("right-stick-x", rx); telemetryD.addData("bot-heading", botHeadingDegrees); + telemetryD.addData("auto-shot", autoShot); telemetryD.addData("lock-on", lockOn); telemetryD.addData("valid-target", resultIsValid); telemetryD.update(); @@ -174,6 +211,7 @@ public void runOpMode() telemetry.addLine(); telemetry.addData("right-stick-x", rx); telemetry.addData("bot-heading", botHeadingDegrees); + telemetry.addData("auto-shot", autoShot); telemetry.addData("lock-on", lockOn); telemetry.addData("valid-target", resultIsValid); telemetry.update(); @@ -188,10 +226,9 @@ public void runOpMode() imu.resetYaw(); } - // pastLeftBumper initializes to false + // LockOn toggle if(leftBumper1 && ! pastLeftBumper1) { - // LockOn toggle lockOn = !lockOn; } @@ -225,12 +262,19 @@ public void runOpMode() scoring[2].setPower(0); } + // "Artemis Take the Flywheel" toggle + if(rightTrigger2 == 1 && b2 && ! pastB2) + { + autoShot = !autoShot; + } + base[0].setPower(frontLeftPower*driveBreakPower); base[1].setPower(backLeftPower*driveBreakPower); base[2].setPower(backRightPower*driveBreakPower); base[3].setPower(frontRightPower*driveBreakPower); pastLeftBumper1 = leftBumper1; + pastB2 = b2; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java index 118368ea34fa..374e51cf10b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java @@ -15,6 +15,9 @@ public class Shooter extends LinearOpMode { private final int CPR = TeamConstants.CPR; + private final double LL_MOUNT_ANGLE = TeamConstants.LL_MOUNT_ANGLE; + private final double LL_LENS_HEIGHT_INCHES = TeamConstants.LL_LENS_HEIGHT_INCHES; + private final double GOAL_HEIGHT_INCHES = TeamConstants.GOAL_HEIGHT_INCHES; private final static FtcDashboard DASH = FtcDashboard.getInstance(); @@ -23,9 +26,6 @@ public void runOpMode() { // CONSTANTS: int INCREMENT = 25; - double LL_MOUNT_ANGLE = 30; - double LL_LENS_HEIGHT_INCHES = 14.5; - double GOAL_HEIGHT_INCHES = 38.75; // Telemetry telemetryM = new MultipleTelemetry( @@ -78,13 +78,15 @@ public void runOpMode() if(intake) { scoring[1].setPower(0.9); + scoring[2].setPower(0.9); } else if(intakeReverse) { scoring[1].setPower(-0.9); + scoring[2].setPower(-0.9); } else { scoring[1].setPower(0); - + scoring[2].setPower(0); } scoring[0].setVelocity((shootingInputRPM / 60) * CPR); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java index cce8aa25309c..7a55b78a4477 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java @@ -18,6 +18,10 @@ public class TeamConstants public static double LOCK_ON_DENOMINATOR = 27.25; public static double LOCK_ON_OFFSET = 0.0; + public static double LL_MOUNT_ANGLE = 30; + public static double LL_LENS_HEIGHT_INCHES = 14.5; + public static double GOAL_HEIGHT_INCHES = 38.75; + public static DcMotorEx[] getDriveMotors(HardwareMap hardwareMap) { // --- Get Motor Objects --- From d9ac9ba77f18e8b0e4adc0080f028ec2ee40c842 Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Wed, 14 Jan 2026 19:31:11 -0500 Subject: [PATCH 04/16] Testing: Shoot-by-wire --- .../TeleOp/TeleOpMecanumField2Drivers.java | 45 +++++++++++++++---- .../ftc/teamcode/util/Shooter.java | 37 +++++++++++++++ 2 files changed, 74 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java index 1abac6b44e62..d52b7b0aec3b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java @@ -82,6 +82,9 @@ public void runOpMode() boolean autoShot = true; boolean pastLeftBumper1 = false; boolean pastB2 = false; + int meanCounter = 0; + double aggregateDistance = 0; + double meanDistance = 0; while(opModeIsActive()) { @@ -105,29 +108,53 @@ public void runOpMode() } // "Artemis Take the Flywheel" logic - double hooperPower; + double hooperPower, rawDistance = 0; if(autoShot) { double targetY = rawResult.getTy(); - double distance = + rawDistance = (GOAL_HEIGHT_INCHES - LL_LENS_HEIGHT_INCHES) / Math.tan( ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) ) ; - if(distance >= 65.7) + //if (meanCounter < 10) { + // aggregateDistance += rawDistance; + // meanCounter++; + //} + //else + //{ + // oldMean = aggregateDistance / 10; + // + // aggregateDistance -= oldMean; + // + // aggregateDistance += rawDistance; + // + // meanDistance = aggregateDistance / 10; + //} + + //if(meanCounter > 10) + //{ + // meanDistance = aggregateDistance / 10; + + // meanCounter = 0; + // aggregateDistance = 0; + //} + + if(rawDistance >= 65.7) { - hooperPower = (28.6*distance)+2623; - } else if(distance >= 47.5) + hooperPower = (28.6*rawDistance)+2623; + } else if(rawDistance >= 47.5) { - hooperPower = (16.5*distance)+3416; - } else if(distance >= 38.8) + hooperPower = (16.5*rawDistance)+3416; + } else if(rawDistance >= 38.8) { - hooperPower = (8.62*distance)+3791; + hooperPower = (8.62*rawDistance)+3791; } else { hooperPower = 3000; } + meanDistance = 0; } else { // Max target RPM: 4900 @@ -199,6 +226,7 @@ public void runOpMode() telemetryD.addData("right-stick-x", rx); telemetryD.addData("bot-heading", botHeadingDegrees); telemetryD.addData("auto-shot", autoShot); + telemetryD.addData("ll-distance", rawDistance); telemetryD.addData("lock-on", lockOn); telemetryD.addData("valid-target", resultIsValid); telemetryD.update(); @@ -212,6 +240,7 @@ public void runOpMode() telemetry.addData("right-stick-x", rx); telemetry.addData("bot-heading", botHeadingDegrees); telemetry.addData("auto-shot", autoShot); + telemetry.addData("ll-distance", rawDistance); telemetry.addData("lock-on", lockOn); telemetry.addData("valid-target", resultIsValid); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java index 374e51cf10b8..1c091aa47e88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java @@ -8,8 +8,10 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @TeleOp(group = "Testing") public class Shooter extends LinearOpMode @@ -34,9 +36,13 @@ public void runOpMode() telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + IMU.Parameters parameters = TeamConstants.getIMUParms(); + IMU imu = TeamConstants.getIMU(hardwareMap); Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + imu.initialize(parameters); limelight.start(); telemetryM.addLine("Ready."); @@ -89,7 +95,38 @@ public void runOpMode() scoring[2].setPower(0); } + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; + double rx = gamepad1.right_stick_x; + + + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double botHeadingDegrees = botHeading * (180.0/Math.PI); + // Rotate the movement direction counter to the bot's rotation + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double backRightPower = (rotY - rotX - rx) / denominator; + double frontRightPower = (rotY + rotX - rx) / denominator; + + scoring[0].setVelocity((shootingInputRPM / 60) * CPR); + base[0].setPower(frontLeftPower); + base[1].setPower(backLeftPower); + base[2].setPower(frontRightPower); + base[3].setPower(backRightPower); telemetryM.addData("shooting-input-rpm", shootingInputRPM); telemetryM.addData("shooting-speed-rpm", (scoring[0].getVelocity() * 60) / CPR); From b5bfef81572c2e6fda82242db6f64a5dbe44d8c0 Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Wed, 14 Jan 2026 19:49:12 -0500 Subject: [PATCH 05/16] made bad auto Plus sorta fixed Pedro pathing, will get better later --- .../firstinspires/ftc/teamcode/AutoBasic.java | 60 +++++++++++-------- .../ftc/teamcode/pedroPathing/Constants.java | 4 +- 2 files changed, 38 insertions(+), 26 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java index 3717861938b8..36c06ec984e5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java @@ -6,42 +6,54 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import org.firstinspires.ftc.teamcode.util.TeamConstants; + @Autonomous public class AutoBasic extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { // Declare motors // Make sure your ID's match your configuration - DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class, "frontLeftMotor"); - DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class, "backLeftMotor"); - DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class, "frontRightMotor"); - DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class, "backRightMotor"); - DcMotorEx bigHooperMotor = hardwareMap.get(DcMotorEx.class, "bigHooperMotor"); - DcMotorEx smallHooperMotor = hardwareMap.get(DcMotorEx.class, "smallHooperMotor"); - - backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); waitForStart(); if(isStopRequested()) return; - frontLeftMotor.setPower(0.5); - frontRightMotor.setPower(0.5); - backLeftMotor.setPower(0.5); - backRightMotor.setPower(0.5); - sleep(2200); - frontLeftMotor.setPower(0.25); - frontRightMotor.setPower(0.25); - backLeftMotor.setPower(-0.25); - backRightMotor.setPower(-0.25); - sleep(750); - frontLeftMotor.setPower(0); - frontRightMotor.setPower(0); - backLeftMotor.setPower(0); - backRightMotor.setPower(0); - bigHooperMotor.setPower(-0.625); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1000); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(-0.25); + base[3].setPower(-0.25); */ + //sleep(750); + base[0].setPower(0); + base[0].setPower(0); + base[0].setPower(0); + base[0].setPower(0); + scoring[0].setVelocity(4500); sleep(2000); - bigHooperMotor.setPower(0); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(600); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(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 index 5fb1c65d1f2b..b40cb1772725 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -16,13 +16,13 @@ @Config public class Constants { - public static double multiplierForward = 0.08294676287147268; + public static double multiplierForward = 0.008294676287147268; public static double multiplierStrafe = 0.008038662919454933; public static double multiplierTurn = 0.02731472448271; public static FollowerConstants followerConstants = new FollowerConstants() - .mass(8.9); + .mass(20.4); public static MecanumConstants driveConstants = new MecanumConstants() .maxPower(1) From d8e1e384cb5534b1c1ce1e9ef8b65d543c252772 Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Wed, 14 Jan 2026 21:17:17 -0500 Subject: [PATCH 06/16] Testing: Reset for thursday, gotta do more data collection for ATF/SBW --- .../TeleOp/{TeleOpBasic.java => Basic.java} | 2 +- ...numField1Driver.java => Field1Driver.java} | 2 +- ...mField2Drivers.java => Field2Drivers.java} | 21 +- .../teamcode/TeleOp/Field2DriversPower.java | 258 ++++++++++++++++++ ...numRobot1Driver.java => Robot1Driver.java} | 2 +- ...mRobot2Drivers.java => Robot2Drivers.java} | 2 +- .../ftc/teamcode/util/Shooter.java | 37 --- 7 files changed, 274 insertions(+), 50 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/{TeleOpBasic.java => Basic.java} (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/{TeleOpMecanumField1Driver.java => Field1Driver.java} (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/{TeleOpMecanumField2Drivers.java => Field2Drivers.java} (96%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/{TeleOpMecanumRobot1Driver.java => Robot1Driver.java} (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/{TeleOpMecanumRobot2Drivers.java => Robot2Drivers.java} (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Basic.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpBasic.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Basic.java index 76a8ac3962da..2a999e2f9dd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Basic.java @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; @TeleOp -public class TeleOpBasic extends LinearOpMode { +public class Basic extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { // Declare our motors diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField1Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField1Driver.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java index 421319fa85cb..1e603b5305dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField1Driver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java @@ -17,7 +17,7 @@ import java.io.IOException; @TeleOp(group = "Field Centric") -public class TeleOpMecanumField1Driver extends LinearOpMode { +public class Field1Driver extends LinearOpMode { private IMU.Parameters parameters; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java index d52b7b0aec3b..a5efd3c6e2dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumField2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java @@ -23,7 +23,7 @@ import java.util.Locale; @TeleOp(group = "Field Centric") -public class TeleOpMecanumField2Drivers extends LinearOpMode +public class Field2Drivers extends LinearOpMode { // Required for the DiagnosticLogger at the bottom of this file: private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); @@ -133,13 +133,16 @@ public void runOpMode() // meanDistance = aggregateDistance / 10; //} - //if(meanCounter > 10) - //{ - // meanDistance = aggregateDistance / 10; + aggregateDistance += rawDistance; + meanCounter++; - // meanCounter = 0; - // aggregateDistance = 0; - //} + if(meanCounter > 10) + { + meanDistance = aggregateDistance / 10; + + meanCounter = 0; + aggregateDistance = 0; + } if(rawDistance >= 65.7) { @@ -267,7 +270,7 @@ public void runOpMode() scoring[0].setPower(-0.9); } else { - scoring[0].setVelocity((hooperPower / 60)*CPR); + scoring[0].setVelocity((hooperPower/60) * CPR); } if (rightBumper2 && a2) @@ -330,4 +333,4 @@ private DiagnosticLogger getLogger() } return logger; } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java new file mode 100644 index 000000000000..6aa70eb79174 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2DriversPower.java @@ -0,0 +1,258 @@ +package org.firstinspires.ftc.teamcode.TeleOp; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +import java.io.IOException; +import java.util.Locale; + +@TeleOp(group = "Field Centric") +public class Field2DriversPower extends LinearOpMode +{ + // Required for the DiagnosticLogger at the bottom of this file: + private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); + + // --- One-time-only constants (non-@Config) --- + // These only update after a restart of the bot. + private final static FtcDashboard DASH = FtcDashboard.getInstance(); + private final static TelemetryPacket packet = new TelemetryPacket(); + + Telemetry telemetryD = DASH.getTelemetry(); + Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); + + @Override + public void runOpMode() + { + // These methods are only initialized when inside of runOpMode: + telemetryD.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + // --- @Config constants --- + final int CPR = TeamConstants.CPR; + final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; + final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; + + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + final IMU imu = TeamConstants.getIMU(hardwareMap); + final Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + + // Final setup before control starts: + imu.initialize(parameters); + limelight.start(); + + telemetryM.addLine("> Ready."); + telemetryM.update(); + + waitForStart(); + + // Ensure telemetry is clear on start. + // FtcDashboard tends to keep telemetry in the buffer even after + // update()-ing. + // (We abuse this a bit later to have nicely formatted text, since + // FtcDashboard also likes alphabetizing data.) + telemetryM.clear(); + + DiagnosticLogger logger = getLogger(); + Thread loggerRuntimeThread = new Thread(logger); + logger.resumeRun(); + loggerRuntimeThread.start(); + + boolean lockOn = false; + boolean pastLeftBumper1 = false; + + while(opModeIsActive()) + { + LLResult rawResult = limelight.getLatestResult(); + boolean resultIsValid = rawResult.isValid(); + + // LockOn logic + double rx; + if(lockOn && resultIsValid) + { + double targetX = rawResult.getTx(); + // A simple proportional input; May use PI (proportional-integral) + // control in the future. + // getTx returns a value in the range of -27.25 to 27.25. + // We want to get a value from -1 to 1, so we can just divide + // by 27.25. (27.25/27.25 = 1) + rx = (targetX+LOCK_ON_OFFSET)/LOCK_ON_DENOMINATOR; + } else + { + rx = gamepad1.right_stick_x; + } + + double hooperPower = gamepad2.left_trigger; + double driveBreakPower = 1-gamepad1.right_trigger; + + // Gamepad variables: + boolean a1 = gamepad1.a; + boolean a2 = gamepad2.a; + boolean b1 = gamepad1.b; + boolean x1 = gamepad1.x; + boolean x2 = gamepad2.x; + boolean y1 = gamepad1.y; + boolean y2 = gamepad2.y; + boolean leftBumper1 = gamepad1.left_bumper; + boolean rightBumper2 = gamepad2.right_bumper; + boolean start1 = gamepad1.start; + + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; + + + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double botHeadingDegrees = botHeading * (180.0/Math.PI); + // Rotate the movement direction counter to the bot's rotation + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + // -- Runtime Telemetry -- + double hooperSpeed = (scoring[0].getVelocity()*60)/CPR; + String runtimeSeconds = String.format( + Locale.ENGLISH, "%.4f", getRuntime() + ); + + packet.clearLines(); + packet.put("", + "" + + "Runtime (Seconds): " + runtimeSeconds + "
" + + "
" + + "== Shooting Motor (scoring[0]) ==" + ); + + // runtimeSeconds shouldn't be graphable,so we make it a line + // instead of data. (FtcDashboard graph already adds time as X) + DASH.sendTelemetryPacket(packet); + telemetryD.addData("shooting-input-rpm", hooperPower); + telemetryD.addData("shooting-speed-rpm", hooperSpeed); + telemetryD.update(); + telemetryD.addData("
", ""); + telemetryD.update(); + telemetryD.addData("right-stick-x", rx); + telemetryD.addData("bot-heading", botHeadingDegrees); + telemetryD.addData("lock-on", lockOn); + telemetryD.addData("valid-target", resultIsValid); + telemetryD.update(); + + telemetry.addData("Runtime (Seconds)", runtimeSeconds); + telemetry.addLine(); + telemetry.addLine("== Shooting Motor (scoring[0]) =="); + telemetry.addData("shooting-input-rpm", hooperPower); + telemetry.addData("shooting-speed-rpm", hooperSpeed); + telemetry.addLine(); + telemetry.addData("right-stick-x", rx); + telemetry.addData("bot-heading", botHeadingDegrees); + telemetry.addData("lock-on", lockOn); + telemetry.addData("valid-target", resultIsValid); + telemetry.update(); + + if (a1 && b1 && x1 && y1) + { + logger.stopRun(); + } + + if (start1) + { + imu.resetYaw(); + } + + // LockOn toggle + if(leftBumper1 && ! pastLeftBumper1) + { + lockOn = !lockOn; + + } + + if(y2) + { + scoring[0].setPower(-0.9); + } else + { + scoring[0].setPower(hooperPower); + } + + if (rightBumper2 && a2) + { + scoring[1].setPower(-0.9); + } else if (a2) + { + scoring[1].setPower(0.9); + } else + { + scoring[1].setPower(0); + } + if(rightBumper2 && x2) + { + scoring[2].setPower(-0.9); + } else if(x2) + { + scoring[2].setPower(0.9); + } else + { + scoring[2].setPower(0); + } + + base[0].setPower(frontLeftPower*driveBreakPower); + base[1].setPower(backLeftPower*driveBreakPower); + base[2].setPower(backRightPower*driveBreakPower); + base[3].setPower(frontRightPower*driveBreakPower); + + pastLeftBumper1 = leftBumper1; + } + } + + @NonNull + private DiagnosticLogger getLogger() + { + DiagnosticLogger logger; + try + { + logger = new DiagnosticLogger( + telemetry, hardwareMap, + null, + new String[] + { + "fl", "rl", + "fr", "rr", + "hooper", "intake1", "intake2" + }, + null, "imu", parameters + ); + } catch (IOException e) + { + throw new RuntimeException(e); + } + return logger; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot1Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot1Driver.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot1Driver.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot1Driver.java index 96b9ce5f11de..efbc820651e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot1Driver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot1Driver.java @@ -17,7 +17,7 @@ import java.io.IOException; @TeleOp(group = "Robot Centric") -public class TeleOpMecanumRobot1Driver extends LinearOpMode { +public class Robot1Driver extends LinearOpMode { private IMU.Parameters parameters; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot2Drivers.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot2Drivers.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot2Drivers.java index 5ea06c9e8e5f..b5909bc23471 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/TeleOpMecanumRobot2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Robot2Drivers.java @@ -17,7 +17,7 @@ import java.io.IOException; @TeleOp(group = "Robot Centric") -public class TeleOpMecanumRobot2Drivers extends LinearOpMode { +public class Robot2Drivers extends LinearOpMode { private IMU.Parameters parameters; @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java index 1c091aa47e88..374e51cf10b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Shooter.java @@ -8,10 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @TeleOp(group = "Testing") public class Shooter extends LinearOpMode @@ -36,13 +34,9 @@ public void runOpMode() telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); - DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); - IMU.Parameters parameters = TeamConstants.getIMUParms(); - IMU imu = TeamConstants.getIMU(hardwareMap); Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); - imu.initialize(parameters); limelight.start(); telemetryM.addLine("Ready."); @@ -95,38 +89,7 @@ public void runOpMode() scoring[2].setPower(0); } - double lsY1 = -gamepad1.left_stick_y; - double lsX1 = gamepad1.left_stick_x*1.1; - double rx = gamepad1.right_stick_x; - - - double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); - double botHeadingDegrees = botHeading * (180.0/Math.PI); - // Rotate the movement direction counter to the bot's rotation - double rotX = 1.1*( - lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) - ); - double rotY = - lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) - ; - - // Denominator is the largest motor power (absolute value) or 1 - // This ensures all the powers maintain the same ratio, - // but only if at least one is out of the range [-1, 1] - double denominator = Math.max( - Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 - ); - double frontLeftPower = (rotY + rotX + rx) / denominator; - double backLeftPower = (rotY - rotX + rx) / denominator; - double backRightPower = (rotY - rotX - rx) / denominator; - double frontRightPower = (rotY + rotX - rx) / denominator; - - scoring[0].setVelocity((shootingInputRPM / 60) * CPR); - base[0].setPower(frontLeftPower); - base[1].setPower(backLeftPower); - base[2].setPower(frontRightPower); - base[3].setPower(backRightPower); telemetryM.addData("shooting-input-rpm", shootingInputRPM); telemetryM.addData("shooting-speed-rpm", (scoring[0].getVelocity() * 60) / CPR); From 474189b903ac795832b86204591b8f71463ae528 Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Wed, 14 Jan 2026 22:12:25 -0500 Subject: [PATCH 07/16] New AutoBasic --- .../firstinspires/ftc/teamcode/AutoBasic.java | 104 ++++++++++-------- 1 file changed, 57 insertions(+), 47 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java index 3717861938b8..22a7b9263cb9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java @@ -1,47 +1,57 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; - -@Autonomous -public class AutoBasic extends LinearOpMode { - @Override - public void runOpMode() throws InterruptedException { - // Declare motors - // Make sure your ID's match your configuration - DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class, "frontLeftMotor"); - DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class, "backLeftMotor"); - DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class, "frontRightMotor"); - DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class, "backRightMotor"); - DcMotorEx bigHooperMotor = hardwareMap.get(DcMotorEx.class, "bigHooperMotor"); - DcMotorEx smallHooperMotor = hardwareMap.get(DcMotorEx.class, "smallHooperMotor"); - - backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); - - waitForStart(); - - if(isStopRequested()) return; - - frontLeftMotor.setPower(0.5); - frontRightMotor.setPower(0.5); - backLeftMotor.setPower(0.5); - backRightMotor.setPower(0.5); - sleep(2200); - frontLeftMotor.setPower(0.25); - frontRightMotor.setPower(0.25); - backLeftMotor.setPower(-0.25); - backRightMotor.setPower(-0.25); - sleep(750); - frontLeftMotor.setPower(0); - frontRightMotor.setPower(0); - backLeftMotor.setPower(0); - backRightMotor.setPower(0); - bigHooperMotor.setPower(-0.625); - sleep(2000); - bigHooperMotor.setPower(0); - - } -}; +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBasic extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1000); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(-0.25); + base[3].setPower(-0.25); */ + //sleep(750); + base[0].setPower(0); + base[0].setPower(0); + base[0].setPower(0); + base[0].setPower(0); + scoring[0].setVelocity(4500); + sleep(2000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(600); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + + + } +}; From e47ac2cfb1eab9c97de9617a1ee7b66c583b715c Mon Sep 17 00:00:00 2001 From: ElectraBytes04 Date: Thu, 15 Jan 2026 17:25:29 -0500 Subject: [PATCH 08/16] Tested: Auto shooting works --- .../ftc/teamcode/TeleOp/Field2Drivers.java | 36 ++++++++++++++----- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java index a5efd3c6e2dc..6bb843fe9cd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java @@ -85,6 +85,8 @@ public void runOpMode() int meanCounter = 0; double aggregateDistance = 0; double meanDistance = 0; + double tempDistance = 0; + double pastDistance = 0; while(opModeIsActive()) { @@ -112,12 +114,14 @@ public void runOpMode() if(autoShot) { double targetY = rawResult.getTy(); - rawDistance = + double distanceCalc = (GOAL_HEIGHT_INCHES - LL_LENS_HEIGHT_INCHES) / Math.tan( - ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) + ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) ) ; + rawDistance = rawResult.isValid() ? distanceCalc : pastDistance; + //if (meanCounter < 10) { // aggregateDistance += rawDistance; // meanCounter++; @@ -142,17 +146,33 @@ public void runOpMode() meanCounter = 0; aggregateDistance = 0; + } - if(rawDistance >= 65.7) + tempDistance = rawDistance; + pastDistance = tempDistance; + + if(tempDistance >= 81.5) + { + hooperPower = (6.25*tempDistance)-644; + } else if(tempDistance >= 76.2) + { + hooperPower = (18.9*tempDistance)+2912; + } else if(tempDistance >= 65.7) + { + hooperPower = (33.3*tempDistance)+1810; + } else if(tempDistance >= 59.1) + { + hooperPower = (22.7*tempDistance)+2507; + } else if(tempDistance >= 54.6) { - hooperPower = (28.6*rawDistance)+2623; - } else if(rawDistance >= 47.5) + hooperPower = (-22.2*tempDistance)+5163; + } else if(tempDistance >= 47.5) { - hooperPower = (16.5*rawDistance)+3416; - } else if(rawDistance >= 38.8) + hooperPower = 3950; + } else if(tempDistance >= 40.4) { - hooperPower = (8.62*rawDistance)+3791; + hooperPower = (-14.1*tempDistance)+4619; } else { hooperPower = 3000; From 987af6262ebb08a08fbfe12436cf3f88b6310411 Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Thu, 15 Jan 2026 17:32:42 -0500 Subject: [PATCH 09/16] bad auto made slightly better --- .../firstinspires/ftc/teamcode/AutoBasic.java | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java index 36c06ec984e5..683223e942b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java @@ -21,32 +21,32 @@ public void runOpMode() throws InterruptedException { if(isStopRequested()) return; - base[0].setPower(-0.5); - base[1].setPower(-0.5); - base[2].setPower(-0.5); - base[3].setPower(-0.5); - sleep(1000); - /*base[0].setPower(0.5); + base[0].setPower(0.5); base[1].setPower(0.5); base[2].setPower(0.5); base[3].setPower(0.5); + sleep(2500); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ base[0].setPower(0.25); base[1].setPower(0.25); base[2].setPower(-0.25); - base[3].setPower(-0.25); */ - //sleep(750); - base[0].setPower(0); - base[0].setPower(0); - base[0].setPower(0); + base[3].setPower(-0.25); + sleep(750); base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); scoring[0].setVelocity(4500); sleep(2000); scoring[1].setPower(1); scoring[2].setPower(1); - sleep(600); + sleep(6000); scoring[1].setPower(0); scoring[2].setPower(0); - sleep(500); + sleep(5000); scoring[1].setPower(1); scoring[2].setPower(1); sleep(800); From 388f221ea8c52d6c098b88c7a8cb46cd70f3a879 Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Fri, 16 Jan 2026 17:02:28 -0500 Subject: [PATCH 10/16] finished auto but not pedro so its really boring --- .../firstinspires/ftc/teamcode/AutoBasic.java | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java index ca22bc4a1fd5..2c7dee8ac465 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java @@ -26,7 +26,7 @@ public void runOpMode() throws InterruptedException { base[1].setPower(0.5); base[2].setPower(0.5); base[3].setPower(0.5); - sleep(2500); + sleep(2700); /*base[0].setPower(0.5); base[1].setPower(0.5); base[2].setPower(0.5); @@ -35,25 +35,40 @@ public void runOpMode() throws InterruptedException { base[1].setPower(0.25); base[2].setPower(-0.25); base[3].setPower(-0.25); - sleep(750); + sleep(1000); base[0].setPower(0); base[1].setPower(0); base[2].setPower(0); base[3].setPower(0); - scoring[0].setVelocity(4500); - sleep(2000); + scoring[0].setVelocity(1920); + sleep(5000); scoring[1].setPower(1); scoring[2].setPower(1); - sleep(6000); + sleep(250); scoring[1].setPower(0); scoring[2].setPower(0); - sleep(5000); + sleep(5500); scoring[1].setPower(1); scoring[2].setPower(1); sleep(800); scoring[0].setPower(0); scoring[1].setPower(0); scoring[2].setPower(0); + base[0].setPower(-0.25); + base[1].setPower(-0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(3400); + base[0].setPower(-0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(0.5); + sleep(2300); + base[0].setPower(-0.25); + base[1].setPower(-0.25); + base[2].setPower(-0.25); + base[3].setPower(-0.25); + sleep(1750); } From 23d164881a0ed5283d207f7cc25d6ba3958feb8c Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Fri, 16 Jan 2026 17:47:24 -0500 Subject: [PATCH 11/16] AutoBasicbleu --- .../ftc/teamcode/AutoBasicbleu.java | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java new file mode 100644 index 000000000000..6ac595461bc5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java @@ -0,0 +1,74 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBasicbleu extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(2700); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(-0.25); + base[1].setPower(-0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(975); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1920); + sleep(5000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(5500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(1500); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(1000); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(-0.25); + base[3].setPower(-0.25); + sleep(3150); + base[0].setPower(-0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(0.5); + sleep(1250); + + + } +}; From 0bb7cda49170c4436a72764ab4e160cc5f47d0cc Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Fri, 16 Jan 2026 19:38:43 -0500 Subject: [PATCH 12/16] AutoBasicbleu --- .../java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java index 6ac595461bc5..ad642b94fa52 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasicbleu.java @@ -34,7 +34,7 @@ public void runOpMode() throws InterruptedException { base[1].setPower(-0.25); base[2].setPower(0.25); base[3].setPower(0.25); - sleep(975); + sleep(993); base[0].setPower(0); base[1].setPower(0); base[2].setPower(0); @@ -57,7 +57,7 @@ public void runOpMode() throws InterruptedException { base[1].setPower(0.25); base[2].setPower(0.25); base[3].setPower(0.25); - sleep(1000); + sleep(1250); base[0].setPower(0.25); base[1].setPower(0.25); base[2].setPower(-0.25); From 654bbeaa324510fde77a9e0e6db7e0fba2804116 Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Mon, 19 Jan 2026 13:42:34 -0500 Subject: [PATCH 13/16] added ultraplanetary controls for intake 1 and updated Field 1 --- .../ftc/teamcode/TeleOp/Field1Driver.java | 388 +++++++++++++----- .../ftc/teamcode/TeleOp/Field2Drivers.java | 4 +- 2 files changed, 282 insertions(+), 110 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java index 1e603b5305dd..04869c2d1fab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field1Driver.java @@ -1,160 +1,332 @@ package org.firstinspires.ftc.teamcode.TeleOp; -import static com.qualcomm.robotcore.hardware.DcMotor.ZeroPowerBehavior.BRAKE; - import androidx.annotation.NonNull; +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.IMU; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; + +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; import org.firstinspires.ftc.teamcode.util.DiagnosticLogger; import java.io.IOException; +import java.util.Locale; @TeleOp(group = "Field Centric") -public class Field1Driver extends LinearOpMode { - private IMU.Parameters parameters; +public class Field2Drivers extends LinearOpMode +{ + // Required for the DiagnosticLogger at the bottom of this file: + private final static IMU.Parameters parameters = TeamConstants.getIMUParms(); + + // --- One-time-only constants (non-@Config) --- + // These only update after a restart of the bot. + private final static FtcDashboard DASH = FtcDashboard.getInstance(); + private final static TelemetryPacket packet = new TelemetryPacket(); + + Telemetry telemetryD = DASH.getTelemetry(); + Telemetry telemetryM = new MultipleTelemetry(telemetry, DASH.getTelemetry()); + @Override - public void runOpMode() throws InterruptedException { - // CPR of a Rev HD Hex Motor. - final int CPR = 28; - // Declare motors - // Make sure your ID's match your configuration - DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class, "frontLeftMotor"); - DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class, "backLeftMotor"); - DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class, "frontRightMotor"); - DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class, "backRightMotor"); - DcMotorEx bigHooperMotor = hardwareMap.get(DcMotorEx.class, "bigHooperMotor"); - DcMotorEx smallHooperMotor = hardwareMap.get(DcMotorEx.class, "smallHooperMotor"); - - VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next(); - - backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); - bigHooperMotor.setDirection(DcMotorSimple.Direction.REVERSE); - - frontLeftMotor.setZeroPowerBehavior(BRAKE); - frontRightMotor.setZeroPowerBehavior(BRAKE); - backRightMotor.setZeroPowerBehavior(BRAKE); - backLeftMotor.setZeroPowerBehavior(BRAKE); - - // Retrieve the IMU from the hardware map - IMU imu = hardwareMap.get(IMU.class, "imu"); - // The private parameters variable is passed to the logger at the bottom of this class. - this.parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.LEFT, - RevHubOrientationOnRobot.UsbFacingDirection.UP)); - // Without this, the REV Hub's orientation is assumed to be logo UP / USB FORWARD + public void runOpMode() + { + // These methods are only initialized when inside of runOpMode: + telemetryD.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetryM.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + // --- @Config constants --- + final int CPR = TeamConstants.CPR; + final double LOCK_ON_DENOMINATOR = TeamConstants.LOCK_ON_DENOMINATOR; + final double LOCK_ON_OFFSET = TeamConstants.LOCK_ON_OFFSET; + final double LL_MOUNT_ANGLE = TeamConstants.LL_MOUNT_ANGLE; + final double LL_LENS_HEIGHT_INCHES = TeamConstants.LL_LENS_HEIGHT_INCHES; + final double GOAL_HEIGHT_INCHES = TeamConstants.GOAL_HEIGHT_INCHES; + + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + final IMU imu = TeamConstants.getIMU(hardwareMap); + final Limelight3A limelight = TeamConstants.getLimelight(hardwareMap); + + // Final setup before control starts: imu.initialize(parameters); + limelight.start(); + + telemetryM.addLine("> Ready."); + telemetryM.update(); + + waitForStart(); + + // Ensure telemetry is clear on start. + // FtcDashboard tends to keep telemetry in the buffer even after + // update()-ing. + // (We abuse this a bit later to have nicely formatted text, since + // FtcDashboard also likes alphabetizing data.) + telemetryM.clear(); DiagnosticLogger logger = getLogger(); Thread loggerRuntimeThread = new Thread(logger); logger.resumeRun(); loggerRuntimeThread.start(); - waitForStart(); + boolean lockOn = false; + boolean autoShot = true; + boolean pastLeftBumper1 = false; + boolean pastB2 = false; + int meanCounter = 0; + double aggregateDistance = 0; + double meanDistance = 0; + double tempDistance = 0; + double pastDistance = 0; - if (isStopRequested()) return; + while(opModeIsActive()) + { + LLResult rawResult = limelight.getLatestResult(); + boolean resultIsValid = rawResult.isValid(); - while (opModeIsActive()) { - double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed - double x = gamepad1.left_stick_x*1.1; - double rx = gamepad1.right_stick_x; - double brakePower = 1-gamepad1.right_trigger; - double bigHooperPower = gamepad1.left_trigger * 6000; + // LockOn logic + double rx; + if(lockOn && resultIsValid) + { + double targetX = rawResult.getTx(); + // A simple proportional input; May use PI (proportional-integral) + // control in the future. + // getTx returns a value in the range of -27.25 to 27.25. + // We want to get a value from -1 to 1, so we can just divide + // by 27.25. (27.25/27.25 = 1) + rx = (targetX+LOCK_ON_OFFSET)/LOCK_ON_DENOMINATOR; + } else + { + rx = gamepad1.right_stick_x; + } - double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + // "Artemis Take the Flywheel" logic + double hooperPower, rawDistance = 0; + if(autoShot) + { + double targetY = rawResult.getTy(); + double distanceCalc = + (GOAL_HEIGHT_INCHES - LL_LENS_HEIGHT_INCHES) / Math.tan( + ((LL_MOUNT_ANGLE + targetY) * (Math.PI/180.0)) + ) + ; - // Rotate the movement direction counter to the bot's rotation - double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); - double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); + rawDistance = rawResult.isValid() ? distanceCalc : pastDistance; + + //if (meanCounter < 10) { + // aggregateDistance += rawDistance; + // meanCounter++; + //} + //else + //{ + // oldMean = aggregateDistance / 10; + // + // aggregateDistance -= oldMean; + // + // aggregateDistance += rawDistance; + // + // meanDistance = aggregateDistance / 10; + //} + + aggregateDistance += rawDistance; + meanCounter++; + + if(meanCounter > 10) + { + meanDistance = aggregateDistance / 10; + + meanCounter = 0; + aggregateDistance = 0; - rotX = rotX * 1.1; + } + + tempDistance = rawDistance; + pastDistance = tempDistance; + + if(tempDistance >= 81.5) + { + hooperPower = (6.25*tempDistance)-644; + } else if(tempDistance >= 76.2) + { + hooperPower = (18.9*tempDistance)+2912; + } else if(tempDistance >= 65.7) + { + hooperPower = (33.3*tempDistance)+1810; + } else if(tempDistance >= 59.1) + { + hooperPower = (22.7*tempDistance)+2507; + } else if(tempDistance >= 54.6) + { + hooperPower = (-22.2*tempDistance)+5163; + } else if(tempDistance >= 47.5) + { + hooperPower = 3950; + } else if(tempDistance >= 40.4) + { + hooperPower = (-14.1*tempDistance)+4619; + } else + { + hooperPower = 3000; + } + meanDistance = 0; + } else + { + // Max target RPM: 4900 + hooperPower = 3000 + (gamepad2.left_trigger * 1900); + } + double driveBreakPower = 1-gamepad1.right_trigger; + + // Gamepad variables: + boolean a1 = gamepad1.a; + boolean a2 = gamepad2.a; + boolean b1 = gamepad1.b; + boolean b2 = gamepad2.b; + boolean x1 = gamepad1.x; + boolean x2 = gamepad2.x; + boolean y1 = gamepad1.y; + boolean y2 = gamepad2.y; + boolean leftBumper1 = gamepad1.left_bumper; + boolean rightBumper1 = gamepad1.right_bumper; + double rightTrigger1 = gamepad1.right_trigger; + boolean start1 = gamepad1.start; + + double lsY1 = -gamepad1.left_stick_y; + double lsX1 = gamepad1.left_stick_x*1.1; + + + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + double botHeadingDegrees = botHeading * (180.0/Math.PI); + // Rotate the movement direction counter to the bot's rotation + double rotX = 1.1*( + lsX1 * Math.cos(-botHeading) - lsY1 * Math.sin(-botHeading) + ); + double rotY = + lsX1 * Math.sin(-botHeading) + lsY1 * Math.cos(-botHeading) + ; // Denominator is the largest motor power (absolute value) or 1 // This ensures all the powers maintain the same ratio, // but only if at least one is out of the range [-1, 1] - double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double denominator = Math.max( + Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 + ); double frontLeftPower = (rotY + rotX + rx) / denominator; double backLeftPower = (rotY - rotX + rx) / denominator; double frontRightPower = (rotY - rotX - rx) / denominator; double backRightPower = (rotY + rotX - rx) / denominator; - telemetry.addData("1, LT", gamepad1.left_trigger); - telemetry.addData("1, RT", gamepad1.right_trigger); - telemetry.addData("1, LB", gamepad1.left_bumper); - telemetry.addData("1, LB", gamepad1.right_bumper); - telemetry.addData("1, LT", gamepad1.left_trigger); - telemetry.addData("1, LS-X", gamepad1.left_stick_x); - telemetry.addData("1, LS-Y", gamepad1.left_stick_y); - telemetry.addData("1, RS-X", gamepad1.right_stick_x); - telemetry.addData("1, RS-Y", gamepad1.right_stick_y); - telemetry.addData("1, DU", gamepad1.dpad_up); - telemetry.addData("1, DL", gamepad1.dpad_left); - telemetry.addData("1, DD", gamepad1.dpad_down); - telemetry.addData("1, DR", gamepad1.dpad_right); - telemetry.addData("1, A", gamepad1.a); - telemetry.addData("1, B", gamepad1.b); - telemetry.addData("1, X", gamepad1.x); - telemetry.addData("1, Y", gamepad1.y); - - telemetry.addData("2, LT", gamepad2.left_trigger); - telemetry.addData("2, RT", gamepad2.right_trigger); - telemetry.addData("2, LB", gamepad2.left_bumper); - telemetry.addData("2, LB", gamepad2.right_bumper); - telemetry.addData("2, LT", gamepad2.left_trigger); - telemetry.addData("2, LS-X", gamepad2.left_stick_x); - telemetry.addData("2, LS-Y", gamepad2.left_stick_y); - telemetry.addData("2, RS-X", gamepad2.right_stick_x); - telemetry.addData("2, RS-Y", gamepad2.right_stick_y); - telemetry.addData("2, DU", gamepad2.dpad_up); - telemetry.addData("2, DL", gamepad2.dpad_left); - telemetry.addData("2, DD", gamepad2.dpad_down); - telemetry.addData("2, DR", gamepad2.dpad_right); - telemetry.addData("2, A", gamepad2.a); - telemetry.addData("2, B", gamepad2.b); - telemetry.addData("2, X", gamepad2.x); - telemetry.addData("2, Y", gamepad2.y); + // -- Runtime Telemetry -- + double hooperSpeed = (scoring[0].getVelocity()*60)/CPR; + String runtimeSeconds = String.format( + Locale.ENGLISH, "%.4f", getRuntime() + ); + + packet.clearLines(); + packet.put("", + "" + + "Runtime (Seconds): " + runtimeSeconds + "
" + + "
" + + "== Shooting Motor (scoring[0]) ==" + ); + + // runtimeSeconds shouldn't be graphable,so we make it a line + // instead of data. (FtcDashboard graph already adds time as X) + DASH.sendTelemetryPacket(packet); + telemetryD.addData("shooting-input-rpm", hooperPower); + telemetryD.addData("shooting-speed-rpm", hooperSpeed); + telemetryD.update(); + telemetryD.addData("
", ""); + telemetryD.update(); + telemetryD.addData("right-stick-x", rx); + telemetryD.addData("bot-heading", botHeadingDegrees); + telemetryD.addData("auto-shot", autoShot); + telemetryD.addData("ll-distance", rawDistance); + telemetryD.addData("lock-on", lockOn); + telemetryD.addData("valid-target", resultIsValid); + telemetryD.update(); + telemetry.addData("Runtime (Seconds)", runtimeSeconds); + telemetry.addLine(); + telemetry.addLine("== Shooting Motor (scoring[0]) =="); + telemetry.addData("shooting-input-rpm", hooperPower); + telemetry.addData("shooting-speed-rpm", hooperSpeed); + telemetry.addLine(); + telemetry.addData("right-stick-x", rx); + telemetry.addData("bot-heading", botHeadingDegrees); + telemetry.addData("auto-shot", autoShot); + telemetry.addData("ll-distance", rawDistance); + telemetry.addData("lock-on", lockOn); + telemetry.addData("valid-target", resultIsValid); telemetry.update(); - if (gamepad1.a && gamepad1.b && gamepad1.x && gamepad1.y) + if (a1 && b1 && x1 && y1) { logger.stopRun(); } - if (gamepad1.start) + if (start1) { imu.resetYaw(); } + // LockOn toggle + if(leftBumper1 && ! pastLeftBumper1) + { + lockOn = !lockOn; + + } - if(gamepad2.y) + if(y1) { - bigHooperMotor.setPower(-0.9); + scoring[0].setPower(-0.9); } else { - bigHooperMotor.setVelocity((bigHooperPower / 60)*CPR); + scoring[0].setVelocity((hooperPower/60) * CPR); } - if (gamepad1.a) + if (rightBumper1 && a1) + { + scoring[1].setPower(-0.2); + } else if (a1) { - smallHooperMotor.setPower(0.9); - } else if (gamepad1.x) { - smallHooperMotor.setPower(-0.9); + scoring[1].setVelocity((1150/60)*28); } else { - smallHooperMotor.setPower(0); + scoring[1].setPower(0); } + if(rightBumper1 && x1) + { + scoring[2].setPower(-0.9); + } else if(x1) + { + scoring[2].setPower(0.9); + } else + { + scoring[2].setPower(0); + } + + // "Artemis Take the Flywheel" toggle + if(rightTrigger1 == 1 && b1 && ! pastB2) + { + autoShot = !autoShot; + } + + base[0].setPower(frontLeftPower*driveBreakPower); + base[1].setPower(backLeftPower*driveBreakPower); + base[2].setPower(backRightPower*driveBreakPower); + base[3].setPower(frontRightPower*driveBreakPower); - frontLeftMotor.setPower(frontLeftPower*brakePower); - backLeftMotor.setPower(backLeftPower*brakePower); - frontRightMotor.setPower(frontRightPower*brakePower); - backRightMotor.setPower(backRightPower*brakePower); + pastLeftBumper1 = leftBumper1; + pastB2 = b1; } } @@ -166,19 +338,19 @@ private DiagnosticLogger getLogger() { logger = new DiagnosticLogger( telemetry, hardwareMap, + null, new String[] - { - "frontLeftMotor", "backLeftMotor", - "frontRightMotor", "backRightMotor", - "bigHooperMotor", "smallHooperMotor" - }, - null, null, "imu", parameters + { + "fl", "rl", + "fr", "rr", + "hooper", "intake1", "intake2" + }, + null, "imu", parameters ); } catch (IOException e) { - telemetry.addData("IOException", e.getMessage()); throw new RuntimeException(e); } return logger; } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java index 6bb843fe9cd1..4ffe48a31655 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java @@ -295,10 +295,10 @@ public void runOpMode() if (rightBumper2 && a2) { - scoring[1].setPower(-0.9); + scoring[1].setPower(-0.2); } else if (a2) { - scoring[1].setPower(0.9); + scoring[1].setVelocity((1150/60)*28); } else { scoring[1].setPower(0); From 806f4e66dcdf12d5f6833e99c2a24055ae52836a Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Fri, 6 Feb 2026 19:05:59 -0500 Subject: [PATCH 14/16] uhh pp --- .../firstinspires/ftc/teamcode/AutoBasic.java | 23 ++-- .../ftc/teamcode/AutoBlueBack.java | 33 +++++ .../ftc/teamcode/AutoBluefromGoal.java | 127 ++++++++++++++++++ .../ftc/teamcode/AutoForward.java | 26 ++++ .../ftc/teamcode/AutoRedBack.java | 33 +++++ .../ftc/teamcode/AutoRedfromGoal.java | 125 +++++++++++++++++ .../ftc/teamcode/TeleOp/Field2Drivers.java | 3 + .../ftc/teamcode/TwoBallAutoBlue.java | 69 ++++++++++ .../ftc/teamcode/TwoBallAutoRed.java | 69 ++++++++++ .../ftc/teamcode/pedroPathing/Constants.java | 10 +- .../ftc/teamcode/pedroPathing/Tuning.java | 6 +- .../ftc/teamcode/util/TeamConstants.java | 2 +- 12 files changed, 508 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java index 2c7dee8ac465..50c8e94eab68 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBasic.java @@ -35,12 +35,12 @@ public void runOpMode() throws InterruptedException { base[1].setPower(0.25); base[2].setPower(-0.25); base[3].setPower(-0.25); - sleep(1000); + sleep(950); base[0].setPower(0); base[1].setPower(0); base[2].setPower(0); base[3].setPower(0); - scoring[0].setVelocity(1920); + scoring[0].setVelocity(1825); sleep(5000); scoring[1].setPower(1); scoring[2].setPower(1); @@ -54,21 +54,22 @@ public void runOpMode() throws InterruptedException { scoring[0].setPower(0); scoring[1].setPower(0); scoring[2].setPower(0); + base[0].setPower(0.25); + base[1].setPower(0.25); + base[2].setPower(0.25); + base[3].setPower(0.25); + sleep(1250); base[0].setPower(-0.25); base[1].setPower(-0.25); base[2].setPower(0.25); base[3].setPower(0.25); sleep(3400); - base[0].setPower(-0.5); - base[1].setPower(0.5); - base[2].setPower(-0.5); - base[3].setPower(0.5); + base[0].setPower(0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(-0.5); sleep(2300); - base[0].setPower(-0.25); - base[1].setPower(-0.25); - base[2].setPower(-0.25); - base[3].setPower(-0.25); - sleep(1750); + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java new file mode 100644 index 000000000000..a7103d127a38 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBlueBack.java @@ -0,0 +1,33 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBlueBack extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(430); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java new file mode 100644 index 000000000000..5f6c5ce01b54 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoBluefromGoal.java @@ -0,0 +1,127 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoBluefromGoal extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1550); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1855); + sleep(4000); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(2500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(327); + base[0].setPower(-0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(0.5); + sleep(638); + scoring[1].setPower(1); + scoring[2].setPower(-.70); + scoring[0].setPower(-1); + base[0].setPower(0.22); + base[1].setPower(0.22); + base[2].setPower(0.22); + base[3].setPower(0.22); + sleep(5950); + scoring[1].setPower(-.35); + scoring[2].setPower(-.35); + sleep(100); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1000); + scoring[1].setPower(0); + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(580); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(310); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1745); + sleep(4000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(3300); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(230); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(70); + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(1030); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java new file mode 100644 index 000000000000..756cf0f60a3f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoForward.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoForward extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(750); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java new file mode 100644 index 000000000000..1ae938d4eae1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedBack.java @@ -0,0 +1,33 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoRedBack extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(430); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java new file mode 100644 index 000000000000..3b0603e020b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRedfromGoal.java @@ -0,0 +1,125 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class AutoRedfromGoal extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1555); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1875); + sleep(4000); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(2500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(260); + base[0].setPower(0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(-0.5); + sleep(520); + scoring[1].setPower(1); + scoring[2].setPower(-.65); + scoring[0].setPower(-1); + base[0].setPower(0.22); + base[1].setPower(0.22); + base[2].setPower(0.22); + base[3].setPower(0.22); + sleep(8000); + scoring[1].setPower(-.3); + scoring[2].setPower(-.3); + sleep(150); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1000); + scoring[1].setPower(0); + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(580); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(235); + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1660); + sleep(4000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(3000); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(800); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(230); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(80); + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(1030); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java index 6bb843fe9cd1..9c14b3453a3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java @@ -216,6 +216,9 @@ public void runOpMode() // Denominator is the largest motor power (absolute value) or 1 // This ensures all the powers maintain the same ratio, // but only if at least one is out of the range [-1, 1] + + // Sample of our code, Please do not touch + double denominator = Math.max( Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1 ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java new file mode 100644 index 000000000000..b487fce5fd73 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoBlue.java @@ -0,0 +1,69 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class TwoBallAutoBlue extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1555); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1875); + sleep(4700); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(2500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); + sleep(260); + base[0].setPower(1); + base[1].setPower(-1); + base[2].setPower(1); + base[3].setPower(-1); + sleep(1000); + + + } +}; \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java new file mode 100644 index 000000000000..af419e9886d2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoBallAutoRed.java @@ -0,0 +1,69 @@ + + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.util.TeamConstants; + +@Autonomous +public class TwoBallAutoRed extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + // Declare motors + // Make sure your ID's match your configuration + final DcMotorEx[] base = TeamConstants.getDriveMotors(hardwareMap); + final DcMotorEx[] scoring = TeamConstants.getScoringMotors(hardwareMap); + + waitForStart(); + + if(isStopRequested()) return; + + base[0].setPower(-0.5); + base[1].setPower(-0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(1400); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(40); + /*base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(0.5); + base[3].setPower(0.5); */ + base[0].setPower(0); + base[1].setPower(0); + base[2].setPower(0); + base[3].setPower(0); + scoring[0].setVelocity(1800); + sleep(4800); + scoring[1].setPower(0); + scoring[2].setPower(1); + sleep(250);; + scoring[1].setPower(0); + scoring[2].setPower(0); + sleep(3500); + scoring[1].setPower(1); + scoring[2].setPower(1); + sleep(650); + scoring[0].setPower(0); + scoring[1].setPower(0); + scoring[2].setPower(0); + base[0].setPower(0.5); + base[1].setPower(0.5); + base[2].setPower(-0.5); + base[3].setPower(-0.5); + sleep(260); + base[0].setPower(-1); + base[1].setPower(1); + base[2].setPower(-1); + base[3].setPower(1); + sleep(1000); + + + } +}; \ No newline at end of file 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 index b40cb1772725..06970b31332f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -16,9 +16,9 @@ @Config public class Constants { - public static double multiplierForward = 0.008294676287147268; - public static double multiplierStrafe = 0.008038662919454933; - public static double multiplierTurn = 0.02731472448271; + public static double multiplierForward = 0.006140795931692835; + public static double multiplierStrafe = -0.0072753221875072275; + public static double multiplierTurn = 0.015429377289117809; public static FollowerConstants followerConstants = new FollowerConstants() @@ -33,6 +33,8 @@ public class Constants { .leftFrontMotorDirection(DcMotorSimple.Direction.FORWARD) .leftRearMotorDirection(DcMotorSimple.Direction.FORWARD) .rightRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .xVelocity(50.15369745035338) + // .yVelocity(56) .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD); public static DriveEncoderConstants localizerConstants = new DriveEncoderConstants() .leftFrontMotorName("fl") @@ -72,6 +74,8 @@ public static Follower createFollower(HardwareMap hardwareMap) { .pathConstraints(pathConstraints) .mecanumDrivetrain(driveConstants) .driveEncoderLocalizer(localizerConstants) + // .forwardZeroPowerAcceleration(deceleration) + // .lateralZeroPowerAcceleration(deceleration) .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 index 7c0496d4212d..bd20d3ff08b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -184,7 +184,7 @@ public void loop() { * @version 1.0, 5/6/2024 */ class ForwardTuner extends OpMode { - public static double DISTANCE = 48; + public static double DISTANCE = 96; @Override public void init() { @@ -233,7 +233,7 @@ public void loop() { * @version 2.0, 6/26/2025 */ class LateralTuner extends OpMode { - public static double DISTANCE = 48; + public static double DISTANCE = 96; @Override public void init() { @@ -281,7 +281,7 @@ public void loop() { * @version 1.0, 5/6/2024 */ class TurnTuner extends OpMode { - public static double ANGLE = 2 * Math.PI; + public static double ANGLE = 4 * Math.PI; @Override public void init() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java index 7a55b78a4477..e9d487cd02d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/TeamConstants.java @@ -15,7 +15,7 @@ public class TeamConstants { // Counts-Per-Revolution of a REV HD Hex Motor. public static int CPR = 28; - public static double LOCK_ON_DENOMINATOR = 27.25; + public static double LOCK_ON_DENOMINATOR = 35.25; public static double LOCK_ON_OFFSET = 0.0; public static double LL_MOUNT_ANGLE = 30; From ec7005f7f4ecc0a996e798082d5d9f4761305c61 Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Thu, 19 Feb 2026 19:57:32 -0500 Subject: [PATCH 15/16] Psuedo Encoders --- .../ftc/teamcode/ServoPsuedoEncoderTest.java | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java new file mode 100644 index 000000000000..51b626a01866 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoPsuedoEncoderTest.java @@ -0,0 +1,47 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; + +@TeleOp +public class ServoPsuedoEncoderTest extends LinearOpMode { + + private CRServo crServo; + + @Override + public void runOpMode() { + + // Get the CR servo from the hardware map + crServo = hardwareMap.get(CRServo.class, "cr_servo"); + + // Optionally set the direction + //crServo.setDirection(CRServo.Direction.FORWARD); + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + + // Drive with right stick Y (gamepad1) + int Degrees = 1; + double Servo_degree_time = 1.13666666; + double Target_time = Degrees*Servo_degree_time; + crServo.setPower(0.9); + try { + Thread.sleep((long) Target_time); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + crServo.setpower(0); + + telemetry.addData("CR Servo Power", power); + telemetry.update(); + } + + // Stop the servo when the op mode ends + crServo.setPower(0); + } +} \ No newline at end of file From 2b2a4e5e78152c6ce740c19d1862be38cc13228b Mon Sep 17 00:00:00 2001 From: Avi Ramdial Date: Tue, 24 Feb 2026 17:17:10 -0500 Subject: [PATCH 16/16] new AS slope --- .../ftc/teamcode/TeleOp/Field2Drivers.java | 50 ++++++++++++------- 1 file changed, 32 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java index 9c14b3453a3c..99efdb141261 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOp/Field2Drivers.java @@ -152,31 +152,45 @@ public void runOpMode() tempDistance = rawDistance; pastDistance = tempDistance; - if(tempDistance >= 81.5) + if(tempDistance >= 65.3) { - hooperPower = (6.25*tempDistance)-644; - } else if(tempDistance >= 76.2) + hooperPower = (28.6*tempDistance)+1484; + } else if(tempDistance >= 62.5) { - hooperPower = (18.9*tempDistance)+2912; - } else if(tempDistance >= 65.7) + hooperPower = (17.9*tempDistance)+2184; + } else if(tempDistance >= 46.2) { - hooperPower = (33.3*tempDistance)+1810; - } else if(tempDistance >= 59.1) - { - hooperPower = (22.7*tempDistance)+2507; - } else if(tempDistance >= 54.6) - { - hooperPower = (-22.2*tempDistance)+5163; - } else if(tempDistance >= 47.5) - { - hooperPower = 3950; - } else if(tempDistance >= 40.4) - { - hooperPower = (-14.1*tempDistance)+4619; + hooperPower = (4.6*tempDistance)+3012; } else { hooperPower = 3000; } + + //if(tempDistance >= 81.5) + //{ + // hooperPower = (6.25*tempDistance)-644; + //} else if(tempDistance >= 76.2) + //{ + // hooperPower = (18.9*tempDistance)+2912; + //} else if(tempDistance >= 65.7) + //{ + // hooperPower = (33.3*tempDistance)+1810; + //} else if(tempDistance >= 59.1) + //{ + // hooperPower = (22.7*tempDistance)+2507; + //} else if(tempDistance >= 54.6) + //{ + // hooperPower = (-22.2*tempDistance)+5163; + //} else if(tempDistance >= 47.5) + //{ + // hooperPower = 3950; + //} else if(tempDistance >= 40.4) + //{ + // hooperPower = (-14.1*tempDistance)+4619; + //} else + //{ + // hooperPower = 3000; + //} meanDistance = 0; } else {