From d1efde9463d17ffee973b47e7b62710f1ab6c514 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Tue, 9 Dec 2025 10:37:26 -0500 Subject: [PATCH 01/41] Created branch and created a new class SorterControls Co-Authored-By: Jeffrey Mello <61124766+melloCoding@users.noreply.github.com> --- .../Utils_13233/MotorConstructor.java | 19 ++-------- .../teamcode/Utils_13233/SorterControls.java | 36 +++++++++++++++++++ 2 files changed, 38 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index d4796ea..5505b2c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -43,18 +43,7 @@ public class MotorConstructor { */ public DcMotor Launcher2; - - /** - * One of the servos used to carry the balls up the ramp - */ - public CRServo rampServo1; - /** - * One of the servos used to carry the balls up the ramp - */ - public CRServo rampServo2; - public CRServo rampServo3; - public CRServo rampServo4; - + public DcMotor Sorter; public VoltageSensor VoltSens; public IMU imu; @@ -72,11 +61,7 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher = hardwareMap.get(DcMotor.class, "Launcher"); Launcher2 = hardwareMap.get(DcMotor.class, "Launcher2"); - // Map the ramp servos - rampServo1 = hardwareMap.get(CRServo.class, "rampServo1"); - rampServo2 = hardwareMap.get(CRServo.class, "rampServo2"); - rampServo3 = hardwareMap.get(CRServo.class, "rampServo3"); - rampServo4 = hardwareMap.get(CRServo.class, "rampServo4"); + Sorter = hardwareMap.get(DcMotor.class, "Sorter"); // Control hub voltage sensor // Used to move at a constant speed in auto regardless of battery voltage diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java new file mode 100644 index 0000000..4fd058e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.HardwareMap; + + +public class SorterControls { + //TODO: Update values to the real positions of the motor + public double intakePos1 = 1; + public double intakePos2 = 2; + public double intakePos3 = 3; + + //TODO: Update values to the real positions of the motor + public double LaunchPos1 = 1; + public double LaunchPos2 = 2; + public double LaunchPos3 = 3; + private MotorConstructor motors; + + + public SorterControls(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + } + + public enum intakePos { + pos1, + pos2, + pos3 + } + + public enum launchPos { + po1, + pos2, + pos3 + } + + +} From 3bb03b2b031d1049f47c19a0bd9e1770c5d4a2b2 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 9 Dec 2025 16:03:12 -0500 Subject: [PATCH 02/41] Refactor SorterControls for improved position handling Changed intake and launch position variables from double to int for motor compatibility. Added methods to move the sorter to specific intake or launch positions using enums, and introduced a unified moveSorterToPos method. Updated enums for clarity and consistency. --- .../teamcode/Utils_13233/SorterControls.java | 113 ++++++++++++++++-- 1 file changed, 100 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 4fd058e..801ba87 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -1,18 +1,21 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; public class SorterControls { //TODO: Update values to the real positions of the motor - public double intakePos1 = 1; - public double intakePos2 = 2; - public double intakePos3 = 3; - + public int intakePos1 = 1; + public int intakePos2 = 2; + public int intakePos3 = 3; + //TODO: Update values to the real positions of the motor - public double LaunchPos1 = 1; - public double LaunchPos2 = 2; - public double LaunchPos3 = 3; + public int LaunchPos1 = 1; + public int LaunchPos2 = 2; + public int LaunchPos3 = 3; private MotorConstructor motors; @@ -20,16 +23,100 @@ public SorterControls(HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); } + /** + * Move the sorter to a position to intake from + * + * @param pos Enum to determine what positon to move to + */ + void moveToIntakePos(intakePos pos) { + switch (pos) { + case INTAKE_POS_1: + motors.Sorter.setTargetPosition(intakePos1); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(1); + break; + case INTAKE_POS_2: + motors.Sorter.setTargetPosition(intakePos2); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(1); + break; + case INTAKE_POS_3: + motors.Sorter.setTargetPosition(intakePos3); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(1); + break; + } + } + + + /** + * Move the sorter to a position to launch from + * + * @param pos Enum to determine what positon to move to + */ + void moveToLaunchPos(launchPos pos) { + switch (pos) { + case LAUNCH_POS_1: + motors.Sorter.setTargetPosition(LaunchPos1); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(1); + break; + case LAUNCH_POS_2: + motors.Sorter.setTargetPosition(LaunchPos2); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(1); + break; + case LAUNCH_POS_3: + motors.Sorter.setTargetPosition(LaunchPos3); + motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motors.Sorter.setPower(1); + break; + } + } + + public void moveSorterToPos(sorterModes mode, int pos) { + if (mode == sorterModes.INTAKE) { + switch (pos) { + case 1: + moveToIntakePos(intakePos.INTAKE_POS_1); + break; + case 2: + moveToIntakePos(intakePos.INTAKE_POS_2); + break; + case 3: + moveToIntakePos(intakePos.INTAKE_POS_3); + break; + } + } else if (mode == sorterModes.LAUNCH) { + switch (pos) { + case 1: + moveToLaunchPos(launchPos.LAUNCH_POS_1); + break; + case 2: + moveToLaunchPos(launchPos.LAUNCH_POS_2); + break; + case 3: + moveToLaunchPos(launchPos.LAUNCH_POS_3); + break; + } + } + } + public enum intakePos { - pos1, - pos2, - pos3 + INTAKE_POS_1, + INTAKE_POS_2, + INTAKE_POS_3 } public enum launchPos { - po1, - pos2, - pos3 + LAUNCH_POS_1, + LAUNCH_POS_2, + LAUNCH_POS_3 + } + + public enum sorterModes { + INTAKE, + LAUNCH } From a83b901ea13aad4189a67e4b8590efcb43be4c43 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 11 Dec 2025 07:50:55 -0500 Subject: [PATCH 03/41] Add validation for sorter position in moveSorterToPos Added error handling to throw a RuntimeException if the position argument in moveSorterToPos is not 1, 2, or 3. This ensures only valid positions are accepted for both intake and launch modes. --- .../ftc/teamcode/Utils_13233/SorterControls.java | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 801ba87..ae80a00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -74,6 +74,14 @@ void moveToLaunchPos(launchPos pos) { } } + + /** + * Moves the sorter to a postion defined in mode and pos allows for both intake and launching + * + * @param mode Enum to determine weather to launch or intake + * @param pos Enum to determine what sorter position to move to can be 1, 2 or 3 + * @throws RuntimeException Throws a runtime error if pos is not 1, 2 or 3 + */ public void moveSorterToPos(sorterModes mode, int pos) { if (mode == sorterModes.INTAKE) { switch (pos) { @@ -86,6 +94,10 @@ public void moveSorterToPos(sorterModes mode, int pos) { case 3: moveToIntakePos(intakePos.INTAKE_POS_3); break; + default: + // throw error when pos is not 1, 2 or 3 + throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + + "say run it?"); } } else if (mode == sorterModes.LAUNCH) { switch (pos) { @@ -98,6 +110,10 @@ public void moveSorterToPos(sorterModes mode, int pos) { case 3: moveToLaunchPos(launchPos.LAUNCH_POS_3); break; + default: + // throw error when pos is not 1, 2 or 3 + throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + + "say run it?"); } } } From 24294fd6232d6fd31303b30f1db851413fbe56d3 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 11 Dec 2025 08:20:45 -0500 Subject: [PATCH 04/41] Refactor controls and add sorter functionality Removed redundant motor and servo assignments from CommonControls and moved intake control logic to RampControls. Added SorterControls integration to TeleOpMain, including gamepad mappings for sorter positions. Minor cleanup and typo fix in SorterControls. --- .../ftc/teamcode/CommonControls.java | 171 +----------------- .../ftc/teamcode/TeleOpMain.java | 36 +++- .../teamcode/Utils_13233/RampControls.java | 4 - .../teamcode/Utils_13233/SorterControls.java | 6 +- 4 files changed, 38 insertions(+), 179 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java index 1be2124..d8be7e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CommonControls.java @@ -10,182 +10,13 @@ * Class the holds common functions and variable for the 13233 DECODE robot */ public class CommonControls { - /** - * Front left drive motor - */ - public DcMotor leftFront; - /** - * Front right drive motor - */ - public DcMotor rightFront; - /** - * Back left drive motor - */ - public DcMotor leftBack; - /** - * Right back drive motor - */ - public DcMotor rightBack; - public float launchPower = 1.0f; - /** - * The motor for that controls the ball intake - */ - private DcMotor intake; - /** - * One of the motors used to shoot the balls - */ - private DcMotor Launcher; - /** - * One of the motors used to shoot the balls - */ - private DcMotor Launcher2; - /** - * One of the servos used to carry the balls up the ramp - */ - private CRServo rampServo1; - /** - * One of the servos used to carry the balls up the ramp - */ - private CRServo rampServo2; - private CRServo rampServo3; - private CRServo rampServo4; - /** * Constructor for the CommonControls class * * @param hardwareMap The hardware map for the robot */ public CommonControls(HardwareMap hardwareMap) { - // Map main Drive Motors - leftFront = hardwareMap.get(DcMotor.class, "leftFront"); - rightFront = hardwareMap.get(DcMotor.class, "rightFront"); - leftBack = hardwareMap.get(DcMotor.class, "leftBack"); - rightBack = hardwareMap.get(DcMotor.class, "rightBack"); - - // Map Intake Motor - intake = hardwareMap.get(DcMotor.class, "intake"); - - // Map the launcher Prototype - Launcher = hardwareMap.dcMotor.get("Launcher"); - Launcher2 = hardwareMap.dcMotor.get("Launcher2"); - rampServo1 = hardwareMap.crservo.get("rampServo1"); - rampServo2 = hardwareMap.crservo.get("rampServo2"); - rampServo3 = hardwareMap.crservo.get("rampServo3"); - rampServo4 = hardwareMap.crservo.get("rampServo4"); - // Set direction of the main drive motors - leftFront.setDirection(DcMotorSimple.Direction.FORWARD); - rightFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftBack.setDirection(DcMotorSimple.Direction.FORWARD); - rightBack.setDirection(DcMotorSimple.Direction.REVERSE); - - // Set the direction of the intake motor - intake.setDirection(DcMotorSimple.Direction.REVERSE); - } - - - // TODO: - // Implement field oriented drive - - /** - * Updates all the drive controls based on the current gamepad stick positions - * - * @param controlLeftStickY Left stick Y setting, up/down - * @param controlLeftStickX Left stick X setting, left/right - * @param controlRightStick Right Stick setting, controls left right turn - * @param speedLimiter Trigger setting, controls speed of the robot - */ - public void setDrivePower(float controlLeftStickY, float controlLeftStickX, - float controlRightStick, float speedLimiter) { - // For readability and flexibility for future control input changes - //noinspection UnnecessaryLocalVariable - float forward = controlLeftStickY; - //noinspection UnnecessaryLocalVariable - float strafe = controlLeftStickX; - //noinspection UnnecessaryLocalVariable - float rotate = controlRightStick; - float speed = Math.max(1.0f - speedLimiter, 0.5f); - - - // Calculate the power for each motor - // Remember the direction of the stick is the opposite of what you would expect - // up is in the negatives and down is in the positives - float frontRightPower = (forward + strafe + rotate) * (speed); - float frontLeftPower = (forward - strafe - rotate) * (speed); - float backRightPower = (forward - strafe + rotate) * (speed); - float backLeftPower = (forward + strafe - rotate) * (speed); - - // Set the power for each of the motors - rightFront.setPower(frontRightPower); - leftFront.setPower(frontLeftPower); - rightBack.setPower(backRightPower); - leftBack.setPower(backLeftPower); - } - - /** - * Updates just the forward and back movement based on controller stick position - * - * @param controlLeftStickY Left stick Y setting, up/down - */ - public void setDrivePower(float controlLeftStickY) { - setDrivePower(controlLeftStickY, 0.0f, 0.0f, 0.0f); - } - - - /** - * Sets the power of all of the launch motors depending on the value of launchInput - * - * @param launchInput Button mapped to launch input - */ - void setLaunchPower(boolean launchInput) { - double power = launchInput ? launchPower : 0.0; - - // Set the the power value to the motors - Launcher.setPower(power); - Launcher2.setPower(-power); - } - - /** - * Controls the spin direction of the intake wheel based on controller buttons - * - * @param intakeForwardInput Button mapped to forward input - * @param intakeReverseInput Button mapped to reverse input - */ - void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) { - double intakePower = 0; - // ^ is the XOR operator, will return true if only one variable is true - // If intakeForwardInput OR intakeReverseInput is true then this is true, but not - // if both are true, which is why this differs from the || logical or operator - if (intakeForwardInput ^ intakeReverseInput) { - // This uses an inline-if statement which is useful when assigning values to variables - // This says set power = 1 if intakeForwardInput is true, else set it to -1 - intakePower = intakeForwardInput ? 1 : -1; - } - rampServo1.setPower(-intakePower); - rampServo2.setPower(intakePower); - rampServo3.setPower(intakePower); - rampServo4.setPower(-intakePower); - intake.setPower(intakePower); - } - - void setDriveBrake() { - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } - - void setDriveFloat() { - leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - } - void setDriveMotorZeroPowerBehavior(boolean input) { - if (input) { - setDriveBrake(); - } else { - setDriveFloat(); - } } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index e995556..9019b0b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -1,12 +1,16 @@ // Import libraries package org.firstinspires.ftc.teamcode; +import static com.qualcomm.robotcore.hardware.Gamepad.RUMBLE_DURATION_CONTINUOUS; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; @TeleOp(name = "TeleOpMain", group = "Drive") @@ -16,6 +20,7 @@ public class TeleOpMain extends LinearOpMode { private DriveControls drive; private LaunchControls launch; private RampControls ramp; + private SorterControls sorter; //This function is executed when this Op Mode is selected from the Driver Station @@ -25,6 +30,9 @@ public void runOpMode() { drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); ramp = new RampControls(hardwareMap); + sorter = new SorterControls(hardwareMap); + + // Wait for the game to start (driver presses PLAY) // The waitForStart() function will wait for the start button to begin // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())" @@ -37,8 +45,34 @@ public void runOpMode() { telemetry.update(); // Set the power to the launch motors based while the x button is being pressed + // and rumble to let the driver know that the launch motors are being controlled + + launch.setLaunchPower(gamepad1.x); // spin up launch motors + if (gamepad1.x) { + gamepad1.rumble(1.0, 1.0, RUMBLE_DURATION_CONTINUOUS); + } else { + gamepad1.rumble(0.0, 0.0, RUMBLE_DURATION_CONTINUOUS); + } + + // Sorter Controls + // Intake positions + if (gamepad2.x) { + sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 1); + } else if (gamepad2.y) { + sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 2); + } else if (gamepad2.b) { + sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 3); + } + + // Launch positons + if (gamepad2.dpad_left) { + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + } else if (gamepad2.dpad_up) { + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + } else if (gamepad2.dpad_right) { + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + } - launch.setLaunchPower(gamepad1.x); //Add option to enable brakes when sharbell holds a drive.setDriveMotorZeroPowerBehavior(gamepad1.a); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java index 4154f50..d5f74b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java @@ -25,10 +25,6 @@ public void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverse // This says set power = 1 if intakeForwardInput is true, else set it to -1 intakePower = intakeForwardInput ? 1 : -1; } - motors.rampServo1.setPower(-intakePower); - motors.rampServo2.setPower(intakePower); - motors.rampServo3.setPower(intakePower); - motors.rampServo4.setPower(-intakePower); motors.intake.setPower(intakePower); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index ae80a00..77944b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode.Utils_13233; -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; - import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -16,7 +14,7 @@ public class SorterControls { public int LaunchPos1 = 1; public int LaunchPos2 = 2; public int LaunchPos3 = 3; - private MotorConstructor motors; + private final MotorConstructor motors; public SorterControls(HardwareMap hardwareMap) { @@ -76,7 +74,7 @@ void moveToLaunchPos(launchPos pos) { /** - * Moves the sorter to a postion defined in mode and pos allows for both intake and launching + * Moves the sorter to a position defined in mode and pos allows for both intake and launching * * @param mode Enum to determine weather to launch or intake * @param pos Enum to determine what sorter position to move to can be 1, 2 or 3 From ef9eea062f080bece6bb542e9b8d860b1e06c7b8 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 15 Dec 2025 08:53:54 -0500 Subject: [PATCH 05/41] Add flipper servo and enhance launch controls Introduced a Flipper servo to MotorConstructor and integrated its control in TeleOpMain. Expanded LaunchControls with overloaded setLaunchPower methods to support variable power and controller rumble feedback. --- .../ftc/teamcode/TeleOpMain.java | 12 +++ .../teamcode/Utils_13233/LaunchControls.java | 75 ++++++++++++++++++- .../Utils_13233/MotorConstructor.java | 7 +- 3 files changed, 92 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 9019b0b..6573fac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -6,8 +6,10 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; @@ -21,6 +23,7 @@ public class TeleOpMain extends LinearOpMode { private LaunchControls launch; private RampControls ramp; private SorterControls sorter; + private MotorConstructor motors; //This function is executed when this Op Mode is selected from the Driver Station @@ -31,6 +34,8 @@ public void runOpMode() { launch = new LaunchControls(hardwareMap); ramp = new RampControls(hardwareMap); sorter = new SorterControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + // Wait for the game to start (driver presses PLAY) @@ -72,6 +77,13 @@ public void runOpMode() { } else if (gamepad2.dpad_right) { sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); } + if (gamepad1.a) { + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(7.0); + } else { + motors.Flipper.setDirection(Servo.Direction.FORWARD); + motors.Flipper.setPosition(0.0); + } //Add option to enable brakes when sharbell holds a diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index b5a5533..9261cf6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; public class LaunchControls { @@ -10,7 +11,7 @@ public LaunchControls(HardwareMap hardwareMap) { } /** - * Sets the power of all of the launch motors depending on the value of launchInput + * Sets the power of all of the launch motors to power 1.0 if launchInput is true * * @param launchInput Button mapped to launch input */ @@ -22,4 +23,76 @@ public void setLaunchPower(boolean launchInput) { motors.Launcher2.setPower(power); } + /** + * Sets the power of all of the launch motors to launchPower depending on if launchInput is true + * + * @param launchInput Button mapped to launch input + * @param launchPower Power to set the motors to + */ + public void setLaunchPower(boolean launchInput, double launchPower) { + // set launch equal to launchPower if launchInput is true + double power = launchInput ? launchPower : 0.0; + + // Set the the power value to the motors + motors.Launcher.setPower(-power); + motors.Launcher2.setPower(power); + } + + /** + * Sets the power of all of the launch motors to power 1.0 if launchInput is true and allows + * for the controllers to rumble + * + * @param launchInput Button mapped to launch input + * @param rumble is rumble active + * @param rumbleGamepad what gamepad to rumble + * @throws RuntimeException throws when no gamepad is specified + */ + public void setLaunchPower(boolean launchInput, boolean rumble, Gamepad rumbleGamepad) { + // set the launch power to 1.0 if launchInput is true + double power = launchInput ? 1.0 : 0.0; + + // Set the rumble power of the motors + if (rumble) { + rumbleGamepad.rumble(1.0, 1.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); + } else if (rumbleGamepad == null) { + throw new RuntimeException("No controller specified for rumble"); // handle errors + } else { + rumbleGamepad.rumble(0.0, 0.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); + } + + // Set the the power value to the motors + motors.Launcher.setPower(-power); + motors.Launcher2.setPower(power); + } + + + /** + * Sets the power of all of the launch motors to power launchPower if launchInput is true and allows + * for the controllers to rumble + * + * @param launchInput Button mapped to launch input + * @param launchPower Power to set the motors to + * @param rumble is rumble active + * @param rumbleGamepad what gamepad to rumble + * @throws RuntimeException throws when no gamepad is specified + */ + public void setLaunchPower(boolean launchInput, double launchPower, boolean rumble, + Gamepad rumbleGamepad) { + // set the launch power to launchPower if launchInput is true + double power = launchInput ? launchPower : 0.0; + + // Set the rumble power of the motors + if (rumble) { + rumbleGamepad.rumble(1.0, 1.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); + } else if (rumbleGamepad == null) { + throw new RuntimeException("No controller specified for rumble"); // handle errors + } else { + rumbleGamepad.rumble(0.0, 0.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); + } + + // Set the the power value to the motors + motors.Launcher.setPower(-power); + motors.Launcher2.setPower(power); + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 5505b2c..6ed4822 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.VoltageSensor; /** @@ -44,6 +45,7 @@ public class MotorConstructor { public DcMotor Launcher2; public DcMotor Sorter; + public Servo Flipper; public VoltageSensor VoltSens; public IMU imu; @@ -61,7 +63,8 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher = hardwareMap.get(DcMotor.class, "Launcher"); Launcher2 = hardwareMap.get(DcMotor.class, "Launcher2"); - Sorter = hardwareMap.get(DcMotor.class, "Sorter"); + Sorter = hardwareMap.get(DcMotor.class, "sorter"); + Flipper = hardwareMap.get(Servo.class, "flipper"); // Control hub voltage sensor // Used to move at a constant speed in auto regardless of battery voltage @@ -84,6 +87,8 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); Launcher2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + //Flipper.setDirection(Servo.Direction.REVERSE); + // Set the direction of the intake motor intake.setDirection(DcMotorSimple.Direction.REVERSE); From 04345f32f5d2bd586d41f969d0f7f0a72644a7a6 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 15 Dec 2025 09:03:20 -0500 Subject: [PATCH 06/41] Refactor launch control and add loop delay Replaced manual rumble logic with a single call to launch.setLaunchPower, passing additional parameters. Added a 50ms sleep at the end of the loop to address issues with motors not working properly due to a fast loop rate. --- .../java/org/firstinspires/ftc/teamcode/TeleOpMain.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 6573fac..fccd829 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -53,11 +53,7 @@ public void runOpMode() { // and rumble to let the driver know that the launch motors are being controlled launch.setLaunchPower(gamepad1.x); // spin up launch motors - if (gamepad1.x) { - gamepad1.rumble(1.0, 1.0, RUMBLE_DURATION_CONTINUOUS); - } else { - gamepad1.rumble(0.0, 0.0, RUMBLE_DURATION_CONTINUOUS); - } + launch.setLaunchPower(gamepad1.x, true, gamepad1); // Sorter Controls // Intake positions @@ -102,6 +98,9 @@ public void runOpMode() { // Sets the power to the drive motors based on current gamepad inputs drive.setDrivePower(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x, gamepad1.left_trigger); + + // Slow down the loop because for some reason it goes to fast and causes the motors to + // not work properly sleep(50); } // Do not place any drive code here outside of the while loop (will fail inspection) From 2418ca8be78843d27c83a451f3072f2b6c7743b6 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 15 Dec 2025 09:04:05 -0500 Subject: [PATCH 07/41] Remove deprecated launch motor control call Eliminated the old call to launch.setLaunchPower(gamepad1.x) in favor of the updated method with additional parameters. This streamlines launch motor control and avoids redundant code. --- .../main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index fccd829..1f756de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -51,8 +51,6 @@ public void runOpMode() { // Set the power to the launch motors based while the x button is being pressed // and rumble to let the driver know that the launch motors are being controlled - - launch.setLaunchPower(gamepad1.x); // spin up launch motors launch.setLaunchPower(gamepad1.x, true, gamepad1); // Sorter Controls From 531990bcdd69d3608248c65be0327ce69a502563 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 5 Jan 2026 20:48:36 -0500 Subject: [PATCH 08/41] Refactor SorterControls and update motor settings SorterControls now receives MotorConstructor instead of HardwareMap, and intake/launch positions have been updated to new values. MotorConstructor initializes Sorter motor direction, zero power behavior, and encoder settings. Power for sorter movements is reduced from 1 to 0.6 for smoother operation. TeleOpMain now displays additional telemetry for the Sorter motor. --- .../ftc/teamcode/TeleOpMain.java | 8 ++++- .../Utils_13233/MotorConstructor.java | 6 ++++ .../teamcode/Utils_13233/SorterControls.java | 29 +++++++++---------- 3 files changed, 27 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 1f756de..945073a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -33,8 +33,8 @@ public void runOpMode() { drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); ramp = new RampControls(hardwareMap); - sorter = new SorterControls(hardwareMap); motors = new MotorConstructor(hardwareMap); + sorter = new SorterControls(motors); // Wait for the game to start (driver presses PLAY) @@ -47,6 +47,12 @@ public void runOpMode() { while (opModeIsActive()) { // Add status data to driver hub display telemetry.addData("Status", "opModeIsActive"); + telemetry.addData("Mode", motors.Sorter.getMode()); + telemetry.addData("Dir", motors.Sorter.getDirection()); + telemetry.addData("Target", motors.Sorter.getTargetPosition()); + telemetry.addData("Current", motors.Sorter.getCurrentPosition()); + telemetry.addData("Busy", motors.Sorter.isBusy()); + telemetry.update(); // Set the power to the launch motors based while the x button is being pressed diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 6ed4822..935f8e6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -87,6 +87,12 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); Launcher2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Sorter.setDirection(DcMotorSimple.Direction.REVERSE); + Sorter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Sorter.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + Sorter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + //Flipper.setDirection(Servo.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 77944b0..a123f17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -6,19 +6,18 @@ public class SorterControls { //TODO: Update values to the real positions of the motor - public int intakePos1 = 1; - public int intakePos2 = 2; - public int intakePos3 = 3; + public int intakePos1 = 0; + public int intakePos2 = 93; + public int intakePos3 = 191; //TODO: Update values to the real positions of the motor - public int LaunchPos1 = 1; - public int LaunchPos2 = 2; - public int LaunchPos3 = 3; + public int LaunchPos1 = 147; + public int LaunchPos2 = 242; + public int LaunchPos3 = 337; private final MotorConstructor motors; - - public SorterControls(HardwareMap hardwareMap) { - this.motors = new MotorConstructor(hardwareMap); + public SorterControls(MotorConstructor motors) { + this.motors = motors; } /** @@ -31,17 +30,17 @@ void moveToIntakePos(intakePos pos) { case INTAKE_POS_1: motors.Sorter.setTargetPosition(intakePos1); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(1); + motors.Sorter.setPower(0.6); break; case INTAKE_POS_2: motors.Sorter.setTargetPosition(intakePos2); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(1); + motors.Sorter.setPower(0.6); break; case INTAKE_POS_3: motors.Sorter.setTargetPosition(intakePos3); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(1); + motors.Sorter.setPower(0.6); break; } } @@ -57,17 +56,17 @@ void moveToLaunchPos(launchPos pos) { case LAUNCH_POS_1: motors.Sorter.setTargetPosition(LaunchPos1); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(1); + motors.Sorter.setPower(0.6); break; case LAUNCH_POS_2: motors.Sorter.setTargetPosition(LaunchPos2); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(1); + motors.Sorter.setPower(0.6); break; case LAUNCH_POS_3: motors.Sorter.setTargetPosition(LaunchPos3); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(1); + motors.Sorter.setPower(0.6); break; } } From 6e6fd569732e32e6ab3c1047cb5bc7f31750dd74 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Wed, 7 Jan 2026 10:38:30 -0500 Subject: [PATCH 09/41] Update TeleOpMain.java flipper update changes values to correct positions --- .../org/firstinspires/ftc/teamcode/TeleOpMain.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 945073a..b462139 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -52,7 +52,7 @@ public void runOpMode() { telemetry.addData("Target", motors.Sorter.getTargetPosition()); telemetry.addData("Current", motors.Sorter.getCurrentPosition()); telemetry.addData("Busy", motors.Sorter.isBusy()); - + telemetry.update(); // Set the power to the launch motors based while the x button is being pressed @@ -77,12 +77,12 @@ public void runOpMode() { } else if (gamepad2.dpad_right) { sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); } - if (gamepad1.a) { - motors.Flipper.setDirection(Servo.Direction.REVERSE); - motors.Flipper.setPosition(7.0); - } else { + if (gamepad1.y) { motors.Flipper.setDirection(Servo.Direction.FORWARD); motors.Flipper.setPosition(0.0); + } else { + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.3); } From 4b727baa9e794ba127da145cc8ad175041dff5d0 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Fri, 9 Jan 2026 17:35:55 -0500 Subject: [PATCH 10/41] sharbell's "interesting" changes i have no idea what he did probably something to do with encoders on launch motors --- .../ftc/teamcode/TeleOpMain.java | 18 ++-- .../teamcode/Utils_13233/LaunchControls.java | 83 ++++--------------- .../Utils_13233/MotorConstructor.java | 4 +- 3 files changed, 31 insertions(+), 74 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index b462139..a337479 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -57,7 +57,16 @@ public void runOpMode() { // Set the power to the launch motors based while the x button is being pressed // and rumble to let the driver know that the launch motors are being controlled - launch.setLaunchPower(gamepad1.x, true, gamepad1); + // launch.setLaunchPower(gamepad1.x, true, gamepad1); + if (gamepad1.x) { + launch.setLaunchPower(gamepad1.x, 0.3f); + sleep(50); + } else if (gamepad1.y) { + launch.setLaunchPower(gamepad1.y, 1.0f); + sleep(50); + } else { + launch.setLaunchPower(false); + } // Sorter Controls // Intake positions @@ -77,18 +86,17 @@ public void runOpMode() { } else if (gamepad2.dpad_right) { sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); } - if (gamepad1.y) { + if (gamepad1.a) { motors.Flipper.setDirection(Servo.Direction.FORWARD); motors.Flipper.setPosition(0.0); } else { motors.Flipper.setDirection(Servo.Direction.REVERSE); - motors.Flipper.setPosition(0.3); + motors.Flipper.setPosition(0.15); } //Add option to enable brakes when sharbell holds a - drive.setDriveMotorZeroPowerBehavior(gamepad1.a); - //Add option to enable brakes when driver 1 holds the "a" button + drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button drive.setDriveMotorZeroPowerBehavior(gamepad1.a); // Set all of the intake motors and servos to go forward when dpad up is pressed diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index 9261cf6..d7872fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -1,98 +1,47 @@ package org.firstinspires.ftc.teamcode.Utils_13233; -import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; public class LaunchControls { + private static ElapsedTime rpmTimer = new ElapsedTime(); private final MotorConstructor motors; + double launch1Rpm; + double launch2Rpm; + double avgLaunchRpm; public LaunchControls(HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); } /** - * Sets the power of all of the launch motors to power 1.0 if launchInput is true + * Sets the power of all of the launch motors depending on the value of launchInput * * @param launchInput Button mapped to launch input */ public void setLaunchPower(boolean launchInput) { - double power = launchInput ? 1.0 : 0.0; + double power = launchInput ? 0.62f : 0.0f; // Set the the power value to the motors motors.Launcher.setPower(-power); motors.Launcher2.setPower(power); } - /** - * Sets the power of all of the launch motors to launchPower depending on if launchInput is true - * - * @param launchInput Button mapped to launch input - * @param launchPower Power to set the motors to - */ - public void setLaunchPower(boolean launchInput, double launchPower) { - // set launch equal to launchPower if launchInput is true - double power = launchInput ? launchPower : 0.0; + public void setLaunchPower(boolean launchInput, float launchPower) { + double power = launchInput ? launchPower : 0.0f; // Set the the power value to the motors motors.Launcher.setPower(-power); motors.Launcher2.setPower(power); } - /** - * Sets the power of all of the launch motors to power 1.0 if launchInput is true and allows - * for the controllers to rumble - * - * @param launchInput Button mapped to launch input - * @param rumble is rumble active - * @param rumbleGamepad what gamepad to rumble - * @throws RuntimeException throws when no gamepad is specified - */ - public void setLaunchPower(boolean launchInput, boolean rumble, Gamepad rumbleGamepad) { - // set the launch power to 1.0 if launchInput is true - double power = launchInput ? 1.0 : 0.0; - - // Set the rumble power of the motors - if (rumble) { - rumbleGamepad.rumble(1.0, 1.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); - } else if (rumbleGamepad == null) { - throw new RuntimeException("No controller specified for rumble"); // handle errors - } else { - rumbleGamepad.rumble(0.0, 0.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); + public double launch1Speed() { + if (rpmTimer.milliseconds() >= 50) { + launch1Rpm = ((double) motors.Launcher.getCurrentPosition() / 28) * 1200; + motors.Launcher.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rpmTimer.reset(); } - - // Set the the power value to the motors - motors.Launcher.setPower(-power); - motors.Launcher2.setPower(power); + return (launch1Rpm); } - - - /** - * Sets the power of all of the launch motors to power launchPower if launchInput is true and allows - * for the controllers to rumble - * - * @param launchInput Button mapped to launch input - * @param launchPower Power to set the motors to - * @param rumble is rumble active - * @param rumbleGamepad what gamepad to rumble - * @throws RuntimeException throws when no gamepad is specified - */ - public void setLaunchPower(boolean launchInput, double launchPower, boolean rumble, - Gamepad rumbleGamepad) { - // set the launch power to launchPower if launchInput is true - double power = launchInput ? launchPower : 0.0; - - // Set the rumble power of the motors - if (rumble) { - rumbleGamepad.rumble(1.0, 1.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); - } else if (rumbleGamepad == null) { - throw new RuntimeException("No controller specified for rumble"); // handle errors - } else { - rumbleGamepad.rumble(0.0, 0.0, Gamepad.RUMBLE_DURATION_CONTINUOUS); - } - - // Set the the power value to the motors - motors.Launcher.setPower(-power); - motors.Launcher2.setPower(power); - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 935f8e6..3da79f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -84,8 +84,8 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher2.setDirection(DcMotorSimple.Direction.REVERSE); // Set the launch motors to run using encoders - Launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - Launcher2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Launcher.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + Launcher2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); Sorter.setDirection(DcMotorSimple.Direction.REVERSE); Sorter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); From a19b934af82fac5e484b6592e4812d8d84992828 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Tue, 13 Jan 2026 10:35:43 -0500 Subject: [PATCH 11/41] Positions for sorter Changing the positions for the sorter when switched to a 20:1 motor. Also changed X as a value for gamepad1 --- .../teamcode/Utils_13233/LaunchControls.java | 2 +- .../teamcode/Utils_13233/SorterControls.java | 19 +++++++++---------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index d7872fa..ed64abb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -21,7 +21,7 @@ public LaunchControls(HardwareMap hardwareMap) { * @param launchInput Button mapped to launch input */ public void setLaunchPower(boolean launchInput) { - double power = launchInput ? 0.62f : 0.0f; + double power = launchInput ? 0.75f : 0.0f; // Set the the power value to the motors motors.Launcher.setPower(-power); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index a123f17..d5a72e6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -5,16 +5,15 @@ public class SorterControls { + private final MotorConstructor motors; //TODO: Update values to the real positions of the motor - public int intakePos1 = 0; - public int intakePos2 = 93; - public int intakePos3 = 191; - + public int intakePos1 = 263; + public int intakePos2 = -87; + public int intakePos3 = 84; //TODO: Update values to the real positions of the motor - public int LaunchPos1 = 147; - public int LaunchPos2 = 242; - public int LaunchPos3 = 337; - private final MotorConstructor motors; + public int LaunchPos1 = 0; + public int LaunchPos2 = 176; + public int LaunchPos3 = 356; public SorterControls(MotorConstructor motors) { this.motors = motors; @@ -94,7 +93,7 @@ public void moveSorterToPos(sorterModes mode, int pos) { default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } else if (mode == sorterModes.LAUNCH) { switch (pos) { @@ -110,7 +109,7 @@ public void moveSorterToPos(sorterModes mode, int pos) { default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } } From 6fd807872b2c76b2a4e215785b3bc2062b3fe110 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 13 Jan 2026 15:53:54 -0500 Subject: [PATCH 12/41] Improve launcher and flipper control logic Added zero power brake behavior to launcher motors for better stopping control. Updated TeleOp logic to add a delay after launch actions and prevent flipper activation while the sorter is busy, improving safety and reliability. --- .../java/org/firstinspires/ftc/teamcode/TeleOpMain.java | 9 ++++++--- .../ftc/teamcode/Utils_13233/MotorConstructor.java | 2 ++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index a337479..6bc13a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -60,14 +60,14 @@ public void runOpMode() { // launch.setLaunchPower(gamepad1.x, true, gamepad1); if (gamepad1.x) { launch.setLaunchPower(gamepad1.x, 0.3f); - sleep(50); } else if (gamepad1.y) { launch.setLaunchPower(gamepad1.y, 1.0f); - sleep(50); } else { launch.setLaunchPower(false); } + sleep(100); + // Sorter Controls // Intake positions if (gamepad2.x) { @@ -86,7 +86,10 @@ public void runOpMode() { } else if (gamepad2.dpad_right) { sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); } - if (gamepad1.a) { + + // Control the flipper and make sure that when the sorter is moving the flipper is set + // to the not active positon + if (gamepad1.a && !motors.Sorter.isBusy()) { motors.Flipper.setDirection(Servo.Direction.FORWARD); motors.Flipper.setPosition(0.0); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 3da79f5..ff360e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -86,6 +86,8 @@ public MotorConstructor(HardwareMap hardwareMap) { // Set the launch motors to run using encoders Launcher.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); Launcher2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + Launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + Launcher2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); Sorter.setDirection(DcMotorSimple.Direction.REVERSE); Sorter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); From 7fcd6d3e41286b38bdf376e9ab4e72a4d8058a19 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Tue, 13 Jan 2026 17:43:36 -0500 Subject: [PATCH 13/41] added a new QuickAutoBlue Cycles the turntable, shooting all 3 balls (hopefully) sets the framework for a limelight auto and a quick auto red --- .../ftc/teamcode/QuickAutoBlue.java | 95 +++++++++++++++++++ .../ftc/teamcode/TeleOpMain.java | 6 +- .../teamcode/Utils_13233/RampControls.java | 2 +- 3 files changed, 99 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java new file mode 100644 index 0000000..99335b3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn; +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +@Autonomous(name = "QuickAutoBlue", group = "Autonomous") +public class QuickAutoBlue extends LinearOpMode { + + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + private AutoDrive autoDrive; + + private AutoTurn autoTurn; + + private LinearOpMode opMode; + + @Override + public void runOpMode() { + + drive = new DriveControls(hardwareMap); +// autoDrive = new AutoDrive(opMode, hardwareMap); +// autoTurn = new AutoTurn(opMode, hardwareMap); + launch = new LaunchControls(hardwareMap); + sorter = new SorterControls(motors); + + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + if (opModeIsActive()) { + + //autoDrive.driveForward(10.0, AutoConstants.quarterPower); + //autoTurn.rotate(90, AutoConstants.quarterPower); + + + //Jacob's so useful code + + drive.setDrivePower(-1.0f); + sleep(1200); + drive.setDriveMotorZeroPowerBehavior(true); + drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + drive.setDrivePower(0.0f);//stops + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 6bc13a2..c0ead8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -99,11 +99,11 @@ public void runOpMode() { //Add option to enable brakes when sharbell holds a - drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button + drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button drive.setDriveMotorZeroPowerBehavior(gamepad1.a); - // Set all of the intake motors and servos to go forward when dpad up is pressed - // and reverse when dpad down is pressed + + ramp.setIntakeDirection(gamepad2.dpad_up, gamepad2.dpad_down); // Wait for motors to speed up before changing value // While it is bad practice to put a sleep in a loop it is the only way to (that I know) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java index d5f74b8..f17fa7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.Utils_13233; import com.qualcomm.robotcore.hardware.HardwareMap; - +//THIS IS ALL USELESS NOW BY THE WAY public class RampControls { private final MotorConstructor motors; From 42cef7b533f6482eeca35e7e804a3b8e9dfd5f05 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Wed, 14 Jan 2026 21:11:29 -0500 Subject: [PATCH 14/41] quick auto red added inverted quickauto red literally the same thing as blue --- .../ftc/teamcode/QuickAutoBlue.java | 5 +- .../ftc/teamcode/QuickAutoRed.java | 105 ++++++++++++++++++ 2 files changed, 109 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java index 99335b3..6e64d23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java @@ -62,6 +62,7 @@ public void runOpMode() { sleep(1000);//runs flipper servo into motor motors.Flipper.setDirection(Servo.Direction.REVERSE); motors.Flipper.setPosition(0.15); + //2nd ball code sleep(750);//waits to spin turntable sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); @@ -71,6 +72,7 @@ public void runOpMode() { sleep(1000); motors.Flipper.setDirection(Servo.Direction.REVERSE); motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + //3rd ball code sleep(1000); sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); @@ -83,12 +85,13 @@ public void runOpMode() { sleep(1000); sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init launch.setLaunchPower(false);//turns off launcher and ramp + //strafing section drive.setDrivePower(0, 1.0f, 0, 0); sleep(1000); drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line sleep(1000); - drive.setDrivePower(0.0f);//stops + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java new file mode 100644 index 0000000..849ed09 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java @@ -0,0 +1,105 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn; +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +@Autonomous(name = "QuickAutoRed", group = "Autonomous") +public class QuickAutoRed extends LinearOpMode { + + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + private AutoDrive autoDrive; + + private AutoTurn autoTurn; + + private LinearOpMode opMode; + + @Override + public void runOpMode() { + + drive = new DriveControls(hardwareMap); +// autoDrive = new AutoDrive(opMode, hardwareMap); +// autoTurn = new AutoTurn(opMode, hardwareMap); + launch = new LaunchControls(hardwareMap); + sorter = new SorterControls(motors); + + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + if (opModeIsActive()) { + + //autoDrive.driveForward(10.0, AutoConstants.quarterPower); + //autoTurn.rotate(90, AutoConstants.quarterPower); + + + //Jacob's so useful code + + drive.setDrivePower(-1.0f); + sleep(1200); + drive.setDriveMotorZeroPowerBehavior(true); + drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(1.0f, 0, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + + + } + } +} +// 傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡,傑弗裡，你是說要運行它嗎？ +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡， +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡， +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡， +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡 +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡 +//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡 From abbb4911938d397cf779ed28acaffb2ac7676e3f Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Thu, 15 Jan 2026 18:34:58 -0500 Subject: [PATCH 15/41] set up for limelight yes jeffery i know there are multiple HUGE errors --- .../ftc/teamcode/Utils_13233/LimeLight.java | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java new file mode 100644 index 0000000..25d1ec3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.IMU; + +public abstract class LimeLight extends LinearOpMode { + private Limelight3A limelight; + private IMU imu; + @Override + public void init() { + limelight = hardwareMap.get(Limelight3A.class,"limelight"); + limelight.pipelineSwitch(8); //remember to change limelight pipeline using browser + imu = hardwareMap.get(IMU.class,"imu"); + RevHubOrientationOnRobot revHubOrientationOnRobot = new RevHubOrientationOnRobot.LogoFacingDirection(RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, + RevHubOrientationOnRobot.UsbFacingDirection.UP); + imu.initalize(new IMU.Parameters(revHubOrientationOnRobot)); + public void start() { + limelight.start(); + } +} From 4ebac40f00775f816a914a550676cc63893ee0da Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Thu, 15 Jan 2026 21:32:23 -0500 Subject: [PATCH 16/41] limelight yes its still a buggy mess need to import limelight sdk but im getting closer to fixing issues --- .../ftc/teamcode/Utils_13233/LimeLight.java | 66 +++++++++++++++---- 1 file changed, 53 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java index 25d1ec3..1a76a00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java @@ -1,22 +1,62 @@ package org.firstinspires.ftc.teamcode.Utils_13233; -import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import com.limelightvision.limelight3a.*; + +@TeleOp(name = "Limelight 3A AprilTag Test", group = "Vision") +public class LimelightAprilTagTest extends LinearOpMode { -public abstract class LimeLight extends LinearOpMode { private Limelight3A limelight; - private IMU imu; + @Override - public void init() { - limelight = hardwareMap.get(Limelight3A.class,"limelight"); - limelight.pipelineSwitch(8); //remember to change limelight pipeline using browser - imu = hardwareMap.get(IMU.class,"imu"); - RevHubOrientationOnRobot revHubOrientationOnRobot = new RevHubOrientationOnRobot.LogoFacingDirection(RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, - RevHubOrientationOnRobot.UsbFacingDirection.UP); - imu.initalize(new IMU.Parameters(revHubOrientationOnRobot)); - public void start() { + public void runOpMode() throws InterruptedException { + + // ===== INIT LIMELIGHT ===== + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + // Pipeline must be an AprilTag pipeline (set in Limelight UI) + limelight.pipelineSwitch(8); + limelight.start(); + + telemetry.addLine("Limelight initialized"); + telemetry.addLine("Waiting for start..."); + telemetry.update(); + + waitForStart(); + + // ===== MAIN LOOP ===== + while (opModeIsActive()) { + + LimelightResults results = limelight.getLatestResults(); + + if (results != null && results.isValid() && results.targets.size() > 0) { + + for (LimelightTarget target : results.targets) { + + if (target.getType() == LimelightTargetType.APRILTAG) { + + telemetry.addLine("=== APRILTAG DETECTED ==="); + telemetry.addData("Tag ID", target.getFiducialID()); + telemetry.addData("X Offset", target.getX()); + telemetry.addData("Y Offset", target.getY()); + telemetry.addData("Area", target.getArea()); + telemetry.addData("Skew", target.getSkew()); + telemetry.addData("Latency (ms)", results.latency); + break; + } + } + + } else { + telemetry.addLine("No AprilTag detected"); + } + + telemetry.update(); + } + + limelight.stop(); } } From 49c06b6ac9a61651896e444d9b27c553c4105d28 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Sat, 17 Jan 2026 20:14:56 -0500 Subject: [PATCH 17/41] still working on it, might need update sdk for limelight --- .../ftc/teamcode/Utils_13233/LimeLight.java | 53 +++++-------------- .../ftc/teamcode/limelightvision.java | 45 ++++++++++++++++ 2 files changed, 57 insertions(+), 41 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java index 1a76a00..e167b9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java @@ -1,62 +1,33 @@ package org.firstinspires.ftc.teamcode.Utils_13233; - +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import com.limelightvision.limelight3a.*; -@TeleOp(name = "Limelight 3A AprilTag Test", group = "Vision") -public class LimelightAprilTagTest extends LinearOpMode { +@Autonomous(name = "Limelight AprilTag Test") - private Limelight3A limelight; +public class LimelightAuto extends LinearOpMode { + Limelight3A limelight; @Override - public void runOpMode() throws InterruptedException { - - // ===== INIT LIMELIGHT ===== + public void runOpMode() { limelight = hardwareMap.get(Limelight3A.class, "limelight"); - // Pipeline must be an AprilTag pipeline (set in Limelight UI) - limelight.pipelineSwitch(8); - + limelight.pipelineSwitch(0); // AprilTag pipeline limelight.start(); - telemetry.addLine("Limelight initialized"); - telemetry.addLine("Waiting for start..."); - telemetry.update(); - waitForStart(); - // ===== MAIN LOOP ===== while (opModeIsActive()) { + var results = limelight.getLatestResults(); - LimelightResults results = limelight.getLatestResults(); - - if (results != null && results.isValid() && results.targets.size() > 0) { - - for (LimelightTarget target : results.targets) { - - if (target.getType() == LimelightTargetType.APRILTAG) { - - telemetry.addLine("=== APRILTAG DETECTED ==="); - telemetry.addData("Tag ID", target.getFiducialID()); - telemetry.addData("X Offset", target.getX()); - telemetry.addData("Y Offset", target.getY()); - telemetry.addData("Area", target.getArea()); - telemetry.addData("Skew", target.getSkew()); - telemetry.addData("Latency (ms)", results.latency); - break; - } + if (results != null && results.isValid()) { + if (!results.targets.isEmpty()) { + int tagID = results.targets.get(0).getFiducialID(); + telemetry.addData("AprilTag ID", tagID); } - - } else { - telemetry.addLine("No AprilTag detected"); } - telemetry.update(); } - - limelight.stop(); } } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java new file mode 100644 index 0000000..cb26d8a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.FiducialResult; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@Autonomous(name = "Limelight AprilTag Test") +public class LimelightAprilTagAuto extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() { + + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + // Set AprilTag pipeline (must match Limelight web config) + limelight.pipelineSwitch(0); + limelight.start(); + + telemetry.addLine("Waiting for start..."); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + LLResult result = limelight.getLatestResult(); + + if (result != null && result.isValid()) { + for (FiducialResult tag : result.getFiducialResults()) { + telemetry.addData("AprilTag ID", tag.getFiducialId()); + telemetry.addData("X", tag.getX()); + telemetry.addData("Y", tag.getY()); + telemetry.addData("Z", tag.getZ()); + } + } else { + telemetry.addLine("No tags detected"); + } + + telemetry.update(); + } + } +} From 300e62b85f8c6500553aec87505a849456978606 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Sat, 17 Jan 2026 21:02:47 -0500 Subject: [PATCH 18/41] very small changes importing libs --- .../ftc/robotcontroller/external/samples/SensorLimelight3A.java | 1 + build.dependencies.gradle | 1 + 2 files changed, 2 insertions(+) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java index 2579cd0..3437a36 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -36,6 +36,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.LLStatus; import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.FiducialResult; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 1c07e8c..bf087b5 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -13,5 +13,6 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'org.firstinspires.ftc:Limelight:1.0.2'//added by jacob 1/17/26 } From 0a1cd3945ae64639c040f4cbc7536a6bf6a4477f Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Sat, 17 Jan 2026 21:25:23 -0500 Subject: [PATCH 19/41] I FIXED IT well mostly anyway --- .../external/samples/SensorLimelight3A.java | 8 ++--- .../ftc/teamcode/Utils_13233/LimeLight.java | 33 ------------------- .../ftc/teamcode/limelightvision.java | 4 +-- 3 files changed, 6 insertions(+), 39 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java index 3437a36..e5ee5b2 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -31,12 +31,11 @@ are permitted (subject to the limitations in the disclaimer below) provided that THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.hardware.limelightvision.FiducialResult; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; // Try this specific path import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -146,6 +145,7 @@ public void runOpMode() throws InterruptedException List colorResults = result.getColorResults(); for (LLResultTypes.ColorResult cr : colorResults) { telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } } else { telemetry.addData("Limelight", "No data available"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java deleted file mode 100644 index e167b9a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimeLight.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.firstinspires.ftc.teamcode.Utils_13233; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -@Autonomous(name = "Limelight AprilTag Test") - -public class LimelightAuto extends LinearOpMode { - Limelight3A limelight; - - @Override - public void runOpMode() { - limelight = hardwareMap.get(Limelight3A.class, "limelight"); - - limelight.pipelineSwitch(0); // AprilTag pipeline - limelight.start(); - - waitForStart(); - - while (opModeIsActive()) { - var results = limelight.getLatestResults(); - - if (results != null && results.isValid()) { - if (!results.targets.isEmpty()) { - int tagID = results.targets.get(0).getFiducialID(); - telemetry.addData("AprilTag ID", tagID); - } - } - telemetry.update(); - } - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java index cb26d8a..0ed0e02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java @@ -2,12 +2,12 @@ import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.FiducialResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @Autonomous(name = "Limelight AprilTag Test") -public class LimelightAprilTagAuto extends LinearOpMode { +public class limeLightTest extends LinearOpMode { private Limelight3A limelight; From 5a5b7c8ac4f08754c7894615f2b05fd4aa8876d4 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Sun, 18 Jan 2026 16:48:32 -0500 Subject: [PATCH 20/41] working on limelight auto testing now cannot resolve fiducial result for some odd reason, i could do it in the other code i put in tho --- .../external/samples/SensorLimelight3A.java | 2 +- .../ftc/teamcode/LimeLightAutoBlue.java | 44 +++++++++++++++++++ .../{QuickAuto.java => QuickAutoForward.java} | 5 +-- ...imelightvision.java => limeLightTest.java} | 4 +- 4 files changed, 48 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{QuickAuto.java => QuickAutoForward.java} (81%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{limelightvision.java => limeLightTest.java} (88%) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java index e5ee5b2..287e7bf 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -70,7 +70,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that @Disabled public class SensorLimelight3A extends LinearOpMode { - private Limelight3A limelight; + public Limelight3A limelight; @Override public void runOpMode() throws InterruptedException diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java new file mode 100644 index 0000000..6ec396c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import java.util.List; + + +@Autonomous(name = "AprilTag Auto", group = "Auto") + +public class LimelightAuto extends LinearOpMode { + + Limelight3A limelight; + + @Override + public void runOpMode() { + + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.pipelineSwitch(0); + limelight.start(); + + waitForStart(); + + while (opModeIsActive()) { + + LLResult result = limelight.getLatestResult(); + + if (result != null && result.isValid()) { + + for (FiducialResult tag : result.getFiducialResults()) { + telemetry.addData("ID", tag.fiducialId); + telemetry.addData("X", tag.poseX); + telemetry.addData("Y", tag.poseY); + } + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java index 335fa71..97519c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoForward.java @@ -4,13 +4,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; -import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; @Autonomous(name = "QuickAuto") -public class QuickAuto extends LinearOpMode { +public class QuickAutoForward extends LinearOpMode { private final DriveControls drive; - public QuickAuto(HardwareMap hardwareMap) { + public QuickAutoForward(HardwareMap hardwareMap) { this.drive = new DriveControls(hardwareMap); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java index 0ed0e02..76326ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limelightvision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java @@ -31,9 +31,7 @@ public void runOpMode() { if (result != null && result.isValid()) { for (FiducialResult tag : result.getFiducialResults()) { telemetry.addData("AprilTag ID", tag.getFiducialId()); - telemetry.addData("X", tag.getX()); - telemetry.addData("Y", tag.getY()); - telemetry.addData("Z", tag.getZ()); + } } else { telemetry.addLine("No tags detected"); From f7847d9e4b62960e8c31ba40ec5b8b21d5c59b41 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Tue, 20 Jan 2026 10:16:17 -0500 Subject: [PATCH 21/41] jacob fixed i lie --- .../ftc/teamcode/LimeLightAutoBlue.java | 15 +++++---- .../ftc/teamcode/QuickAutoBlue.java | 31 +++++++++---------- .../ftc/teamcode/QuickAutoRed.java | 23 ++++++-------- .../ftc/teamcode/TeleOpMain.java | 9 +++--- .../teamcode/Utils_13233/LaunchControls.java | 4 ++- .../teamcode/Utils_13233/SorterControls.java | 2 +- .../ftc/teamcode/limeLightTest.java | 2 ++ build.dependencies.gradle | 2 +- 8 files changed, 41 insertions(+), 47 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index 6ec396c..c26cab0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -1,20 +1,21 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import java.util.List; +import java.util.List; -@Autonomous(name = "AprilTag Auto", group = "Auto") +@Autonomous(name = "AprilTagBlueAuto", group = "Auto") -public class LimelightAuto extends LinearOpMode { +public class LimeLightAutoBlue extends LinearOpMode { - Limelight3A limelight; + private Limelight3A limelight; @Override public void runOpMode() { @@ -30,15 +31,13 @@ public void runOpMode() { LLResult result = limelight.getLatestResult(); if (result != null && result.isValid()) { - for (FiducialResult tag : result.getFiducialResults()) { - telemetry.addData("ID", tag.fiducialId); - telemetry.addData("X", tag.poseX); - telemetry.addData("Y", tag.poseY); + telemetry.addData("ID", tag.getFiducialId()); } } telemetry.update(); + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java index 6e64d23..ba0e1ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java @@ -14,47 +14,42 @@ @Autonomous(name = "QuickAutoBlue", group = "Autonomous") public class QuickAutoBlue extends LinearOpMode { - + // Constructors for the utils classes private DriveControls drive; private LaunchControls launch; + private RampControls ramp; private SorterControls sorter; private MotorConstructor motors; - private AutoDrive autoDrive; - - private AutoTurn autoTurn; - - private LinearOpMode opMode; @Override public void runOpMode() { - - drive = new DriveControls(hardwareMap); -// autoDrive = new AutoDrive(opMode, hardwareMap); -// autoTurn = new AutoTurn(opMode, hardwareMap); - launch = new LaunchControls(hardwareMap); - sorter = new SorterControls(motors); - - telemetry.addData("Status", "Initialized"); telemetry.update(); waitForStart(); if (opModeIsActive()) { + // Create the utils classes + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + ramp = new RampControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + sorter = new SorterControls(motors); + //autoDrive.driveForward(10.0, AutoConstants.quarterPower); //autoTurn.rotate(90, AutoConstants.quarterPower); - //Jacob's so useful code + //Jacob's not so useful code drive.setDrivePower(-1.0f); - sleep(1200); + sleep(1600); drive.setDriveMotorZeroPowerBehavior(true); drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); //sets position to ensure the ball is correctly lined up - launch.setLaunchPower(true); + launch.setLaunchPower(true, 0.9f); sleep(2500);//wait state to wait for launcher to spin up motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball @@ -62,6 +57,7 @@ public void runOpMode() { sleep(1000);//runs flipper servo into motor motors.Flipper.setDirection(Servo.Direction.REVERSE); motors.Flipper.setPosition(0.15); + sleep(2000); //2nd ball code sleep(750);//waits to spin turntable @@ -72,6 +68,7 @@ public void runOpMode() { sleep(1000); motors.Flipper.setDirection(Servo.Direction.REVERSE); motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); //3rd ball code sleep(1000); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java index 849ed09..d94db7b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java @@ -15,25 +15,16 @@ @Autonomous(name = "QuickAutoRed", group = "Autonomous") public class QuickAutoRed extends LinearOpMode { + // Constructors for the utils classes private DriveControls drive; private LaunchControls launch; + private RampControls ramp; private SorterControls sorter; private MotorConstructor motors; - private AutoDrive autoDrive; - - private AutoTurn autoTurn; - - private LinearOpMode opMode; @Override public void runOpMode() { - drive = new DriveControls(hardwareMap); -// autoDrive = new AutoDrive(opMode, hardwareMap); -// autoTurn = new AutoTurn(opMode, hardwareMap); - launch = new LaunchControls(hardwareMap); - sorter = new SorterControls(motors); - telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -41,13 +32,17 @@ public void runOpMode() { waitForStart(); if (opModeIsActive()) { + // Create the utils classes + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + ramp = new RampControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + sorter = new SorterControls(motors); + //autoDrive.driveForward(10.0, AutoConstants.quarterPower); //autoTurn.rotate(90, AutoConstants.quarterPower); - - //Jacob's so useful code - drive.setDrivePower(-1.0f); sleep(1200); drive.setDriveMotorZeroPowerBehavior(true); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index c0ead8c..4c65d2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -59,14 +59,14 @@ public void runOpMode() { // and rumble to let the driver know that the launch motors are being controlled // launch.setLaunchPower(gamepad1.x, true, gamepad1); if (gamepad1.x) { - launch.setLaunchPower(gamepad1.x, 0.3f); - } else if (gamepad1.y) { - launch.setLaunchPower(gamepad1.y, 1.0f); + launch.setLaunchPower(gamepad1.x, 0.7f); + } else if (gamepad1.left_bumper) { + launch.setLaunchPower(gamepad1.left_bumper, 1.0f); } else { launch.setLaunchPower(false); } - sleep(100); +// sleep(100); // Sorter Controls // Intake positions @@ -103,7 +103,6 @@ public void runOpMode() { drive.setDriveMotorZeroPowerBehavior(gamepad1.a); - ramp.setIntakeDirection(gamepad2.dpad_up, gamepad2.dpad_down); // Wait for motors to speed up before changing value // While it is bad practice to put a sleep in a loop it is the only way to (that I know) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index ed64abb..26fdf19 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -20,8 +20,10 @@ public LaunchControls(HardwareMap hardwareMap) { * * @param launchInput Button mapped to launch input */ + + public void setLaunchPower(boolean launchInput) { - double power = launchInput ? 0.75f : 0.0f; + double power = launchInput ? 1.0f : 0.0f; // Set the the power value to the motors motors.Launcher.setPower(-power); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index d5a72e6..1c37eb7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -12,7 +12,7 @@ public class SorterControls { public int intakePos3 = 84; //TODO: Update values to the real positions of the motor public int LaunchPos1 = 0; - public int LaunchPos2 = 176; + public int LaunchPos2 = 180; public int LaunchPos3 = 356; public SorterControls(MotorConstructor motors) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java index 76326ae..002c14a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/limeLightTest.java @@ -4,6 +4,7 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @Autonomous(name = "Limelight AprilTag Test") @@ -11,6 +12,7 @@ public class limeLightTest extends LinearOpMode { private Limelight3A limelight; + @Override public void runOpMode() { diff --git a/build.dependencies.gradle b/build.dependencies.gradle index bf087b5..241cbff 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -13,6 +13,6 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' implementation 'org.firstinspires.ftc:Vision:11.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'org.firstinspires.ftc:Limelight:1.0.2'//added by jacob 1/17/26 + //implementation 'org.firstinspires.ftc:Limelight:1.0.2'//added by jacob 1/17/26 update jeffery 1/20/26 jacob is stupid } From c44cab8047fcf4e8b3675f0e5fa981cf87b6039f Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 20 Jan 2026 13:43:47 -0500 Subject: [PATCH 22/41] Remove RampControls and add LimelightControls utility Deleted RampControls and its related test, refactored main and auto classes to remove RampControls usage. Added LimelightControls utility for AprilTag detection and updated MotorConstructor to include Limelight3A initialization. Updated MockMotorUtil for Limelight3A and removed ramp servo mocks. --- .../ftc/teamcode/LimeLightAutoBlue.java | 39 ++++++----- .../ftc/teamcode/QuickAutoBlue.java | 3 - .../ftc/teamcode/QuickAutoRed.java | 3 - .../ftc/teamcode/TeleOpMain.java | 5 -- .../Utils_13233/LimelightControls.java | 28 ++++++++ .../Utils_13233/MotorConstructor.java | 6 ++ .../teamcode/Utils_13233/RampControls.java | 31 --------- .../ftc/teamcode/MockMotorUtil.java | 23 ++++--- .../ftc/teamcode/RampControlsTest.java | 66 ------------------- 9 files changed, 70 insertions(+), 134 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java delete mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index c26cab0..d01bdce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -9,35 +9,42 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LimelightControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + import java.util.List; @Autonomous(name = "AprilTagBlueAuto", group = "Auto") public class LimeLightAutoBlue extends LinearOpMode { - private Limelight3A limelight; + // Constructors for the utils classes + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + private LimelightControls limelightCont; + + //This function is executed when this Op Mode is selected from the Driver Station @Override public void runOpMode() { - - limelight = hardwareMap.get(Limelight3A.class, "limelight"); - limelight.pipelineSwitch(0); - limelight.start(); - + // Create the utils classes + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + limelightCont = new LimelightControls(hardwareMap); + sorter = new SorterControls(motors); waitForStart(); - while (opModeIsActive()) { - - LLResult result = limelight.getLatestResult(); - - if (result != null && result.isValid()) { - for (FiducialResult tag : result.getFiducialResults()) { - telemetry.addData("ID", tag.getFiducialId()); - } - } + while (opModeIsActive()) { + telemetry.addData("ID", limelightCont.getTagID()); telemetry.update(); - + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java index ba0e1ec..afed258 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java @@ -9,7 +9,6 @@ import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; @Autonomous(name = "QuickAutoBlue", group = "Autonomous") @@ -17,7 +16,6 @@ public class QuickAutoBlue extends LinearOpMode { // Constructors for the utils classes private DriveControls drive; private LaunchControls launch; - private RampControls ramp; private SorterControls sorter; private MotorConstructor motors; @@ -32,7 +30,6 @@ public void runOpMode() { // Create the utils classes drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); - ramp = new RampControls(hardwareMap); motors = new MotorConstructor(hardwareMap); sorter = new SorterControls(motors); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java index d94db7b..f1965d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java @@ -9,7 +9,6 @@ import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; @Autonomous(name = "QuickAutoRed", group = "Autonomous") @@ -18,7 +17,6 @@ public class QuickAutoRed extends LinearOpMode { // Constructors for the utils classes private DriveControls drive; private LaunchControls launch; - private RampControls ramp; private SorterControls sorter; private MotorConstructor motors; @@ -35,7 +33,6 @@ public void runOpMode() { // Create the utils classes drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); - ramp = new RampControls(hardwareMap); motors = new MotorConstructor(hardwareMap); sorter = new SorterControls(motors); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 4c65d2e..3a550d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -10,7 +10,6 @@ import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; @@ -21,7 +20,6 @@ public class TeleOpMain extends LinearOpMode { // Constructors for the utils classes private DriveControls drive; private LaunchControls launch; - private RampControls ramp; private SorterControls sorter; private MotorConstructor motors; @@ -32,7 +30,6 @@ public void runOpMode() { // Create the utils classes drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); - ramp = new RampControls(hardwareMap); motors = new MotorConstructor(hardwareMap); sorter = new SorterControls(motors); @@ -102,8 +99,6 @@ public void runOpMode() { drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button drive.setDriveMotorZeroPowerBehavior(gamepad1.a); - - ramp.setIntakeDirection(gamepad2.dpad_up, gamepad2.dpad_down); // Wait for motors to speed up before changing value // While it is bad practice to put a sleep in a loop it is the only way to (that I know) // to make the intake not jitter I also diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java new file mode 100644 index 0000000..2b1331b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.robotcore.hardware.HardwareMap; + + +public class LimelightControls { + private final MotorConstructor motors; + + public LimelightControls(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + } + + public int getTagID() { + motors.limelight.pipelineSwitch(0); + motors.limelight.start(); + + LLResult result = motors.limelight.getLatestResult(); + + if (result != null && result.isValid()) { + for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) { + return (tag.getFiducialId()); + } + } + return 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index ff360e3..71becf7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -48,6 +49,7 @@ public class MotorConstructor { public Servo Flipper; public VoltageSensor VoltSens; public IMU imu; + public Limelight3A limelight; public MotorConstructor(HardwareMap hardwareMap) { // Map main Drive Motors @@ -94,6 +96,10 @@ public MotorConstructor(HardwareMap hardwareMap) { Sorter.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); Sorter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.pipelineSwitch(0); + limelight.start(); + //Flipper.setDirection(Servo.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java deleted file mode 100644 index f17fa7b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/RampControls.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.firstinspires.ftc.teamcode.Utils_13233; - -import com.qualcomm.robotcore.hardware.HardwareMap; -//THIS IS ALL USELESS NOW BY THE WAY -public class RampControls { - private final MotorConstructor motors; - - public RampControls(HardwareMap hardwareMap) { - this.motors = new MotorConstructor(hardwareMap); - } - - /** - * Controls the spin direction of the intake wheel based on controller buttons - * - * @param intakeForwardInput Button mapped to forward input - * @param intakeReverseInput Button mapped to reverse input - */ - public void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) { - double intakePower = 0; - // ^ is the XOR operator, will return true if only one variable is true - // If intakeForwardInput OR intakeReverseInput is true then this is true, but not - // if both are true, which is why this differs from the || logical or operator - if (intakeForwardInput ^ intakeReverseInput) { - // This uses an inline-if statement which is useful when assigning values to variables - // This says set power = 1 if intakeForwardInput is true, else set it to -1 - intakePower = intakeForwardInput ? 1 : -1; - } - motors.intake.setPower(intakePower); - } - -} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java index 6c3c416..d7ef148 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java @@ -3,11 +3,13 @@ import static org.mockito.Mockito.mock; import static org.mockito.Mockito.when; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; @@ -16,10 +18,11 @@ import org.mockito.MockitoAnnotations; public class MockMotorUtil { - public DcMotor launcher, launcher2, intake, rightFront, leftFront, leftBack, rightBack; - public CRServo rampServo1, rampServo2, rampServo3, rampServo4; + public DcMotor launcher, launcher2, intake, rightFront, leftFront, leftBack, rightBack, Sorter; + public Servo Flipper; public HardwareMap hardwareMap; public IMU imu; + public Limelight3A limelight; @Mock HardwareMap.DeviceMapping voltageSensorMap; @@ -44,14 +47,14 @@ public void setUp() { launcher = mock(DcMotor.class); launcher2 = mock(DcMotor.class); - rampServo1 = mock(CRServo.class); - rampServo2 = mock(CRServo.class); - rampServo3 = mock(CRServo.class); - rampServo4 = mock(CRServo.class); + Sorter = mock(DcMotor.class); + Flipper = mock(Servo.class); VoltSens = mock(VoltageSensor.class); imu = mock(IMU.class); + limelight = mock(Limelight3A.class); + // Mock the motors when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake); @@ -64,10 +67,9 @@ public void setUp() { when(hardwareMap.get(DcMotor.class, "Launcher")).thenReturn(launcher); when(hardwareMap.get(DcMotor.class, "Launcher2")).thenReturn(launcher2); - when(hardwareMap.get(CRServo.class, "rampServo1")).thenReturn(rampServo1); - when(hardwareMap.get(CRServo.class, "rampServo2")).thenReturn(rampServo2); - when(hardwareMap.get(CRServo.class, "rampServo3")).thenReturn(rampServo3); - when(hardwareMap.get(CRServo.class, "rampServo4")).thenReturn(rampServo4); + when(hardwareMap.get(DcMotor.class, "sorter")).thenReturn(Sorter); + when(hardwareMap.get(Servo.class, "flipper")).thenReturn(Flipper); + when(hardwareMap.get(VoltageSensor.class, "Control Hub")).thenReturn(VoltSens); hardwareMap.voltageSensor = voltageSensorMap; @@ -75,6 +77,7 @@ public void setUp() { // Mock the IMU (gyroscope) when(hardwareMap.get(IMU.class, "imu")).thenReturn(imu); + when(hardwareMap.get(Limelight3A.class, "limelight")).thenReturn(limelight); // Set direction of the main drive motors leftFront.setDirection(DcMotorSimple.Direction.FORWARD); diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java deleted file mode 100644 index 044bc3f..0000000 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/RampControlsTest.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; -import org.junit.jupiter.api.BeforeEach; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.mockito.Mockito.mock; -import static org.mockito.Mockito.verify; -import static org.mockito.Mockito.when; - -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.junit.jupiter.api.DisplayName; -import org.junit.jupiter.api.Test; - - -public class RampControlsTest { - public RampControls rampControls; - - MockMotorUtil mockMotor = new MockMotorUtil(); - - @BeforeEach - void motorSetup(){ - mockMotor.setUp(); - rampControls = new RampControls(mockMotor.hardwareMap); - } - - @DisplayName("Intake should spin forward when forward button is pressed") - @Test - void testIntakeForward() { - rampControls.setIntakeDirection(true, false); - verify(mockMotor.intake).setPower(1.0); - verify(mockMotor.rampServo1).setPower(-1.0); - verify(mockMotor.rampServo2).setPower(1.0); - } - - @DisplayName("Intake should spin backward when reverse button is pressed") - @Test - void testIntakeReverse() { - rampControls.setIntakeDirection(false, true); - verify(mockMotor.intake).setPower(-1.0); - verify(mockMotor.rampServo1).setPower(1.0); - verify(mockMotor.rampServo2).setPower(-1.0); - } - - @DisplayName("Intake should not spin when direction buttons are not depressed") - @Test - void testIntakeNeutral() { - rampControls.setIntakeDirection(false, false); - verify(mockMotor.intake).setPower(0.0); - verify(mockMotor.rampServo1).setPower(-0.0); - verify(mockMotor.rampServo2).setPower(0.0); - } - - @DisplayName("Intake should stop when both direction buttons pressed at the same time") - @Test - void testIntakeConflict() { - rampControls.setIntakeDirection(true, true); - verify(mockMotor.intake).setPower(0.0); - verify(mockMotor.rampServo1).setPower(-0.0); - verify(mockMotor.rampServo2).setPower(0.0); - } -} From ac53e08cf84030e4bff5cf0902b42de8cd20dc62 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 20 Jan 2026 13:59:34 -0500 Subject: [PATCH 23/41] Refactor and document Limelight and Sorter controls Added JavaDoc comments to LimelightControls for improved clarity. Refactored SorterControls to use a configurable motor speed variable instead of hardcoded values, added comments for position variables, and improved code formatting. Removed an unnecessary blank line in LimeLightAutoBlue. --- .../ftc/teamcode/LimeLightAutoBlue.java | 1 - .../Utils_13233/LimelightControls.java | 10 +++++++ .../teamcode/Utils_13233/SorterControls.java | 26 ++++++++++++------- 3 files changed, 26 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index d01bdce..ffbd802 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -44,7 +44,6 @@ public void runOpMode() { while (opModeIsActive()) { telemetry.addData("ID", limelightCont.getTagID()); telemetry.update(); - } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java index 2b1331b..0954ea7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java @@ -8,10 +8,20 @@ public class LimelightControls { private final MotorConstructor motors; + /** + * Constructor for the LimelightControls class + * + * @param hardwareMap Hardware map for the robot + */ public LimelightControls(HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); } + /** + * Get the ID of the tag that is being seen by the limelight + * + * @return Returns the ID of the tag that is being seen by the limelight + */ public int getTagID() { motors.limelight.pipelineSwitch(0); motors.limelight.start(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 1c37eb7..409728d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -6,15 +6,21 @@ public class SorterControls { private final MotorConstructor motors; - //TODO: Update values to the real positions of the motor + + // Sorter intake positions public int intakePos1 = 263; public int intakePos2 = -87; public int intakePos3 = 84; - //TODO: Update values to the real positions of the motor + + // Sorter launch positions public int LaunchPos1 = 0; public int LaunchPos2 = 180; public int LaunchPos3 = 356; + + // Motor Speed + float motorSpeed = 1.0f; + public SorterControls(MotorConstructor motors) { this.motors = motors; } @@ -29,17 +35,17 @@ void moveToIntakePos(intakePos pos) { case INTAKE_POS_1: motors.Sorter.setTargetPosition(intakePos1); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(0.6); + motors.Sorter.setPower(motorSpeed); break; case INTAKE_POS_2: motors.Sorter.setTargetPosition(intakePos2); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(0.6); + motors.Sorter.setPower(motorSpeed); break; case INTAKE_POS_3: motors.Sorter.setTargetPosition(intakePos3); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(0.6); + motors.Sorter.setPower(motorSpeed); break; } } @@ -55,17 +61,17 @@ void moveToLaunchPos(launchPos pos) { case LAUNCH_POS_1: motors.Sorter.setTargetPosition(LaunchPos1); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(0.6); + motors.Sorter.setPower(motorSpeed); break; case LAUNCH_POS_2: motors.Sorter.setTargetPosition(LaunchPos2); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(0.6); + motors.Sorter.setPower(motorSpeed); break; case LAUNCH_POS_3: motors.Sorter.setTargetPosition(LaunchPos3); motors.Sorter.setMode(DcMotor.RunMode.RUN_TO_POSITION); - motors.Sorter.setPower(0.6); + motors.Sorter.setPower(motorSpeed); break; } } @@ -93,7 +99,7 @@ public void moveSorterToPos(sorterModes mode, int pos) { default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } else if (mode == sorterModes.LAUNCH) { switch (pos) { @@ -109,7 +115,7 @@ public void moveSorterToPos(sorterModes mode, int pos) { default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } } From ef625ed71de4b8f5c3f5fc2aaf10a84fd7ea3cf2 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 20 Jan 2026 15:17:39 -0500 Subject: [PATCH 24/41] Add ballColors enum to SorterControls Introduced a new ballColors enum with PURPLE and GREEN values to SorterControls for improved color handling. --- .../ftc/teamcode/Utils_13233/SorterControls.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 409728d..ade3a86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.Utils_13233; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; public class SorterControls { @@ -137,5 +136,10 @@ public enum sorterModes { LAUNCH } + public enum ballColors { + PURPLE, + GREEN + } + } From 8b825f0a27e625d58898272aae8732f757bcb7cc Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Thu, 22 Jan 2026 10:22:18 -0500 Subject: [PATCH 25/41] updates limelight auto blue to actually have auto yes i know there are 9 errors but it does work i dont know --- .../ftc/teamcode/LimeLightAutoBlue.java | 162 +++++++++++++++++- .../Utils_13233/MotorConstructor.java | 2 + 2 files changed, 163 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index ffbd802..633a5c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -8,6 +8,7 @@ import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; @@ -17,7 +18,7 @@ import java.util.List; -@Autonomous(name = "AprilTagBlueAuto", group = "Auto") +@Autonomous(name = "LimeLightAutoBlue", group = "Auto") public class LimeLightAutoBlue extends LinearOpMode { @@ -42,8 +43,167 @@ public void runOpMode() { while (opModeIsActive()) { + drive.setDrivePower(-1.0f); + sleep(1600); + drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f);//drives back + sleep(750);//turns + drive.setDrivePower(0.0f); + telemetry.addData("ID", limelightCont.getTagID()); telemetry.update(); + + sleep(1000); + drive.setDrivePower(0.0f, 0.0f, 1.0f, 0.0f);//drives back + sleep(750); + drive.setDrivePower(0.0f); + + switch (limelightCont.getTagID()) { + case 21: + telemetry.addLine("AUTO: Green Purple Purple"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + //id 21 + + switch (limelightCont.getTagID()) { + case 22: + telemetry.addLine("AUTO: Purple Green Purple"); + telemetry.update(); + //runs PGP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + + switch (limelightCont.getTagID()) { + case 23: + //runs PPG + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + + + } + } + } } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 71becf7..0effb7c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -108,4 +108,6 @@ public MotorConstructor(HardwareMap hardwareMap) { intake.setDirection(DcMotorSimple.Direction.REVERSE); } + public void leftFront(float v) { + } } From 8413d2eda8f107e8d16ff3d9b33dfbf664af6073 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 22 Jan 2026 10:45:28 -0500 Subject: [PATCH 26/41] Added move to specific color --- .../ftc/teamcode/TeleOpMain.java | 3 ++ .../Utils_13233/LimelightControls.java | 3 +- .../teamcode/Utils_13233/SorterControls.java | 48 +++++++++++++++++-- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 3a550d4..fc058cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -94,6 +94,9 @@ public void runOpMode() { motors.Flipper.setPosition(0.15); } + if (gamepad1.left_bumper) { + sorter.moveGreenToLaunchPos(); + } //Add option to enable brakes when sharbell holds a drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java index 0954ea7..44cb9ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java @@ -7,6 +7,7 @@ public class LimelightControls { private final MotorConstructor motors; + private final int aprilTagPipeline = 0; /** * Constructor for the LimelightControls class @@ -23,7 +24,7 @@ public LimelightControls(HardwareMap hardwareMap) { * @return Returns the ID of the tag that is being seen by the limelight */ public int getTagID() { - motors.limelight.pipelineSwitch(0); + motors.limelight.pipelineSwitch(aprilTagPipeline); motors.limelight.start(); LLResult result = motors.limelight.getLatestResult(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index ade3a86..9993a58 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -17,6 +17,8 @@ public class SorterControls { public int LaunchPos3 = 356; + ballColors[] currentSorterStates = {ballColors.NULL, ballColors.NULL, ballColors.GREEN}; + // Motor Speed float motorSpeed = 1.0f; @@ -29,7 +31,7 @@ public SorterControls(MotorConstructor motors) { * * @param pos Enum to determine what positon to move to */ - void moveToIntakePos(intakePos pos) { + private void moveToIntakePos(intakePos pos) { switch (pos) { case INTAKE_POS_1: motors.Sorter.setTargetPosition(intakePos1); @@ -55,7 +57,7 @@ void moveToIntakePos(intakePos pos) { * * @param pos Enum to determine what positon to move to */ - void moveToLaunchPos(launchPos pos) { + private void moveToLaunchPos(launchPos pos) { switch (pos) { case LAUNCH_POS_1: motors.Sorter.setTargetPosition(LaunchPos1); @@ -119,26 +121,66 @@ public void moveSorterToPos(sorterModes mode, int pos) { } } + /** + * Sorts through the balls in the sorter and moves a green ball to the launch position + * automatically + */ + public void moveGreenToLaunchPos() { + for (int i = 0; i < currentSorterStates.length; i++) { + if (currentSorterStates[i] == ballColors.GREEN) { + moveSorterToPos(sorterModes.LAUNCH, i + 1); + break; + } + } + } + + /** + * Sorts through the balls in the sorter and moves a purple ball to the launch position + * automatically + */ + public void moveToPurpleLaunchPos() { + for (int i = 0; i < currentSorterStates.length; i++) { + if (currentSorterStates[i] == ballColors.PURPLE) { + moveSorterToPos(sorterModes.LAUNCH, i + 1); + break; + } + } + } + + /** + * Intake positions + */ public enum intakePos { INTAKE_POS_1, INTAKE_POS_2, INTAKE_POS_3 } + /** + * Launch positions + */ public enum launchPos { LAUNCH_POS_1, LAUNCH_POS_2, LAUNCH_POS_3 } + + /** + * Different modes that the sorter can be moved to + */ public enum sorterModes { INTAKE, LAUNCH } + /** + * Possible colors that the balls can be + */ public enum ballColors { PURPLE, - GREEN + GREEN, + NULL } From 46fc8d0ecc44e9c89f808efbeb92067f27fcb471 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 22 Jan 2026 15:11:56 -0500 Subject: [PATCH 27/41] Refactor MotorConstructor field comments Replaced verbose Javadoc comments with concise inline comments for class fields in MotorConstructor. This improves readability and maintains essential documentation without excessive verbosity. --- .../Utils_13233/MotorConstructor.java | 34 +++++++------------ 1 file changed, 13 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 0effb7c..2dc82a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -13,42 +13,34 @@ * Class holds motors and servos that are used throughout the code */ public class MotorConstructor { - /** - * Front left drive motor - */ + + // Drive Motor public DcMotor leftFront; - /** - * Front right drive motor - */ public DcMotor rightFront; - /** - * Back left drive motor - */ public DcMotor leftBack; - /** - * Right back drive motor - */ public DcMotor rightBack; - /** - * The motor for that controls the ball intake - */ + // Intake public DcMotor intake; - /** - * One of the motors used to shoot the balls - */ + // Launch motors to shoot the balls out of the robot public DcMotor Launcher; - /** - * One of the motors used to shoot the balls - */ public DcMotor Launcher2; + // The motor that controls the sorter position public DcMotor Sorter; + + // The servo the pushes the balls into the launcher flywheels public Servo Flipper; + + // The battery voltage sensor public VoltageSensor VoltSens; + + //Rotation Sensor on the Control hub public IMU imu; + + // The limelight 3a used for scanning April Tags public Limelight3A limelight; public MotorConstructor(HardwareMap hardwareMap) { From e0f6573e20a7f6de232108387cc9d2f21db07ecd Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 22 Jan 2026 15:15:12 -0500 Subject: [PATCH 28/41] Preserve last scanned tag in LimelightControls Introduced a lastTagScanned field to retain the last valid fiducial ID detected by the Limelight. If no new tag is found, getTagId() now returns the last scanned tag instead of 0. Also added a call to stop the Limelight after processing tags. --- .../ftc/teamcode/Utils_13233/LimelightControls.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java index 44cb9ac..98d2f07 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LimelightControls.java @@ -8,6 +8,7 @@ public class LimelightControls { private final MotorConstructor motors; private final int aprilTagPipeline = 0; + private int lastTagScanned; /** * Constructor for the LimelightControls class @@ -31,9 +32,11 @@ public int getTagID() { if (result != null && result.isValid()) { for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) { + lastTagScanned = tag.getFiducialId(); return (tag.getFiducialId()); } + motors.limelight.stop(); } - return 0; + return lastTagScanned; } } From cea764cd253dd8883d2b13b68d037c17c96790f0 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 22 Jan 2026 15:25:10 -0500 Subject: [PATCH 29/41] Track current sorter position with new enum Added a sorterPositions enum and a currentSorterPosition field to track the current position of the sorter. Updated setSorterPos to set currentSorterPosition based on the selected mode and position. --- .../teamcode/Utils_13233/SorterControls.java | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 9993a58..66e9a3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -16,6 +16,8 @@ public class SorterControls { public int LaunchPos2 = 180; public int LaunchPos3 = 356; + public sorterPositions currentSorterPosition; + ballColors[] currentSorterStates = {ballColors.NULL, ballColors.NULL, ballColors.GREEN}; @@ -90,33 +92,39 @@ public void moveSorterToPos(sorterModes mode, int pos) { switch (pos) { case 1: moveToIntakePos(intakePos.INTAKE_POS_1); + currentSorterPosition = sorterPositions.INTAKE_POS_1; break; case 2: moveToIntakePos(intakePos.INTAKE_POS_2); + currentSorterPosition = sorterPositions.INTAKE_POS_2; break; case 3: moveToIntakePos(intakePos.INTAKE_POS_3); + currentSorterPosition = sorterPositions.INTAKE_POS_3; break; default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } else if (mode == sorterModes.LAUNCH) { switch (pos) { case 1: moveToLaunchPos(launchPos.LAUNCH_POS_1); + currentSorterPosition = sorterPositions.LAUNCH_POS_1; break; case 2: moveToLaunchPos(launchPos.LAUNCH_POS_2); + currentSorterPosition = sorterPositions.LAUNCH_POS_2; break; case 3: moveToLaunchPos(launchPos.LAUNCH_POS_3); + currentSorterPosition = sorterPositions.LAUNCH_POS_3; break; default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } } @@ -183,5 +191,14 @@ public enum ballColors { NULL } + public enum sorterPositions { + INTAKE_POS_1, + INTAKE_POS_2, + INTAKE_POS_3, + LAUNCH_POS_1, + LAUNCH_POS_2, + LAUNCH_POS_3 + } + } From e50195e9bb876fd6e6f561070b6e1c676295d160 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Thu, 22 Jan 2026 15:49:19 -0500 Subject: [PATCH 30/41] Update TeleOpMain.java --- .../java/org/firstinspires/ftc/teamcode/TeleOpMain.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index fc058cf..b999c06 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -33,9 +33,10 @@ public void runOpMode() { motors = new MotorConstructor(hardwareMap); sorter = new SorterControls(motors); + sorter.currentSorterPosition = SorterControls.sorterPositions.LAUNCH_POS_1; - // Wait for the game to start (driver presses PLAY) + // Wait for the game to start (driver presses PLAY) // The waitForStart() function will wait for the start button to begin // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())" // THIS WILL CAUSE PROBLEMS WHEN GOING THROUGH INSPECTION @@ -44,12 +45,11 @@ public void runOpMode() { while (opModeIsActive()) { // Add status data to driver hub display telemetry.addData("Status", "opModeIsActive"); - telemetry.addData("Mode", motors.Sorter.getMode()); - telemetry.addData("Dir", motors.Sorter.getDirection()); + telemetry.addData("Sorter ", "Stats"); telemetry.addData("Target", motors.Sorter.getTargetPosition()); telemetry.addData("Current", motors.Sorter.getCurrentPosition()); telemetry.addData("Busy", motors.Sorter.isBusy()); - + telemetry.addData("Current Position", sorter.currentSorterPosition.toString()); telemetry.update(); // Set the power to the launch motors based while the x button is being pressed From 49851491c28bbc80617ce71a00208826d8d664c2 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Thu, 22 Jan 2026 15:50:51 -0500 Subject: [PATCH 31/41] turnign turned --- .../org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index 633a5c1..6c116d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -45,7 +45,9 @@ public void runOpMode() { while (opModeIsActive()) { drive.setDrivePower(-1.0f); sleep(1600); - drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f);//drives back + // turns to view april tag tower + drive.setDrivePower(0.0f, 0.0f, 1.0f, + 0.0f); sleep(750);//turns drive.setDrivePower(0.0f); @@ -53,7 +55,9 @@ public void runOpMode() { telemetry.update(); sleep(1000); - drive.setDrivePower(0.0f, 0.0f, 1.0f, 0.0f);//drives back + // turns back viewing goal + drive.setDrivePower(0.0f, 0.0f, -1.0f, + 0.0f); sleep(750); drive.setDrivePower(0.0f); From c9c47f1f6ec7290a8bf2c0d733f0ac564b8e71b3 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Thu, 22 Jan 2026 15:59:53 -0500 Subject: [PATCH 32/41] auto fix --- .../ftc/teamcode/LimeLightAutoBlue.java | 199 +++++++++--------- 1 file changed, 99 insertions(+), 100 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index 6c116d6..64fc566 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -48,7 +48,7 @@ public void runOpMode() { // turns to view april tag tower drive.setDrivePower(0.0f, 0.0f, 1.0f, 0.0f); - sleep(750);//turns + sleep(550);//turns drive.setDrivePower(0.0f); telemetry.addData("ID", limelightCont.getTagID()); @@ -58,7 +58,7 @@ public void runOpMode() { // turns back viewing goal drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f); - sleep(750); + sleep(550); drive.setDrivePower(0.0f); switch (limelightCont.getTagID()) { @@ -108,105 +108,104 @@ public void runOpMode() { drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line sleep(1000); //id 21 + break; + + case 22: + telemetry.addLine("AUTO: Purple Green Purple"); + telemetry.update(); + //runs PGP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + break; + + + case 23: + //runs PPG + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + break; - switch (limelightCont.getTagID()) { - case 22: - telemetry.addLine("AUTO: Purple Green Purple"); - telemetry.update(); - //runs PGP - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); - //sets position to ensure the ball is correctly lined up - launch.setLaunchPower(true, 0.9f); - - sleep(2500);//wait state to wait for launcher to spin up - motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball - motors.Flipper.setPosition(0.0); - sleep(1000);//runs flipper servo into motor - motors.Flipper.setDirection(Servo.Direction.REVERSE); - motors.Flipper.setPosition(0.15); - sleep(2000); - - //2nd ball code - sleep(750);//waits to spin turntable - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); - sleep(1500);//waits while turntable spins - motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball - motors.Flipper.setPosition(0.0); - sleep(1000); - motors.Flipper.setDirection(Servo.Direction.REVERSE); - motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision - sleep(2000); - - //3rd ball code - sleep(1000); - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); - sleep(1000);//turns off ramp - motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball - motors.Flipper.setPosition(0.0); - sleep(1000);// sets servo back to init pos - motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball - motors.Flipper.setPosition(0.15); - sleep(1000); - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init - launch.setLaunchPower(false);//turns off launcher and ramp - - //strafing section - drive.setDrivePower(0, 1.0f, 0, 0); - sleep(1000); - drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line - sleep(1000); - - switch (limelightCont.getTagID()) { - case 23: - //runs PPG - telemetry.addLine("AUTO: Purple Purple Green"); - telemetry.update(); - //runs GPP - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); - //sets position to ensure the ball is correctly lined up - launch.setLaunchPower(true, 0.9f); - - sleep(2500);//wait state to wait for launcher to spin up - motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball - motors.Flipper.setPosition(0.0); - sleep(1000);//runs flipper servo into motor - motors.Flipper.setDirection(Servo.Direction.REVERSE); - motors.Flipper.setPosition(0.15); - sleep(2000); - - //2nd ball code - sleep(750);//waits to spin turntable - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); - sleep(1500);//waits while turntable spins - motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball - motors.Flipper.setPosition(0.0); - sleep(1000); - motors.Flipper.setDirection(Servo.Direction.REVERSE); - motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision - sleep(2000); - - //3rd ball code - sleep(1000); - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); - sleep(1000);//turns off ramp - motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball - motors.Flipper.setPosition(0.0); - sleep(1000);// sets servo back to init pos - motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball - motors.Flipper.setPosition(0.15); - sleep(1000); - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init - launch.setLaunchPower(false);//turns off launcher and ramp - - //strafing section - drive.setDrivePower(0, 1.0f, 0, 0); - sleep(1000); - drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line - sleep(1000); - - - } - } } } } From 1efe84f8bbf0e799861160183f11dd70f9c53282 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Thu, 22 Jan 2026 21:35:20 -0500 Subject: [PATCH 33/41] test test --- .../ftc/teamcode/LimeLightAutoBlue.java | 49 +++- .../ftc/teamcode/LimeLightAutoBlueTEST.java | 221 ++++++++++++++++++ 2 files changed, 269 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java index 633a5c1..671bd7c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlue.java @@ -204,6 +204,53 @@ public void runOpMode() { } } } - } + }switch (limelightCont.getTagID()) { + case 23: + //runs PPG + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); } } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java new file mode 100644 index 0000000..9379bc1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java @@ -0,0 +1,221 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + + +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.LimelightControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; +import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; + +@Autonomous(name = "AprilTag Auto", group = "Auto") +public class LimeLightAutoBlueTEST extends LinearOpMode { + + // ================= HARDWARE ================= + + private DriveControls drive; + private LaunchControls launch; + private SorterControls sorter; + private MotorConstructor motors; + private LimelightControls limelightCont; + + @Override + public void runOpMode() { + + // ---------- INIT ---------- + drive = new DriveControls(hardwareMap); + launch = new LaunchControls(hardwareMap); + motors = new MotorConstructor(hardwareMap); + limelightCont = new LimelightControls(hardwareMap); + sorter = new SorterControls(motors); + telemetry.addLine("Initialized. Waiting for start..."); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + // ---------- PHASE 1: MOVE TO SEE TAG ---------- + preMove(); + + // ---------- PHASE 2: DETECT TAG ---------- + int tagID = scanTag(); + + telemetry.addData("ID", limelightCont.getTagID()); + telemetry.update(); + sleep(500); + + // ---------- PHASE 3: RUN AUTO ---------- + runAuto(tagID); + + // ---------- END ---------- + requestOpModeStop(); + } + + // ========================================================== + // ===================== PHASE METHODS ====================== + // ========================================================== + + private void preMove() { + drive.setDrivePower(-1.0f); + sleep(1600); + + drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f); + sleep(750); + + drive.setDrivePower(0.0f); + } + + private int scanTag() { + int tagID = -1; + long startTime = System.currentTimeMillis(); + + while (opModeIsActive() + && tagID == -1 + && System.currentTimeMillis() - startTime < 2000) { + + tagID = limelightCont.getTagID(); + + telemetry.addData("Scanning Tag", tagID); + telemetry.update(); + } + + return tagID; + } + + private void runAuto(int tagID) { + + switch (tagID) { + + case 21: + telemetry.addLine("AUTO: Green Purple Purple"); + telemetry.update(); + auto21(); + break; + + case 22: + telemetry.addLine("AUTO: Purple Green Purple"); + telemetry.update(); + auto22(); + break; + + case 23: + telemetry.addLine("AUTO: Purple Purple Green"); + telemetry.update(); + auto23(); + break; + + default: + telemetry.addLine("AUTO: DEFAULT (NO TAG)"); + telemetry.update(); + break; + } + } + + // ========================================================== + // ===================== AUTO PATHS ========================= + // ========================================================== + + private void auto21() { + //runs GPP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + //id 21 + } + + private void auto22() { + //runs PGP + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + } + + private void auto23() { + // PUT YOUR EXISTING CASE 23 CODE HERE + } +} From 1061bc700b048f90266753ce48e27f697509a12a Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Fri, 23 Jan 2026 17:07:51 -0500 Subject: [PATCH 34/41] testing something --- .../ftc/teamcode/LimeLightAutoBlueTEST.java | 47 ++++++++++++++++++- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java index 9379bc1..f8315e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java @@ -65,8 +65,8 @@ public void runOpMode() { // ========================================================== private void preMove() { - drive.setDrivePower(-1.0f); - sleep(1600); + drive.setDrivePower(1.0f, 0, 0, 0);//strafes over to launch + sleep(1400); drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f); sleep(750); @@ -216,6 +216,49 @@ private void auto22() { } private void auto23() { + //runs PPG + + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); // PUT YOUR EXISTING CASE 23 CODE HERE } } From 949b56538721a288e9e1561d9c04da6c6dbb7232 Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Fri, 23 Jan 2026 19:09:03 -0500 Subject: [PATCH 35/41] Update LimeLightAutoBlueTEST.java --- .../ftc/teamcode/LimeLightAutoBlueTEST.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java index f8315e7..59322e6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java @@ -30,7 +30,7 @@ public class LimeLightAutoBlueTEST extends LinearOpMode { @Override public void runOpMode() { - // ---------- INIT ---------- + //INIT drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); motors = new MotorConstructor(hardwareMap); @@ -43,20 +43,20 @@ public void runOpMode() { if (isStopRequested()) return; - // ---------- PHASE 1: MOVE TO SEE TAG ---------- + // - PHASE 1: MOVE TO SEE TAG preMove(); - // ---------- PHASE 2: DETECT TAG ---------- + // - PHASE 2: DETECT TAG int tagID = scanTag(); telemetry.addData("ID", limelightCont.getTagID()); telemetry.update(); sleep(500); - // ---------- PHASE 3: RUN AUTO ---------- + // PHASE 3: RUN AUTO runAuto(tagID); - // ---------- END ---------- + // END requestOpModeStop(); } From d505f2872ca7295da4e19af097e72d9e5048a5eb Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Sat, 24 Jan 2026 13:32:03 -0500 Subject: [PATCH 36/41] Update LimeLightAutoBlueTEST.java --- .../ftc/teamcode/LimeLightAutoBlueTEST.java | 53 ++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java index 59322e6..6c97fc1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java @@ -116,6 +116,7 @@ private void runAuto(int tagID) { default: telemetry.addLine("AUTO: DEFAULT (NO TAG)"); telemetry.update(); + auto24(); break; } } @@ -259,6 +260,56 @@ private void auto23() { sleep(1000); drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line sleep(1000); - // PUT YOUR EXISTING CASE 23 CODE HERE + } + + + private void auto24() { + //runs default (PPG) + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); + //sets position to ensure the ball is correctly lined up + launch.setLaunchPower(true, 0.9f); + + sleep(2500);//wait state to wait for launcher to spin up + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.0); + sleep(1000);//runs flipper servo into motor + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15); + sleep(2000); + + //2nd ball code + sleep(750);//waits to spin turntable + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); + sleep(1500);//waits while turntable spins + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball + motors.Flipper.setPosition(0.0); + sleep(1000); + motors.Flipper.setDirection(Servo.Direction.REVERSE); + motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision + sleep(2000); + + //3rd ball code + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); + sleep(1000);//turns off ramp + motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball + motors.Flipper.setPosition(0.0); + sleep(1000);// sets servo back to init pos + motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball + motors.Flipper.setPosition(0.15); + sleep(1000); + sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init + launch.setLaunchPower(false);//turns off launcher and ramp + + //strafing section + drive.setDrivePower(0, 1.0f, 0, 0); + sleep(1000); + drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line + sleep(1000); + } + + } + + From 442b698c54be875a32cc303d76f5febf156cbea6 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 26 Jan 2026 15:11:02 -0500 Subject: [PATCH 37/41] Add detailed sorter state telemetry and fix visibility Added telemetry output for each sorter slot's current state in TeleOpMain for improved debugging. Made currentSorterStates public in SorterControls to allow external access. Minor formatting adjustments to exception messages. --- .../java/org/firstinspires/ftc/teamcode/TeleOpMain.java | 5 +++++ .../ftc/teamcode/Utils_13233/SorterControls.java | 6 +++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index b999c06..be93a35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -50,6 +50,11 @@ public void runOpMode() { telemetry.addData("Current", motors.Sorter.getCurrentPosition()); telemetry.addData("Busy", motors.Sorter.isBusy()); telemetry.addData("Current Position", sorter.currentSorterPosition.toString()); + telemetry.addData("", ""); + telemetry.addData("Current Sorter Positions", ":"); + telemetry.addData("Sorter Slot 1", sorter.currentSorterStates[0].toString()); + telemetry.addData("Sorter Slot 2", sorter.currentSorterStates[1].toString()); + telemetry.addData("Sorter Slot 3", sorter.currentSorterStates[2].toString()); telemetry.update(); // Set the power to the launch motors based while the x button is being pressed diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 66e9a3c..8b49e0a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -19,7 +19,7 @@ public class SorterControls { public sorterPositions currentSorterPosition; - ballColors[] currentSorterStates = {ballColors.NULL, ballColors.NULL, ballColors.GREEN}; + public ballColors[] currentSorterStates = {ballColors.NULL, ballColors.NULL, ballColors.GREEN}; // Motor Speed float motorSpeed = 1.0f; @@ -105,7 +105,7 @@ public void moveSorterToPos(sorterModes mode, int pos) { default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } else if (mode == sorterModes.LAUNCH) { switch (pos) { @@ -124,7 +124,7 @@ public void moveSorterToPos(sorterModes mode, int pos) { default: // throw error when pos is not 1, 2 or 3 throw new RuntimeException("Not a valid position must be 1, 2, or 3, did you" + - "say run it?"); + "say run it?"); } } } From 474fbfb130544abe20125d8993e4d0ad3b968b51 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 26 Jan 2026 15:15:01 -0500 Subject: [PATCH 38/41] Add IntakeControls utility class Introduces IntakeControls for managing intake wheel direction based on controller input. This class uses MotorConstructor to set intake motor power according to forward and reverse button states. --- .../teamcode/Utils_13233/IntakeControls.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java new file mode 100644 index 0000000..64b9794 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/IntakeControls.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class IntakeControls { + private final MotorConstructor motors; + + public IntakeControls(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + } + + /** + * Controls the spin direction of the intake wheel based on controller buttons + * + * @param intakeForwardInput Button mapped to forward input + * @param intakeReverseInput Button mapped to reverse input + */ + public void setIntakeDirection(boolean intakeForwardInput, boolean intakeReverseInput) { + double intakePower = 0; + // ^ is the XOR operator, will return true if only one variable is true + // If intakeForwardInput OR intakeReverseInput is true then this is true, but not + // if both are true, which is why this differs from the || logical or operator + if (intakeForwardInput ^ intakeReverseInput) { + // This uses an inline-if statement which is useful when assigning values to variables + // This says set power = 1 if intakeForwardInput is true, else set it to -1 + intakePower = intakeForwardInput ? 1 : -1; + } + motors.intake.setPower(intakePower); + } + +} From 25b64fc18a010b28d18f7f0dda7f61f25b1e24dc Mon Sep 17 00:00:00 2001 From: gatorgrad96 Date: Mon, 26 Jan 2026 15:15:21 -0500 Subject: [PATCH 39/41] auto fix --- .../ftc/teamcode/LimeLightAutoBlueTEST.java | 28 ++++++++++--------- .../ftc/teamcode/TeleOpMain.java | 6 ++-- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java index 6c97fc1..0b48764 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimeLightAutoBlueTEST.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode; + import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -16,7 +17,7 @@ import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; -@Autonomous(name = "AprilTag Auto", group = "Auto") +@Autonomous(name = "LimeLightBlueTEST ", group = "Auto") public class LimeLightAutoBlueTEST extends LinearOpMode { // ================= HARDWARE ================= @@ -44,11 +45,19 @@ public void runOpMode() { if (isStopRequested()) return; // - PHASE 1: MOVE TO SEE TAG - preMove(); +// preMove(); + drive.setDrivePower(0, -1.0f, 0, 0);//strafes over to launch + sleep(1400); - // - PHASE 2: DETECT TAG int tagID = scanTag(); + drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f); + sleep(550); + + drive.setDrivePower(0.0f); + + // - PHASE 2: DETECT TAG + telemetry.addData("ID", limelightCont.getTagID()); telemetry.update(); sleep(500); @@ -64,15 +73,8 @@ public void runOpMode() { // ===================== PHASE METHODS ====================== // ========================================================== - private void preMove() { - drive.setDrivePower(1.0f, 0, 0, 0);//strafes over to launch - sleep(1400); - - drive.setDrivePower(0.0f, 0.0f, -1.0f, 0.0f); - sleep(750); - - drive.setDrivePower(0.0f); - } +// private void preMove() { +// } private int scanTag() { int tagID = -1; @@ -129,7 +131,7 @@ private void auto21() { //runs GPP sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); //sets position to ensure the ball is correctly lined up - launch.setLaunchPower(true, 0.9f); + launch.setLaunchPower(true, 0.8f); sleep(2500);//wait state to wait for launcher to spin up motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index b999c06..1ef6938 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -94,9 +94,9 @@ public void runOpMode() { motors.Flipper.setPosition(0.15); } - if (gamepad1.left_bumper) { - sorter.moveGreenToLaunchPos(); - } +// if (gamepad1.left_bumper) { +// sorter.moveGreenToLaunchPos(); +// } //Add option to enable brakes when sharbell holds a drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button From daa14df8b534bb6a5666f75078374ad5e39a9caa Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 27 Jan 2026 07:48:13 -0500 Subject: [PATCH 40/41] Update TeleOp controls and streamline launch logic Added detailed control documentation as a comment block. Simplified launch motor power logic to respond directly to button presses. Updated brake control to use the 'b' button. Improved telemetry formatting for better readability. --- .../ftc/teamcode/TeleOpMain.java | 53 +++++++++++-------- 1 file changed, 32 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index a9b3434..fa66a13 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -13,6 +13,30 @@ import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls; +/* + Current Controls: + Gamepad1: + Left Stick Y: Forward backward movement + Left Stick X: Left right strafe movement + Right Stick X: Turn the entier robot left and right + + A: Move the flipper to push the artifact + B: Set the drive motors to brake + X: Set launch wheels to 0.7 power + Left Bumper: Set launch wheels to 1.0 power + + Gamepad2: + D-Pad: + Left: Move sorter to launch position 1 + Up: Move sorter to launch position 2 + Right: Move sorter to launch position 3 + + X: Move sorter to intake position 1 + Y: Move sorter to intake position 2 + B: Move sorter to intake position 3 + + */ + @TeleOp(name = "TeleOpMain", group = "Drive") public class TeleOpMain extends LinearOpMode { @@ -45,12 +69,13 @@ public void runOpMode() { while (opModeIsActive()) { // Add status data to driver hub display telemetry.addData("Status", "opModeIsActive"); + telemetry.addLine(); telemetry.addData("Sorter ", "Stats"); telemetry.addData("Target", motors.Sorter.getTargetPosition()); telemetry.addData("Current", motors.Sorter.getCurrentPosition()); telemetry.addData("Busy", motors.Sorter.isBusy()); telemetry.addData("Current Position", sorter.currentSorterPosition.toString()); - telemetry.addData("", ""); + telemetry.addLine(); telemetry.addData("Current Sorter Positions", ":"); telemetry.addData("Sorter Slot 1", sorter.currentSorterStates[0].toString()); telemetry.addData("Sorter Slot 2", sorter.currentSorterStates[1].toString()); @@ -59,19 +84,11 @@ public void runOpMode() { // Set the power to the launch motors based while the x button is being pressed // and rumble to let the driver know that the launch motors are being controlled - // launch.setLaunchPower(gamepad1.x, true, gamepad1); - if (gamepad1.x) { - launch.setLaunchPower(gamepad1.x, 0.7f); - } else if (gamepad1.left_bumper) { - launch.setLaunchPower(gamepad1.left_bumper, 1.0f); - } else { - launch.setLaunchPower(false); - } - -// sleep(100); + launch.setLaunchPower(gamepad1.x, 0.7f); + launch.setLaunchPower(gamepad1.left_bumper, 1.0f); - // Sorter Controls - // Intake positions + // Allows for driver Control of the sorter + // Intake positions for the sorter if (gamepad2.x) { sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 1); } else if (gamepad2.y) { @@ -80,7 +97,7 @@ public void runOpMode() { sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 3); } - // Launch positons + // Launch positons for the sorter if (gamepad2.dpad_left) { sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); } else if (gamepad2.dpad_up) { @@ -104,13 +121,7 @@ public void runOpMode() { // } //Add option to enable brakes when sharbell holds a - drive.setDriveMotorZeroPowerBehavior(gamepad1.a); //Add option to enable brakes when driver 1 holds the "a" button - drive.setDriveMotorZeroPowerBehavior(gamepad1.a); - - // Wait for motors to speed up before changing value - // While it is bad practice to put a sleep in a loop it is the only way to (that I know) - // to make the intake not jitter I also - + drive.setDriveMotorZeroPowerBehavior(gamepad1.b); // Sets the power to the drive motors based on current gamepad inputs drive.setDrivePower(gamepad1.left_stick_y, gamepad1.left_stick_x, From 65f221cd0c5f93cd39d3dc4c692b78b8754204f5 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 27 Jan 2026 08:14:32 -0500 Subject: [PATCH 41/41] Refactor sorter control logic with helper method Replaced repetitive button handling for sorter positions in TeleOpMain with a new simpleSorterPosition method in SorterControls. This improves code readability and maintainability by centralizing the logic for setting sorter positions based on button input. --- .../ftc/teamcode/TeleOpMain.java | 21 ++++--------------- .../teamcode/Utils_13233/SorterControls.java | 11 ++++++++++ 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index fa66a13..78ea378 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -1,11 +1,8 @@ // Import libraries package org.firstinspires.ftc.teamcode; -import static com.qualcomm.robotcore.hardware.Gamepad.RUMBLE_DURATION_CONTINUOUS; - import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; @@ -89,22 +86,12 @@ public void runOpMode() { // Allows for driver Control of the sorter // Intake positions for the sorter - if (gamepad2.x) { - sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 1); - } else if (gamepad2.y) { - sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 2); - } else if (gamepad2.b) { - sorter.moveSorterToPos(SorterControls.sorterModes.INTAKE, 3); - } + sorter.simpleSorterPosition(gamepad2.x, gamepad2.y, gamepad2.b, + SorterControls.sorterModes.INTAKE); // Launch positons for the sorter - if (gamepad2.dpad_left) { - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1); - } else if (gamepad2.dpad_up) { - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2); - } else if (gamepad2.dpad_right) { - sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3); - } + sorter.simpleSorterPosition(gamepad2.dpad_left, gamepad2.dpad_up, gamepad2.dpad_right, + SorterControls.sorterModes.LAUNCH); // Control the flipper and make sure that when the sorter is moving the flipper is set // to the not active positon diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java index 8b49e0a..0675c6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/SorterControls.java @@ -129,6 +129,17 @@ public void moveSorterToPos(sorterModes mode, int pos) { } } + public void simpleSorterPosition(boolean position1Button, boolean position2Button, + boolean position3Button, sorterModes mode) { + if (position1Button) { + moveSorterToPos(mode, 1); + } else if (position2Button) { + moveSorterToPos(mode, 2); + } else if (position3Button) { + moveSorterToPos(mode, 3); + } + } + /** * Sorts through the balls in the sorter and moves a green ball to the launch position * automatically