From 05260d5e00e1691799e4a78ed4eb5e67e2021565 Mon Sep 17 00:00:00 2001 From: tango8 Date: Wed, 1 Oct 2025 08:50:13 -0400 Subject: [PATCH 01/67] First commit, start of attempt of a kinematic solution, fixed yaw aiming --- .../teamcode/subsystems/AprilTagAimer.java | 135 +++++++++++++++++- .../ftc/teamcode/testing/AprilTagTester.java | 7 +- 2 files changed, 136 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index 2b3379b1548c..721b78b6244a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -11,16 +13,28 @@ public class AprilTagAimer { private final IMU imu; private final DcMotor leftFront, rightFront, leftBack, rightBack; + private final Outtake outtake; + private double power; private final double kP = 0.01; // Full PID can be done later private Double targetAngle = null; - public AprilTagAimer(HardwareMap hardwareMap) { - Movement movement = new Movement(hardwareMap); + public AprilTagAimer(HardwareMap hardwareMap, Movement movement, Outtake outtake) { leftFront = movement.getLeftFront(); rightFront = movement.getRightFront(); leftBack = movement.getLeftBack(); rightBack = movement.getRightBack(); imu = movement.getImu(); + this.outtake = outtake; + power = outtake.getPower(); + } + + public AprilTagAimer(HardwareMap hardwareMap, Movement movement) { + leftFront = movement.getLeftFront(); + rightFront = movement.getRightFront(); + leftBack = movement.getLeftBack(); + rightBack = movement.getRightBack(); + imu = movement.getImu(); + this.outtake = null; } private double angleWrapDegrees(double angle) { @@ -61,4 +75,119 @@ private void stopMotors() { leftBack.setPower(0); rightBack.setPower(0); } -} \ No newline at end of file + + private void setPower(double range, double elevation) { + // It'll be guess and check i guess + outtake.setPower(power); + } + + private void setPowerPhysicsBased(double range, double elevation) { + /* + * Calculates and sets the appropriate power for a flywheel shooter + * based on the desired horizontal range and elevation angle. + * + * This method uses a physics-based model to determine the required + * launch velocity (vInit) to reach a target at a certain distance and height, + * accounting for both gravitational and aerodynamic drag losses. + * + * PHYSICS MODEL + * + * The launch velocity is computed by rearranging the projectile motion formula: + * + * y = h + R * tan(θ) - (g * R²) / (2 * v² * cos²(θ)) + * + * Solving for v: + * + * v = sqrt((g * R²) / (2 * cos²(θ) * (R * tan(θ) + h - y))) + * + * Where: + * - R = horizontal range to target + * - θ = launch angle in radians + * - h = launch height (hood or shooter height) + * - y = target height + * - g = gravitational acceleration + * + * Drag is modeled as: + * Fdrag = (1/2) * Cd * ρ * A * v² + * Pdrag = Fdrag * v = (1/2) * Cd * ρ * A * v³ + * + * Drag energy loss is approximated as: + * dragEnergy = Pdrag * flightTime + * + * The total energy required is: + * totalEnergy = kineticEnergy + dragEnergy + * + * Mechanical power is then: + * requiredPower = totalEnergy / launchDuration + * + * Final motor power is scaled by: + * powerFraction = requiredPower / maxMotorPower + * + * ASSUMPTIONS + * - Ball is launched at a fixed elevation angle. + * - Drag is constant over short trajectory (reasonable for short-range shots). + * - Flywheel imparts energy over a short, fixed time window. + * - No Magnus effect or backspin modeling. + * - Perfect energy transfer (can be tuned with efficiency factor if needed). + * + * TUNABLE PARAMETERS + * - Cd (drag coefficient) + * - launchDuration (how quickly flywheel imparts energy) + * - Pmax (max mechanical power output of motor) + * + */ + + // Constants + final double g = 9.80665; // gravitational acceleration (m/s^2) + final double h = 0; // UNKNOWN launch height (m) + final double y = 1.175; // target height (m) bottom opening lip is .9845, top is 1.3655 + final double rFlywheel = 0; // UNKNOWN flywheel radius (m) + final double Cd = 0.6; // drag coefficient + final double rho = 1.21; // air density (kg/m^3) + final double ballRadius = 0.0635; // 5-inch diameter ball + final double A = Math.PI * ballRadius * ballRadius; // cross-sectional area (m²) + final double maxRPM = 0; // UNKNOWN max RPM of flywheel motor + final double Pmax = 0; // UNKNOWN max mechanical power output of motor (W) + final double ballMass = 0.15; // UNKNOWN should measure ball mass (kg) + final double launchDuration = 0.1; // time flywheel imparts energy to ball (s) + + // Angle conversions + double theta = Math.toRadians(elevation); + double cosTheta = Math.cos(theta); + double tanTheta = Math.tan(theta); + double cosThetaSquared = cosTheta * cosTheta; + + // Check if physically reachable + double denominator = 2 * cosThetaSquared * (range * tanTheta + h - y); + if (denominator <= 0) { + telemetry.addData("Invalid trajectory:", "Denominator ≤ 0 (unreachable target)"); + telemetry.update(); + outtake.setPower(0); + return; + } + + // Required initial velocity + double vInit = Math.sqrt((g * range * range) / denominator); + + // Drag Force and power + double Fdrag = 0.5 * Cd * rho * A * vInit * vInit; // drag force at launch + double Pdrag = Fdrag * vInit; // instantaneous drag power loss + + // Approximate flight time + double vHorizontal = vInit * cosTheta; + double flightTime = range / vHorizontal; + + // Energy calcs + double kineticEnergy = 0.5 * ballMass * vInit * vInit; // energy needed to reach vInit + double dragEnergy = Pdrag * flightTime; // approximate energy lost to drag + double totalEnergyRequired = kineticEnergy + dragEnergy; // total energy to launch + + // Needed mechanical power + double requiredMechanicalPower = totalEnergyRequired / launchDuration; + + double powerFraction = requiredMechanicalPower / Pmax; + powerFraction = Math.max(0, Math.min(1, powerFraction)); + + outtake.setPower(powerFraction); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index ad6ff084aedb..98130f5286e6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -15,8 +15,9 @@ public class AprilTagTester extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { AprilTag aprilTag = new AprilTag(hardwareMap); - AprilTagAimer aprilAimer = new AprilTagAimer(hardwareMap); Movement movement = new Movement(hardwareMap); + // Outtake outtake = new Outtake(hardwareMap); + AprilTagAimer aprilAimer = new AprilTagAimer(hardwareMap, movement); // Pass in outtake when possible GamepadEx gamePadOne = new GamepadEx(gamepad1); GamepadEx gamePadTwo = new GamepadEx(gamepad2); @@ -38,7 +39,7 @@ public void runOpMode() throws InterruptedException { double bearing = aprilTag.getBearing(); aprilAimer.startTurnToAprilTag(bearing); - while (!aprilAimer.updateTurn()) { + while (aprilAimer.updateTurn()) { telemetry.addData("Turning towards angle", bearing); telemetry.update(); sleep(10); @@ -54,7 +55,7 @@ public void runOpMode() throws InterruptedException { double bearing = aprilTag.getBearing(); aprilAimer.startTurnToAprilTag(bearing); - while (!aprilAimer.updateTurn()) { + while (aprilAimer.updateTurn()) { telemetry.addData("Turning towards angle", bearing); telemetry.update(); sleep(10); From e2f7578af121dd38c5c5bc0cf190dba7d70f86ff Mon Sep 17 00:00:00 2001 From: Noah Turner Date: Wed, 1 Oct 2025 16:27:14 -0400 Subject: [PATCH 02/67] Created ColorSensorSystem.java so color sensor can be more easily used by Indexer.java --- .../subsystems/ColorSensorSystem.java | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ColorSensorSystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ColorSensorSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ColorSensorSystem.java new file mode 100644 index 000000000000..e03fa89a06d5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ColorSensorSystem.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class ColorSensorSystem { + + private final int[] GREEN_RGB = {0,255,0}; + private final double GREEN_TOLERANCE = 0.5; //50% + private final int[] PURPLE_RGB = {128,0,128}; + private final double PURPLE_TOLERANCE = 0.5; //50% + private final ColorSensor colorSensor; + + public ColorSensorSystem(HardwareMap hardwareMap) { + colorSensor = hardwareMap.get(ColorSensor.class, "color"); + } + + // checks if the int provided is less than a max and a min calculated using a starting int and a tolerance percent + private boolean checkRange(int num, int origin, double tolerance) { + boolean valid = false; + int min = (int)(origin * (1 - tolerance)); + int max = (int)(origin * (1 + tolerance)); + if (num > min && num < max) { + valid = true; + } + return valid; + } + + // checks if the camera sees purple or green and returns -1 (neither found) 0 (purple found) 1 (green found) + public Indexer.ArtifactColor getColor() { + Indexer.ArtifactColor foundColor = Indexer.ArtifactColor.unknown; + int[] rgb = {colorSensor.red(),colorSensor.green(),colorSensor.blue()}; + boolean foundPurple = true; + for (int i = 0; i < 2; i++) { + foundPurple = (foundPurple && checkRange(rgb[i],PURPLE_RGB[i],PURPLE_TOLERANCE)); + } + if (foundPurple) { + foundColor = Indexer.ArtifactColor.purple; + } else { + boolean foundGreen = true; + for (int i = 0; i < 2; i++) { + foundGreen = (foundGreen && checkRange(rgb[i],GREEN_RGB[i],GREEN_TOLERANCE)); + } + if (foundGreen) foundColor = Indexer.ArtifactColor.green; + } + return foundColor; + } +} \ No newline at end of file From 584ace69b005192892b6dd7002bc60fe39665cdf Mon Sep 17 00:00:00 2001 From: Noah Turner Date: Wed, 1 Oct 2025 16:30:34 -0400 Subject: [PATCH 03/67] In Indexer.java, - added Artifact array to keep track of what colors are in which positions - added stateToColor() - added scanArtifact() - added enum ArtifactColor - added shiftArtifacts() - added moveToColor() - edited moveTo() to yield the thread until the servo finishes turning --- .../ftc/teamcode/subsystems/Indexer.java | 86 +++++++++++++++++-- 1 file changed, 78 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 33010703362a..61d5f557c515 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -4,6 +4,8 @@ import com.arcrobotics.ftclib.hardware.SimpleServo; import com.qualcomm.robotcore.hardware.HardwareMap; +import java.util.concurrent.ConcurrentLinkedQueue; + public class Indexer { //TODO: Optimize the algorithms, it'll work as is though @@ -17,10 +19,21 @@ public class Indexer { private final int ANGLE_TWO = 120; private final int ANGLE_THREE = 240; private final int ANGLE_ALT = 360; - - - private IndexerState state; - + private final IndexerState COLOR_SENSOR_POSITION = IndexerState.one; + private ArtifactColor[] artifacts = { + ArtifactColor.unknown, + ArtifactColor.unknown, + ArtifactColor.unknown + }; + private final ColorSensorSystem colorSensor; + + private IndexerState state = IndexerState.one; + + public enum ArtifactColor { + unknown, + purple, + green + } public enum IndexerState { //i swear these names are temporary we'll do some color coding or sum @@ -34,8 +47,46 @@ public enum IndexerState public Indexer (HardwareMap hardwareMap) { indexerServo = new SimpleServo(hardwareMap, "index",0,360); + colorSensor = new ColorSensorSystem(hardwareMap); } + public ArtifactColor stateToColor(IndexerState colorState) { + ArtifactColor color = ArtifactColor.unknown; + int stateNum = stateToNum(colorState); + if (stateNum == 4) stateNum = 1; + stateNum -= 1; + color = artifacts[stateNum]; + return color; + } + public void scanArtifact() { + ArtifactColor scannedColor = colorSensor.getColor(); + int stateNum = stateToNum(COLOR_SENSOR_POSITION); + if (stateNum == 4) stateNum = 1; + artifacts[stateNum] = scannedColor; + } + public void shiftArtifacts(IndexerState oldState, IndexerState newState) { + int oldNum = stateToNum(oldState) - 1; + if (oldNum == 3) oldNum = 0; + int newNum = stateToNum(newState) - 1; + if (newNum == 3) newNum = 0; + + int difference = newNum - oldNum; + for (int i = 0; i < Math.abs(difference); i++) { + if (difference < 0) { + artifacts = new ArtifactColor[]{artifacts[1],artifacts[2],artifacts[0]}; + } else { + artifacts = new ArtifactColor[]{artifacts[2],artifacts[0],artifacts[1]}; + } + } + } + public void moveToColor(ArtifactColor color) { + ArtifactColor checkingColor = stateToColor(IndexerState.one); + if (checkingColor == color) {moveTo(IndexerState.one); return;} + checkingColor = stateToColor(IndexerState.three); + if (checkingColor == color) {moveTo(IndexerState.three); return;} + checkingColor = stateToColor(IndexerState.two); + if (checkingColor == color) {moveTo(IndexerState.two);} + } public void quickSpin() { switch(state) @@ -56,16 +107,35 @@ public void quickSpin() } public void moveInOrder(int[] arr) { - for(int i : arr){ - moveTo(numToState(i)); - } + Thread moveThread = new Thread(() -> { + for(int i : arr){ + moveTo(numToState(i)); + } + }); + moveThread.start(); } public void moveTo(IndexerState newState) { - indexerServo.turnToAngle((stateToNum(newState) - 1) * 120); + shiftArtifacts(state, newState); + state = newState; + double newAngle = stateToAngle(newState); + indexerServo.turnToAngle(newAngle); + while (indexerServo.getAngle() > newAngle - 5 && indexerServo.getAngle() < newAngle + 5) { + try { + // Simulate the blocking operation of the servo + Thread.sleep(50); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + scanArtifact(); + } + public double stateToAngle(IndexerState newState) { + return (stateToNum(newState) - 1) * 120; } + public IndexerState numToState(int num) { switch (num) From f67d0b5bb14c8a5de526f1b659acea57ed2344ab Mon Sep 17 00:00:00 2001 From: Noah Turner Date: Fri, 3 Oct 2025 17:15:48 -0400 Subject: [PATCH 04/67] Edited the shiftArtifacts() to remove the artifact in the outtake from the array if the ramp actuator is activated --- .../ftc/teamcode/subsystems/Indexer.java | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index b853b96c3c2e..1cbc80c2c962 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -40,12 +40,14 @@ public enum IndexerState oneAlt } private final SimpleServo indexerServo; + private final Actuator actuator; public Indexer (HardwareMap hardwareMap) { state = IndexerState.one; indexerServo = new SimpleServo(hardwareMap, "index",0,360); colorSensor = new ColorSensorSystem(hardwareMap); + actuator = new Actuator(hardwareMap); } public void setIntaking(boolean isIntaking) @@ -85,6 +87,7 @@ public void scanArtifact() { ArtifactColor scannedColor = colorSensor.getColor(); int stateNum = stateToNum(COLOR_SENSOR_POSITION); if (stateNum == 4) stateNum = 1; + stateNum -= 1; artifacts[stateNum] = scannedColor; } public void shiftArtifacts(IndexerState oldState, IndexerState newState) { @@ -101,6 +104,12 @@ public void shiftArtifacts(IndexerState oldState, IndexerState newState) { artifacts = new ArtifactColor[]{artifacts[2],artifacts[0],artifacts[1]}; } } + if (actuator.isActivated()) { // && !getIntaking() not ever set to false yet + int stateNum = stateToNum(state); + if (stateNum == 4) stateNum = 1; + stateNum -= 1; + artifacts[stateNum] = ArtifactColor.unknown; + } } public void moveToColor(ArtifactColor color) { ArtifactColor checkingColor = stateToColor(IndexerState.one); @@ -143,10 +152,10 @@ public void moveInOrder(int[] arr) { public void moveTo(IndexerState newState) { shiftArtifacts(state, newState); - double newAngle = stateToAngle(newState); + double newAngle = (stateToNum(newState) - 1) * 120; // outtake angle calculation if(intaking) { - newAngle = 360%((stateToNum(newState)-1)*120+180); + newAngle = 360%((stateToNum(newState)-1)*120+180); // intake angle calculation indexerServo.turnToAngle(newAngle); } else { indexerServo.turnToAngle(newAngle); @@ -162,9 +171,6 @@ public void moveTo(IndexerState newState) } scanArtifact(); } - public double stateToAngle(IndexerState newState) { - return (stateToNum(newState) - 1) * 120; - } public IndexerState numToState(int num) From f04ddefc44f5c55f9e11b76b15197e67a5f9a014 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 6 Oct 2025 16:57:23 -0400 Subject: [PATCH 05/67] prototyping is cooked in this branch --- .../ftc/teamcode/subsystems/Outtake.java | 2 ++ .../ftc/teamcode/teleop/Prototyping.java | 33 +++++++++++-------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java index eb8b8ea8a41b..07f767bf5b43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java @@ -6,10 +6,12 @@ public class Outtake { private final MotorEx motor; private double power; + final double TICKS_PER_REV = 112.0; public Outtake(HardwareMap hardwareMap) { motor = new MotorEx(hardwareMap, "outtake"); + } public void stop() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java index f6f6810798e1..eaf3e61c2bc4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.ThreeDeadWheelLocalizer; import org.firstinspires.ftc.teamcode.subsystems.Actuator; import org.firstinspires.ftc.teamcode.subsystems.Indexer; import org.firstinspires.ftc.teamcode.subsystems.Intake; @@ -20,34 +21,40 @@ public class Prototyping extends LinearOpMode { //private Intake intake; //private Indexer indexer; //private Actuator actuator; - //private Outtake outtake; - private Movement movement; + private Outtake outtake; + //private Movement movement; + public static class Params + { + public double outtakepower = 0.0; + } + + public static Prototyping.Params PARAMS = new Prototyping.Params(); @Override public void runOpMode() throws InterruptedException { // intake = new Intake(hardwareMap); // indexer = new Indexer(hardwareMap); // actuator = new Actuator(hardwareMap); - movement = new Movement(hardwareMap); - //outtake = new Outtake(hardwareMap); + //movement = new Movement(hardwareMap); + outtake = new Outtake(hardwareMap); GamepadEx gamePadOne = new GamepadEx(gamepad1); GamepadEx gamePadTwo = new GamepadEx(gamepad2); - waitForStart(); while (opModeIsActive()) { gamePadOne.readButtons(); gamePadTwo.readButtons(); teleopTick(gamePadOne, gamePadTwo, telemetry); - + telemetry.update(); } } public void teleopTick(GamepadEx padOne, GamepadEx padTwo, Telemetry telemetry) { - movement.teleopTick(padOne.getLeftX(),padOne.getLeftY(),padOne.getRightX());//,padOne.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER),telemetry); - - //telemetry.addData("Outtake Power: ",outtake.getPower()); + //movement.teleopTick(padOne.getLeftX(),padOne.getLeftY(),padOne.getRightX());//,padOne.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER),telemetry); + //outtake.setPower(PARAMS.outtakepower); + //outtake.run(); + telemetry.addData("Outtake Power: ",outtake.getPower()); if(padTwo.wasJustPressed(GamepadKeys.Button.A)) { //intake.run(!intake.isRunning()); @@ -65,18 +72,18 @@ public void teleopTick(GamepadEx padOne, GamepadEx padTwo, Telemetry telemetry) //indexer.quickSpin(); } if(padTwo.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01){ - //outtake.run(); + outtake.run(); } else { - //outtake.stop(); + outtake.stop(); } if(padTwo.isDown(GamepadKeys.Button.DPAD_UP)) { - //outtake.setPower(outtake.getPower()+0.0005); + outtake.setPower(outtake.getPower()+0.0005); } else if(padTwo.isDown(GamepadKeys.Button.DPAD_DOWN)) { - //outtake.setPower(outtake.getPower()-0.0005); + outtake.setPower(outtake.getPower()-0.0005); } } From 702075930a912a411daffb59acbbd2b3af915e8b Mon Sep 17 00:00:00 2001 From: tango8 Date: Sun, 12 Oct 2025 15:15:47 -0400 Subject: [PATCH 06/67] No longer stopping after locked in on apriltag, its continuous until you tell it to --- .../teamcode/subsystems/AprilTagAimer.java | 42 ++++++------ .../ftc/teamcode/subsystems/Movement.java | 22 +++---- .../ftc/teamcode/testing/AprilTagTester.java | 66 ++++++++++--------- 3 files changed, 65 insertions(+), 65 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index 2b3379b1548c..98d62a0b7d05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -32,33 +32,33 @@ public void startTurnToAprilTag(double bearing) { targetAngle = angleWrapDegrees(currentYaw + bearing); } - public boolean updateTurn() { - if (targetAngle == null) return true; + public double updateTurn() { + if (targetAngle == null) return 0; double currentYaw = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle; double error = angleWrapDegrees(targetAngle - currentYaw); - if (Math.abs(error) < 1) { // within 1 degree - stopMotors(); + double power = kP * error; + power = Math.max(-1, Math.min(1, power)); + + return power; + } + + public boolean checkIfComplete() { + if (targetAngle == null) return true; + + double currentYaw = imu.getRobotOrientation( + AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES + ).firstAngle; + + double error = angleWrapDegrees(targetAngle - currentYaw); + + // within 1 degree + if (Math.abs(error) < 1) { targetAngle = null; return true; } - else { - double power = kP * error; - power = Math.max(-1, Math.min(1, power)); - - leftFront.setPower(power); - rightFront.setPower(-power); - leftBack.setPower(power); - rightBack.setPower(-power); - return false; - } - } - private void stopMotors() { - leftFront.setPower(0); - rightFront.setPower(0); - leftBack.setPower(0); - rightBack.setPower(0); + return false; } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java index 931e941d9c06..fdeb58a91274 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java @@ -50,26 +50,22 @@ public Movement(@NonNull HardwareMap map){ // tick for teleop - public void teleopTick(double leftStickX, double leftStickY, double rightStickX/*, boolean toggle,*/){ - double axial = -leftStickY * STRAFE_MULTIPLIER; // Note: pushing stick forward gives negative value + public void teleopTick(double leftStickX, double leftStickY, double rightStickX, double turnCorrection){ + double axial = -leftStickY * STRAFE_MULTIPLIER; double lateral = -leftStickX * STRAFE_MULTIPLIER; - double yaw = rightStickX * ROTATION_MULTIPLIER; - // Combine the joystick requests for each axis-motion to determine each wheel's power. - // Set up a variable for each drive wheel to save the power level for telemetry. + double yaw = (rightStickX * ROTATION_MULTIPLIER) + turnCorrection; + double leftFrontPower = axial + lateral + yaw; double rightFrontPower = axial - lateral - yaw; double leftBackPower = axial - lateral + yaw; double rightBackPower = axial + lateral - yaw; - - // Normalize the values so no wheel power exceeds 100% - // This ensures that the robot maintains the desired motion. double max = Math.max(Math.max(Math.max( - Math.abs(leftFrontPower), - Math.abs(rightFrontPower)), - Math.abs(leftBackPower)), - Math.abs(rightBackPower)); + Math.abs(leftFrontPower), + Math.abs(rightFrontPower)), + Math.abs(leftBackPower)), + Math.abs(rightBackPower)); if (max > 1.0) { leftFrontPower /= max; rightFrontPower /= max; @@ -77,13 +73,13 @@ public void teleopTick(double leftStickX, double leftStickY, double rightStickX/ rightBackPower /= max; } - // Send calculated power to wheels leftFront.setPower(leftFrontPower); rightFront.setPower(rightFrontPower); leftBack.setPower(leftBackPower); rightBack.setPower(rightBackPower); } + // NOTE DOESN'T WORK WITH APRILTAG RN public void teleopTickFieldCentric(double leftStickX, double leftStickY, double rightStickX, boolean start){ double axial = -leftStickY; // Remember, Y stick value is reversed double lateral = -leftStickX; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index ad6ff084aedb..b5714f013f8d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -19,57 +19,61 @@ public void runOpMode() throws InterruptedException { Movement movement = new Movement(hardwareMap); GamepadEx gamePadOne = new GamepadEx(gamepad1); GamepadEx gamePadTwo = new GamepadEx(gamepad2); + boolean continuousAprilTagLock = false; + double turnCorrection = 0; waitForStart(); while (opModeIsActive()) { gamePadOne.readButtons(); gamePadTwo.readButtons(); - movement.teleopTick(gamePadOne.getLeftX(),gamePadOne.getLeftY(),gamePadOne.getRightX()); + if (continuousAprilTagLock) { + turnCorrection = aprilAimer.updateTurn(); + } + else { + turnCorrection = 0; + } + movement.teleopTick(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection); - telemetry.addData("X:", "Scan obelisk apriltag"); - telemetry.addData("A", "Test april tag aimer with blue alliance apriltag"); - telemetry.addData("B", "Test april tag aimer with red alliance apriltag"); + telemetry.addData("Y:", "Scan obelisk apriltag"); + telemetry.addData("A:", "Continuously lock into apriltag"); + telemetry.addData("B:", "Stop continuously locking into apriltag"); + telemetry.addData("Left Bumper", "Set to blue alliance apriltag"); + telemetry.addData("Right Bumper", "Set to red alliance apriltag"); telemetry.update(); + if (gamePadTwo.wasJustPressed(GamepadKeys.Button.Y)) { + aprilTag.scanObeliskTag(); + telemetry.addData("This is probably only for auto,", "as we can just memorize the 3 possible patterns for teleop"); + telemetry.addData("Obelisk apriltag ID: ", aprilTag.getObeliskId()); + telemetry.update(); + } + if (gamePadTwo.wasJustPressed(GamepadKeys.Button.A)) { - aprilTag.setGoalTagID(20); aprilTag.scanGoalTag(); double bearing = aprilTag.getBearing(); aprilAimer.startTurnToAprilTag(bearing); + continuousAprilTagLock = true; - while (!aprilAimer.updateTurn()) { - telemetry.addData("Turning towards angle", bearing); - telemetry.update(); - sleep(10); - } - - telemetry.addData("Finished", "locking on to apriltag"); + telemetry.addData("Continuously locked in on", "apriltag"); telemetry.update(); } - + if (gamePadTwo.wasJustPressed(GamepadKeys.Button.B)) { - aprilTag.setGoalTagID(24); - aprilTag.scanGoalTag(); - double bearing = aprilTag.getBearing(); - aprilAimer.startTurnToAprilTag(bearing); - - while (!aprilAimer.updateTurn()) { - telemetry.addData("Turning towards angle", bearing); - telemetry.update(); - sleep(10); - } - - telemetry.addData("Finished", "locking on to apriltag"); + continuousAprilTagLock = false; + + telemetry.addData("Stopped continuous lock in on", "apriltag"); telemetry.update(); } - if (gamePadTwo.wasJustPressed(GamepadKeys.Button.X)) { - aprilTag.scanObeliskTag(); - telemetry.addData("This is probably only for auto,", "as we can just memorize the 3 possible patterns for teleop"); - telemetry.addData("Obelisk apriltag ID: ", aprilTag.getObeliskId()); - telemetry.update(); + if(gamePadTwo.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) { + aprilTag.setGoalTagID(20); + telemetry.addData("Set to", "Blue Alliance") ; + } + if(gamePadTwo.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { + aprilTag.setGoalTagID(24); + telemetry.addData("Set to", "Red Alliance"); } } } -} \ No newline at end of file +} From ee7c8c3b73a91179550e0ef641d576912230ed60 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sun, 12 Oct 2025 15:21:18 -0400 Subject: [PATCH 07/67] fixed prototyping parameter error --- .../java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java index f6f6810798e1..13b775f11841 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java @@ -45,7 +45,7 @@ public void runOpMode() throws InterruptedException { } public void teleopTick(GamepadEx padOne, GamepadEx padTwo, Telemetry telemetry) { - movement.teleopTick(padOne.getLeftX(),padOne.getLeftY(),padOne.getRightX());//,padOne.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER),telemetry); + movement.teleopTick(padOne.getLeftX(),padOne.getLeftY(),padOne.getRightX(), 0);//,padOne.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER),telemetry); //telemetry.addData("Outtake Power: ",outtake.getPower()); if(padTwo.wasJustPressed(GamepadKeys.Button.A)) From 5dfd5ac40f23375fee804d94083deea57d0827c6 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sun, 12 Oct 2025 15:55:30 -0400 Subject: [PATCH 08/67] Removed redundancies and added PIDF --- .../teamcode/subsystems/AprilTagAimer.java | 63 +++++++++---------- .../ftc/teamcode/testing/AprilTagTester.java | 12 ++-- 2 files changed, 38 insertions(+), 37 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index 98d62a0b7d05..6454cb9588d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -10,16 +10,24 @@ public class AprilTagAimer { private final IMU imu; - private final DcMotor leftFront, rightFront, leftBack, rightBack; - private final double kP = 0.01; // Full PID can be done later - private Double targetAngle = null; + private final double kP = 0.01; + private final double kI = 0.001; + private final double kD = 0.005; + private final double kF = 0.05; + + private double integral = 0; + private double lastError = 0; + private long lastTimestamp = 0; + + /* When and why to tune these + P (Proportional) Changes core power of turns, its proportional + I (Integral) Maybe rarely if robot consistently falls short of the target (steady-state error). + D (Derivative) Increase to dampen motion and reduce overshoot. Good for smoothing quick heading corrections. + F (Feedforward) Maybe, its a constant, increase to help overcome drivetrain static friction and give better response when error is small. + */ public AprilTagAimer(HardwareMap hardwareMap) { Movement movement = new Movement(hardwareMap); - leftFront = movement.getLeftFront(); - rightFront = movement.getRightFront(); - leftBack = movement.getLeftBack(); - rightBack = movement.getRightBack(); imu = movement.getImu(); } @@ -27,38 +35,29 @@ private double angleWrapDegrees(double angle) { return (angle + 180) % 360 - 180; } - public void startTurnToAprilTag(double bearing) { - double currentYaw = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle; - targetAngle = angleWrapDegrees(currentYaw + bearing); - } - - public double updateTurn() { - if (targetAngle == null) return 0; - + public double calculateTurnPowerToBearing(double bearing) { double currentYaw = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle; + double targetAngle = angleWrapDegrees(currentYaw + bearing); double error = angleWrapDegrees(targetAngle - currentYaw); - double power = kP * error; - power = Math.max(-1, Math.min(1, power)); + // Time diff in seconds + long currentTime = System.currentTimeMillis(); + double deltaTime = (currentTime - lastTimestamp) / 1000.0; + lastTimestamp = currentTime; - return power; - } - - public boolean checkIfComplete() { - if (targetAngle == null) return true; + // Integral accumulation + integral += error * deltaTime; - double currentYaw = imu.getRobotOrientation( - AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES - ).firstAngle; + // Derivative term + double derivative = (error - lastError) / deltaTime; + lastError = error; - double error = angleWrapDegrees(targetAngle - currentYaw); + // Gets proper direction + double feedforward = Math.signum(error) * kF; - // within 1 degree - if (Math.abs(error) < 1) { - targetAngle = null; - return true; - } + // PID output + double power = kP * error + kI * integral + kD * derivative + feedforward; - return false; + return Math.max(-1, Math.min(1, power)); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index b5714f013f8d..c2f64d21688e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -20,18 +20,21 @@ public void runOpMode() throws InterruptedException { GamepadEx gamePadOne = new GamepadEx(gamepad1); GamepadEx gamePadTwo = new GamepadEx(gamepad2); boolean continuousAprilTagLock = false; - double turnCorrection = 0; waitForStart(); while (opModeIsActive()) { gamePadOne.readButtons(); gamePadTwo.readButtons(); + double turnCorrection; if (continuousAprilTagLock) { - turnCorrection = aprilAimer.updateTurn(); + aprilTag.scanGoalTag(); + double bearing = aprilTag.getBearing(); + turnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); } else { turnCorrection = 0; + } movement.teleopTick(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection); @@ -50,9 +53,6 @@ public void runOpMode() throws InterruptedException { } if (gamePadTwo.wasJustPressed(GamepadKeys.Button.A)) { - aprilTag.scanGoalTag(); - double bearing = aprilTag.getBearing(); - aprilAimer.startTurnToAprilTag(bearing); continuousAprilTagLock = true; telemetry.addData("Continuously locked in on", "apriltag"); @@ -69,10 +69,12 @@ public void runOpMode() throws InterruptedException { if(gamePadTwo.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) { aprilTag.setGoalTagID(20); telemetry.addData("Set to", "Blue Alliance") ; + telemetry.update(); } if(gamePadTwo.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { aprilTag.setGoalTagID(24); telemetry.addData("Set to", "Red Alliance"); + telemetry.update(); } } } From 0b8e7625bf8bf25a42df47629435e2def4913a9f Mon Sep 17 00:00:00 2001 From: tango8 Date: Sun, 12 Oct 2025 16:21:17 -0400 Subject: [PATCH 09/67] Control hub direction and Z is yaw not X thanks andrew --- .../firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java | 2 +- .../org/firstinspires/ftc/teamcode/subsystems/Movement.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index 6454cb9588d4..6d995ac11d2f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -36,7 +36,7 @@ private double angleWrapDegrees(double angle) { } public double calculateTurnPowerToBearing(double bearing) { - double currentYaw = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).firstAngle; + double currentYaw = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle; double targetAngle = angleWrapDegrees(currentYaw + bearing); double error = angleWrapDegrees(targetAngle - currentYaw); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java index fdeb58a91274..06ed2efae217 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java @@ -33,7 +33,7 @@ public Movement(@NonNull HardwareMap map){ imu = map.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, RevHubOrientationOnRobot.UsbFacingDirection.UP)); imu.initialize(parameters); From c2a30fcae5b4653dc772ad4299b09fdf09d1fdbe Mon Sep 17 00:00:00 2001 From: tango8 Date: Sun, 12 Oct 2025 23:45:04 -0400 Subject: [PATCH 10/67] Many small changes, smoother joystick apriltag improved, no more imu, more logging --- .../ftc/teamcode/subsystems/AprilTag.java | 15 ++++---- .../teamcode/subsystems/AprilTagAimer.java | 30 ++++++++++------ .../ftc/teamcode/subsystems/Movement.java | 35 ++++++++----------- .../ftc/teamcode/teleop/Prototyping.java | 20 +++++------ .../ftc/teamcode/testing/AprilTagTester.java | 32 +++++++++-------- 5 files changed, 68 insertions(+), 64 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java index 5bdd96f3a3d3..58922ccc70e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java @@ -1,8 +1,6 @@ package org.firstinspires.ftc.teamcode.subsystems; -import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; @@ -31,7 +29,7 @@ public AprilTag(HardwareMap hardwareMap) { .setTagLibrary(library) .build(); WebcamName webcamname = hardwareMap.get(WebcamName.class, "webcam"); - portal = VisionPortal.easyCreateWithDefaults(hardwareMap.get(WebcamName.class, "webcam"), processor); + portal = VisionPortal.easyCreateWithDefaults(webcamname, processor); } public void toggle(boolean bool) { @@ -51,12 +49,15 @@ public void scanObeliskTag() { public void scanGoalTag() { id = -1; - bearing = Integer.MAX_VALUE; + bearing = Double.NaN; + elevation = Double.NaN; + range = Double.NaN; + // If camera is facing to the right of the center of the cam (if it needs to move to the left) the bearing is positive. List detectionList = processor.getDetections(); for (AprilTagDetection detection : detectionList) { // goalTagID should be gotten before round/during auto - if (detection.id == goalTagID) { + if (detection.id == goalTagID && detection.ftcPose != null) { range = detection.ftcPose.range; // distance from camera to center of tag bearing = detection.ftcPose.bearing; // the angle (left/right) the camera must turn to directly point at the tag center elevation = detection.ftcPose.elevation; // the angle (up/down) the camera must turn to directly point at the tag center @@ -64,10 +65,6 @@ public void scanGoalTag() { break; } } - - if (bearing == Integer.MAX_VALUE) { - bearing = 0; - } } public void setGoalTagID(int allianceTagID) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index 6d995ac11d2f..f997c4519a1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -1,20 +1,17 @@ package org.firstinspires.ftc.teamcode.subsystems; -import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; public class AprilTagAimer { - private final IMU imu; - private final double kP = 0.01; private final double kI = 0.001; private final double kD = 0.005; private final double kF = 0.05; + private final double maxIntegral = 1.0; private double integral = 0; private double lastError = 0; @@ -27,8 +24,6 @@ public class AprilTagAimer { F (Feedforward) Maybe, its a constant, increase to help overcome drivetrain static friction and give better response when error is small. */ public AprilTagAimer(HardwareMap hardwareMap) { - Movement movement = new Movement(hardwareMap); - imu = movement.getImu(); } private double angleWrapDegrees(double angle) { @@ -36,17 +31,32 @@ private double angleWrapDegrees(double angle) { } public double calculateTurnPowerToBearing(double bearing) { - double currentYaw = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle; - double targetAngle = angleWrapDegrees(currentYaw + bearing); - double error = angleWrapDegrees(targetAngle - currentYaw); + // If apriltag lost - reset PID state and return no correction + if (Double.isNaN(bearing)) { + integral = 0; + lastError = 0; + lastTimestamp = 0; + return 0.0; + } + + double error = -angleWrapDegrees(bearing); // Time diff in seconds long currentTime = System.currentTimeMillis(); double deltaTime = (currentTime - lastTimestamp) / 1000.0; + + if (lastTimestamp == 0) { + deltaTime = 0.01; // assume 10ms for first call + } else { + deltaTime = (currentTime - lastTimestamp) / 1000.0; + } + lastTimestamp = currentTime; + // Integral accumulation integral += error * deltaTime; + integral = Math.max(-maxIntegral, Math.min(maxIntegral, integral)); // Derivative term double derivative = (error - lastError) / deltaTime; @@ -55,7 +65,7 @@ public double calculateTurnPowerToBearing(double bearing) { // Gets proper direction double feedforward = Math.signum(error) * kF; - // PID output + // PIDF output double power = kP * error + kI * integral + kD * derivative + feedforward; return Math.max(-1, Math.min(1, power)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java index 06ed2efae217..15cc076e08c8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java @@ -18,7 +18,7 @@ public class Movement { private final DcMotor leftFront, leftBack, rightFront, rightBack; private final IMU imu; - private final double STRAFE_MULTIPLIER = 0.8, ROTATION_MULTIPLIER = 0.3; + private final double STRAFE_MULTIPLIER = 0.6, ROTATION_MULTIPLIER = 0.5; /** * Initializes a Movement instance. @@ -38,9 +38,7 @@ public Movement(@NonNull HardwareMap map){ imu.initialize(parameters); - // reversed motor, so that when positive power is applied to all motors left and right spin in opp. directions(move foward instead of spinning) - rightFront.setDirection(DcMotorSimple.Direction.REVERSE); - rightBack.setDirection(DcMotorSimple.Direction.REVERSE); + leftBack.setDirection(DcMotorSimple.Direction.REVERSE); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -53,25 +51,20 @@ public Movement(@NonNull HardwareMap map){ public void teleopTick(double leftStickX, double leftStickY, double rightStickX, double turnCorrection){ double axial = -leftStickY * STRAFE_MULTIPLIER; double lateral = -leftStickX * STRAFE_MULTIPLIER; - - double yaw = (rightStickX * ROTATION_MULTIPLIER) + turnCorrection; + double yaw = -(rightStickX * ROTATION_MULTIPLIER) + turnCorrection; double leftFrontPower = axial + lateral + yaw; double rightFrontPower = axial - lateral - yaw; double leftBackPower = axial - lateral + yaw; double rightBackPower = axial + lateral - yaw; - double max = Math.max(Math.max(Math.max( - Math.abs(leftFrontPower), - Math.abs(rightFrontPower)), - Math.abs(leftBackPower)), - Math.abs(rightBackPower)); - if (max > 1.0) { - leftFrontPower /= max; - rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; - } + // For smoother joystick movement + double denominator = Math.max(1.0, Math.abs(axial) + Math.abs(lateral) + Math.abs(yaw)); + + leftFrontPower /= denominator; + rightFrontPower /= denominator; + leftBackPower /= denominator; + rightBackPower /= denominator; leftFront.setPower(leftFrontPower); rightFront.setPower(rightFrontPower); @@ -80,10 +73,10 @@ public void teleopTick(double leftStickX, double leftStickY, double rightStickX, } // NOTE DOESN'T WORK WITH APRILTAG RN - public void teleopTickFieldCentric(double leftStickX, double leftStickY, double rightStickX, boolean start){ - double axial = -leftStickY; // Remember, Y stick value is reversed - double lateral = -leftStickX; - double yaw = rightStickX; + public void teleopTickFieldCentric(double leftStickX, double leftStickY, double rightStickX, double turnCorrection, boolean start){ + double axial = -leftStickY * STRAFE_MULTIPLIER; + double lateral = -leftStickX * STRAFE_MULTIPLIER; + double yaw = -(rightStickX * ROTATION_MULTIPLIER) + turnCorrection; // This button choice was made so that it is hard to hit on accident, // it can be freely changed based on preference. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java index 13b775f11841..d49989f3ac9b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java @@ -17,20 +17,20 @@ */ @TeleOp(name = "Proto", group = "AA_main") public class Prototyping extends LinearOpMode { - //private Intake intake; + private Intake intake; //private Indexer indexer; //private Actuator actuator; - //private Outtake outtake; + private Outtake outtake; private Movement movement; @Override public void runOpMode() throws InterruptedException { - // intake = new Intake(hardwareMap); + intake = new Intake(hardwareMap); // indexer = new Indexer(hardwareMap); // actuator = new Actuator(hardwareMap); movement = new Movement(hardwareMap); - //outtake = new Outtake(hardwareMap); + outtake = new Outtake(hardwareMap); GamepadEx gamePadOne = new GamepadEx(gamepad1); GamepadEx gamePadTwo = new GamepadEx(gamepad2); @@ -47,10 +47,10 @@ public void runOpMode() throws InterruptedException { public void teleopTick(GamepadEx padOne, GamepadEx padTwo, Telemetry telemetry) { movement.teleopTick(padOne.getLeftX(),padOne.getLeftY(),padOne.getRightX(), 0);//,padOne.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER),telemetry); - //telemetry.addData("Outtake Power: ",outtake.getPower()); + telemetry.addData("Outtake Power: ",outtake.getPower()); if(padTwo.wasJustPressed(GamepadKeys.Button.A)) { - //intake.run(!intake.isRunning()); + intake.run(!intake.isRunning()); } if(padTwo.wasJustPressed(GamepadKeys.Button.B)) { @@ -65,18 +65,18 @@ public void teleopTick(GamepadEx padOne, GamepadEx padTwo, Telemetry telemetry) //indexer.quickSpin(); } if(padTwo.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01){ - //outtake.run(); + outtake.run(); } else { - //outtake.stop(); + outtake.stop(); } if(padTwo.isDown(GamepadKeys.Button.DPAD_UP)) { - //outtake.setPower(outtake.getPower()+0.0005); + outtake.setPower(outtake.getPower()+0.0005); } else if(padTwo.isDown(GamepadKeys.Button.DPAD_DOWN)) { - //outtake.setPower(outtake.getPower()-0.0005); + outtake.setPower(outtake.getPower()-0.0005); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index c2f64d21688e..dcc3f93653d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -21,6 +21,13 @@ public void runOpMode() throws InterruptedException { GamepadEx gamePadTwo = new GamepadEx(gamepad2); boolean continuousAprilTagLock = false; + telemetry.addData("Gamepad 2 Y:", "Scan obelisk apriltag"); + telemetry.addData("Gamepad 2 A:", "Continuously lock into apriltag"); + telemetry.addData("Gamepad 2 B:", "Stop continuously locking into apriltag"); + telemetry.addData("Gamepad 2 Left Bumper", "Set to blue alliance apriltag"); + telemetry.addData("Gamepad 2 Right Bumper", "Set to red alliance apriltag"); + telemetry.update(); + waitForStart(); while (opModeIsActive()) { gamePadOne.readButtons(); @@ -30,52 +37,49 @@ public void runOpMode() throws InterruptedException { if (continuousAprilTagLock) { aprilTag.scanGoalTag(); double bearing = aprilTag.getBearing(); - turnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); + if (Double.isNaN(bearing)) { + turnCorrection = 0; + } else { + turnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); + } } else { turnCorrection = 0; - } - movement.teleopTick(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection); - telemetry.addData("Y:", "Scan obelisk apriltag"); - telemetry.addData("A:", "Continuously lock into apriltag"); - telemetry.addData("B:", "Stop continuously locking into apriltag"); - telemetry.addData("Left Bumper", "Set to blue alliance apriltag"); - telemetry.addData("Right Bumper", "Set to red alliance apriltag"); - telemetry.update(); + movement.teleopTick(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection); if (gamePadTwo.wasJustPressed(GamepadKeys.Button.Y)) { aprilTag.scanObeliskTag(); telemetry.addData("This is probably only for auto,", "as we can just memorize the 3 possible patterns for teleop"); telemetry.addData("Obelisk apriltag ID: ", aprilTag.getObeliskId()); - telemetry.update(); } if (gamePadTwo.wasJustPressed(GamepadKeys.Button.A)) { continuousAprilTagLock = true; telemetry.addData("Continuously locked in on", "apriltag"); - telemetry.update(); + telemetry.addData("Goal tag bearing", aprilTag.getBearing()); + telemetry.addData("Goal tag elevation", aprilTag.getElevation()); + telemetry.addData("Goal tag range", aprilTag.getRange()); } if (gamePadTwo.wasJustPressed(GamepadKeys.Button.B)) { continuousAprilTagLock = false; telemetry.addData("Stopped continuous lock in on", "apriltag"); - telemetry.update(); } if(gamePadTwo.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) { aprilTag.setGoalTagID(20); telemetry.addData("Set to", "Blue Alliance") ; - telemetry.update(); } if(gamePadTwo.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) { aprilTag.setGoalTagID(24); telemetry.addData("Set to", "Red Alliance"); - telemetry.update(); } + + telemetry.update(); } } } From 384007ea76775a92f69464b1eec59c9ec02ade3c Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Thu, 16 Oct 2025 19:25:00 -0400 Subject: [PATCH 11/67] testing 1 2 3 (aura) --- .../firstinspires/ftc/teamcode/testing/SpindexerTester.java | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java new file mode 100644 index 000000000000..463144f830d9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.testing; + +public class SpindexerTester { +} From 1435c7509f965b12e943b15209d52e70b19c6a02 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Thu, 16 Oct 2025 19:56:19 -0400 Subject: [PATCH 12/67] setIntaking logic update --- .../org/firstinspires/ftc/teamcode/subsystems/Indexer.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 1cbc80c2c962..94ec7c0bf9b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -52,7 +52,12 @@ public Indexer (HardwareMap hardwareMap) public void setIntaking(boolean isIntaking) { - intaking = isIntaking; + if(isIntaking != intaking) { + if (isIntaking) + startIntake(); + else + startOuttake(); + } } public void startIntake() From cb6abd3417db02151c1871441e4b1a003d84fe20 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Thu, 16 Oct 2025 19:56:40 -0400 Subject: [PATCH 13/67] Spindexer+color sensor test opmode (sigma boy) --- .../ftc/teamcode/testing/SpindexerTester.java | 67 ++++++++++++++++++- 1 file changed, 66 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index 463144f830d9..2a8c38c03725 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -1,4 +1,69 @@ package org.firstinspires.ftc.teamcode.testing; -public class SpindexerTester { +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.OpticalDistanceSensor; + +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + +@TeleOp(name = "SpindexerTest", group = "Teleop") +public class SpindexerTester extends LinearOpMode { + ColorSensor colorSensor; + Indexer indexer; + GamepadEx gp2; + + @Override + public void runOpMode(){ + colorSensor = hardwareMap.get(ColorSensor.class, "color"); + indexer = new Indexer(hardwareMap); + gp2 = new GamepadEx(gamepad2); + + waitForStart(); + while(opModeIsActive()){ + if(gp2.wasJustPressed(GamepadKeys.Button.DPAD_UP)){ + indexer.nextState(); + } + if(gp2.wasJustPressed(GamepadKeys.Button.A)) + { + indexer.setIntaking(!indexer.getIntaking()); + } + telemetry.addData("Light Detected:",((OpticalDistanceSensor) colorSensor).getLightDetected()); + telemetry.addData("Red", colorSensor.red()); + telemetry.addData("Green", colorSensor.green()); + telemetry.addData("Blue", colorSensor.blue()); + + int r = colorSensor.red(); + int g = colorSensor.green(); + int b = colorSensor.blue(); + + double total = r + g + b; + double rRatio = r / total; + double gRatio = g / total; + double bRatio = b / total; + + String mainColor; + + if (rRatio > 0.45 && gRatio < 0.35 && bRatio < 0.35) { + mainColor = "Red"; + } else if (rRatio > 0.45 && gRatio > 0.25 && bRatio < 0.20) { + mainColor = "Orange"; + } else if (rRatio > 0.38 && gRatio > 0.38 && bRatio < 0.25) { + mainColor = "Yellow"; + } else if (gRatio > 0.45 && rRatio < 0.35 && bRatio < 0.35) { + mainColor = "Green"; + } else if (bRatio > 0.45 && rRatio < 0.35 && gRatio < 0.35) { + mainColor = "Blue"; + } else if (rRatio > 0.35 && bRatio > 0.35 && gRatio < 0.30) { + mainColor = "Purple"; + } else { + mainColor = "Unclear"; + } + telemetry.addData("Color Detected overall:",mainColor); + + telemetry.update(); + } + } } From 5f6f65cfd1518dc65aa581f8ae54a7a1c8ee464f Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Thu, 16 Oct 2025 20:00:39 -0400 Subject: [PATCH 14/67] HOLY CANNOLI ANDROID STUDIO HAS AN AUTOMATIC THING THAT BREAKS OUT A FUNCTION --- .../ftc/teamcode/testing/SpindexerTester.java | 52 +++++++++++-------- 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index 2a8c38c03725..e2eb0f7a6601 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.testing; +import androidx.annotation.NonNull; + import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -39,31 +41,37 @@ public void runOpMode(){ int g = colorSensor.green(); int b = colorSensor.blue(); - double total = r + g + b; - double rRatio = r / total; - double gRatio = g / total; - double bRatio = b / total; - - String mainColor; - - if (rRatio > 0.45 && gRatio < 0.35 && bRatio < 0.35) { - mainColor = "Red"; - } else if (rRatio > 0.45 && gRatio > 0.25 && bRatio < 0.20) { - mainColor = "Orange"; - } else if (rRatio > 0.38 && gRatio > 0.38 && bRatio < 0.25) { - mainColor = "Yellow"; - } else if (gRatio > 0.45 && rRatio < 0.35 && bRatio < 0.35) { - mainColor = "Green"; - } else if (bRatio > 0.45 && rRatio < 0.35 && gRatio < 0.35) { - mainColor = "Blue"; - } else if (rRatio > 0.35 && bRatio > 0.35 && gRatio < 0.30) { - mainColor = "Purple"; - } else { - mainColor = "Unclear"; - } + String mainColor = getMainColor(r, g, b); telemetry.addData("Color Detected overall:",mainColor); telemetry.update(); } } + + @NonNull + private static String getMainColor(int r, int g, int b) { + double total = r+g+b; + double rRatio = r / total; + double gRatio = g / total; + double bRatio = b / total; + + String mainColor; + + if (rRatio > 0.45 && gRatio < 0.35 && bRatio < 0.35) { + mainColor = "Red"; + } else if (rRatio > 0.45 && gRatio > 0.25 && bRatio < 0.20) { + mainColor = "Orange"; + } else if (rRatio > 0.38 && gRatio > 0.38 && bRatio < 0.25) { + mainColor = "Yellow"; + } else if (gRatio > 0.45 && rRatio < 0.35 && bRatio < 0.35) { + mainColor = "Green"; + } else if (bRatio > 0.45 && rRatio < 0.35 && gRatio < 0.35) { + mainColor = "Blue"; + } else if (rRatio > 0.35 && bRatio > 0.35 && gRatio < 0.30) { + mainColor = "Purple"; + } else { + mainColor = "Unclear"; + } + return mainColor; + } } From d456944bd7f99641f7a76d55d95be80a76c6382f Mon Sep 17 00:00:00 2001 From: tango8 Date: Fri, 17 Oct 2025 16:15:46 -0400 Subject: [PATCH 15/67] Spindexer stuff for testing --- .../ftc/teamcode/subsystems/Intake.java | 2 +- .../ftc/teamcode/testing/SpindexerTester.java | 33 ++++++++++++------- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index b6f184f97ff6..f4482f7f1747 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -7,7 +7,7 @@ public class Intake { // ARC Thunder Vortex - private final double INTAKING_POWER = 0.0; + private final double INTAKING_POWER = -1.0; private MotorEx intakeMotor; private boolean running; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index e2eb0f7a6601..7da596ad4e9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -10,39 +10,48 @@ import com.qualcomm.robotcore.hardware.OpticalDistanceSensor; import org.firstinspires.ftc.teamcode.subsystems.Indexer; +import org.firstinspires.ftc.teamcode.subsystems.Intake; @TeleOp(name = "SpindexerTest", group = "Teleop") public class SpindexerTester extends LinearOpMode { - ColorSensor colorSensor; + //ColorSensor colorSensor; Indexer indexer; + Intake intake; GamepadEx gp2; @Override public void runOpMode(){ - colorSensor = hardwareMap.get(ColorSensor.class, "color"); + // colorSensor = hardwareMap.get(ColorSensor.class, "color"); indexer = new Indexer(hardwareMap); + intake = new Intake(hardwareMap); gp2 = new GamepadEx(gamepad2); waitForStart(); while(opModeIsActive()){ if(gp2.wasJustPressed(GamepadKeys.Button.DPAD_UP)){ - indexer.nextState(); + indexer.moveTo(indexer.nextState()); } if(gp2.wasJustPressed(GamepadKeys.Button.A)) { indexer.setIntaking(!indexer.getIntaking()); } - telemetry.addData("Light Detected:",((OpticalDistanceSensor) colorSensor).getLightDetected()); - telemetry.addData("Red", colorSensor.red()); - telemetry.addData("Green", colorSensor.green()); - telemetry.addData("Blue", colorSensor.blue()); + if(gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01){ + intake.run(true); + } + else { + intake.run(false); + } + //telemetry.addData("Light Detected:",((OpticalDistanceSensor) colorSensor).getLightDetected()); + //telemetry.addData("Red", colorSensor.red()); + //telemetry.addData("Green", colorSensor.green()); + //telemetry.addData("Blue", colorSensor.blue()); - int r = colorSensor.red(); - int g = colorSensor.green(); - int b = colorSensor.blue(); + //int r = colorSensor.red(); + //int g = colorSensor.green(); + //int b = colorSensor.blue(); - String mainColor = getMainColor(r, g, b); - telemetry.addData("Color Detected overall:",mainColor); + //String mainColor = getMainColor(r, g, b); + //telemetry.addData("Color Detected overall:",mainColor); telemetry.update(); } From 509367f24d5510781d89372bad2ad65ad65c21c0 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sat, 18 Oct 2025 12:03:19 -0400 Subject: [PATCH 16/67] Spindexer fixes, dynamic waiting times from servo move to sensing color --- .../ftc/teamcode/subsystems/Indexer.java | 70 ++++++++++++------- .../ftc/teamcode/testing/SpindexerTester.java | 21 +++++- 2 files changed, 63 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 94ec7c0bf9b9..457a4324887c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -1,19 +1,12 @@ package org.firstinspires.ftc.teamcode.subsystems; -import com.arcrobotics.ftclib.hardware.ServoEx; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; import java.util.concurrent.ConcurrentLinkedQueue; public class Indexer { - - //TODO: Optimize the algorithms, it'll work as is though - - // 0, 120, 240, 360 are the possible angles - // since 0 and 360 are the same ball, no matter where you are, - // you can "quick spin" to shoot all 3 balls quickly - private IndexerState state; private boolean intaking = true; @@ -42,6 +35,14 @@ public enum IndexerState private final SimpleServo indexerServo; private final Actuator actuator; + // Color sensor timing stuff + private final ElapsedTime scanTimer = new ElapsedTime(); + private boolean scanPending = false; + private double scanDelay; + private final double msPerDegree = 0.6; // tune this + private final double minWait = 100; + private final double maxWait = 300; + private double lastAngle = 0; public Indexer (HardwareMap hardwareMap) { state = IndexerState.one; @@ -50,13 +51,10 @@ public Indexer (HardwareMap hardwareMap) actuator = new Actuator(hardwareMap); } - public void setIntaking(boolean isIntaking) - { - if(isIntaking != intaking) { - if (isIntaking) - startIntake(); - else - startOuttake(); + public void setIntaking(boolean isIntaking) { + if (this.intaking != isIntaking) { + this.intaking = isIntaking; + moveTo(nextState()); } } @@ -143,6 +141,7 @@ public void quickSpin() } } + // TODO Either low key get rid of this or implement wait(short thread.sleep should be fine?) there is no wait in between so color sensing doesn't work public void moveInOrder(int[] arr) { Thread moveThread = new Thread(() -> { for(int i : arr){ @@ -157,26 +156,37 @@ public void moveInOrder(int[] arr) { public void moveTo(IndexerState newState) { shiftArtifacts(state, newState); - double newAngle = (stateToNum(newState) - 1) * 120; // outtake angle calculation + double oldAngle = lastAngle; + double newAngle; if(intaking) { - newAngle = 360%((stateToNum(newState)-1)*120+180); // intake angle calculation + newAngle = ((stateToNum(newState)-1)*120+60)%360; // intake angle calculation indexerServo.turnToAngle(newAngle); } else { + newAngle = (stateToNum(newState) - 1) * 120; // outtake angle calculation indexerServo.turnToAngle(newAngle); } + + double angleDelta = Math.abs(newAngle - oldAngle); + if (angleDelta > 180) angleDelta = 360 - angleDelta; // shortest path + + double waitTime = Math.min(maxWait, Math.max(minWait, angleDelta * msPerDegree)); + + scanTimer.reset(); + scanDelay = waitTime; + scanPending = true; + + lastAngle = newAngle; + state = newState; - while (indexerServo.getAngle() > newAngle - 5 && indexerServo.getAngle() < newAngle + 5) { - try { - // Simulate the blocking operation of the servo - Thread.sleep(50); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - } - scanArtifact(); } + public void updateColorScanning() { + if (scanPending && scanTimer.milliseconds() >= scanDelay) { + scanArtifact(); + scanPending = false; + } + } public IndexerState numToState(int num) { @@ -226,4 +236,12 @@ public IndexerState closestZero() } return state; } + + public IndexerState getState() { + return state; + } + + public boolean isBusy() { + return scanPending; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index 7da596ad4e9e..08d5a6f6ca64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -20,6 +20,7 @@ public class SpindexerTester extends LinearOpMode { GamepadEx gp2; @Override + // Make sure to check if busy every time you do an indexer move public void runOpMode(){ // colorSensor = hardwareMap.get(ColorSensor.class, "color"); indexer = new Indexer(hardwareMap); @@ -27,13 +28,28 @@ public void runOpMode(){ gp2 = new GamepadEx(gamepad2); waitForStart(); + indexer.startIntake(); while(opModeIsActive()){ + gp2.readButtons(); + telemetry.addData("CurrentState: ", indexer.getState()); + telemetry.addData("NextState: ", indexer.nextState()); + if(gp2.wasJustPressed(GamepadKeys.Button.DPAD_UP)){ - indexer.moveTo(indexer.nextState()); + if (!indexer.isBusy()) { + indexer.moveTo(indexer.nextState()); + } } if(gp2.wasJustPressed(GamepadKeys.Button.A)) { - indexer.setIntaking(!indexer.getIntaking()); + if (!indexer.isBusy()) { + indexer.setIntaking(true); + } + } + if(gp2.wasJustPressed(GamepadKeys.Button.B)) + { + if (!indexer.isBusy()) { + indexer.setIntaking(false); + } } if(gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01){ intake.run(true); @@ -53,6 +69,7 @@ public void runOpMode(){ //String mainColor = getMainColor(r, g, b); //telemetry.addData("Color Detected overall:",mainColor); + indexer.updateColorScanning(); telemetry.update(); } } From df878236651e7a7ca5a04a519076071abcb88094 Mon Sep 17 00:00:00 2001 From: tango8 Date: Fri, 24 Oct 2025 15:46:37 -0400 Subject: [PATCH 17/67] fixing gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 6dd7fd3483ce..118d5e459d52 100644 --- a/build.gradle +++ b/build.gradle @@ -11,7 +11,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:8.13.0' + classpath 'com.android.tools.build:gradle:8.8.0' } } From ba929f6fd8cedd66bc914712259481b7419414cc Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Mon, 27 Oct 2025 16:11:22 -0400 Subject: [PATCH 18/67] camera streaming stuff --- .../ftc/teamcode/testing/AprilTagTester.java | 57 ++++++++++++++++++- 1 file changed, 55 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index ad6ff084aedb..8aa151dd1cc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -5,13 +5,18 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.subsystems.AprilTag; import org.firstinspires.ftc.teamcode.subsystems.AprilTagAimer; import org.firstinspires.ftc.teamcode.subsystems.Movement; +import org.opencv.core.Mat; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.*; + @TeleOp(name = "AprilTagTester", group = "AA_main") public class AprilTagTester extends LinearOpMode { - + OpenCvCamera camera; @Override public void runOpMode() throws InterruptedException { AprilTag aprilTag = new AprilTag(hardwareMap); @@ -71,5 +76,53 @@ public void runOpMode() throws InterruptedException { telemetry.update(); } } + //vibe code stuff for the camera stream + // Get webcam from config + WebcamName webcamName = hardwareMap.get(WebcamName.class, "webcam"); + + // Get the viewport ID to display on Driver Hub / Dashboard + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "webcam", "id", hardwareMap.appContext.getPackageName()); + + // Create a webcam instance with live preview ID + camera = OpenCvCameraFactory.getInstance().createWebcam(webcamName, cameraMonitorViewId); + + // Simple pipeline that just returns the camera feed unchanged + camera.setPipeline(new OpenCvPipeline() { + @Override + public Mat processFrame(Mat input) { + Imgproc.putText(input, "Preview Active", new org.opencv.core.Point(10, 30), + Imgproc.FONT_HERSHEY_SIMPLEX, 1, new org.opencv.core.Scalar(255, 255, 255), 2); + return input; + } + }); + + // Open camera asynchronously + camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + camera.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + telemetry.addData("Camera Error: ", errorCode); + telemetry.update(); + } + }); + + telemetry.addLine("Press INIT, then open Camera Stream from menu"); + telemetry.update(); + + waitForStart(); + + // Stream continues while OpMode runs + while (opModeIsActive()) { + telemetry.addData("Status", "Streaming..."); + telemetry.update(); + sleep(50); + } + + camera.stopStreaming(); + } } -} \ No newline at end of file From 859b7a4e36e02b4ea3f7094324732fdcdeaa7ebb Mon Sep 17 00:00:00 2001 From: tango8 Date: Tue, 4 Nov 2025 16:16:51 -0500 Subject: [PATCH 19/67] added telemetry --- .../org/firstinspires/ftc/teamcode/testing/AprilTagTester.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index 8f85c8d20455..1bc50e1343dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -64,6 +64,7 @@ public void runOpMode() throws InterruptedException { continuousAprilTagLock = true; telemetry.addData("Continuously locked in on", "apriltag"); + telemetry.addData("Last detected tag ID", aprilTag.getCurrentId()); telemetry.addData("Goal tag bearing", aprilTag.getBearing()); telemetry.addData("Goal tag elevation", aprilTag.getElevation()); telemetry.addData("Goal tag range", aprilTag.getRange()); From c87b80ea7d7fae87816f76c389d8dd77850191d2 Mon Sep 17 00:00:00 2001 From: tango8 Date: Tue, 4 Nov 2025 16:17:43 -0500 Subject: [PATCH 20/67] camera calibrations to hopefully fix camera to get apriltag pose --- .../ftc/teamcode/subsystems/AprilTag.java | 32 +++++++++++++++++-- .../main/res/xml/teamwebcamcalibrations.xml | 4 +-- 2 files changed, 31 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java index 58922ccc70e9..3c0c0b0a6240 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java @@ -2,13 +2,23 @@ import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.android.util.Size; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibrationIdentity; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.VendorProductCalibrationIdentity; +import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibrationManager; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.xmlpull.v1.XmlPullParser; +import java.util.ArrayList; import java.util.List; // TODO IMPORTANT NOTES: For goalTagID, just have separate teleops one for red alliance one for blue where blue teleop can setGoalTagID(20) and red teleop can setGoalTagID(24) @@ -17,6 +27,7 @@ public class AprilTag { private int id; private int obeliskId; private int goalTagID; // our current alliance goal + private int cameraScannedId; private double bearing; private double elevation; private double range; @@ -25,9 +36,25 @@ public class AprilTag { public AprilTag(HardwareMap hardwareMap) { AprilTagLibrary library = AprilTagGameDatabase.getCurrentGameTagLibrary(); + + XmlPullParser parser = hardwareMap.appContext.getResources().getXml(R.xml.teamwebcamcalibrations); + List parsers = new ArrayList<>(); + parsers.add(parser); + + CameraCalibrationManager camCalManager = new CameraCalibrationManager(parsers); + + CameraCalibrationIdentity identity = new VendorProductCalibrationIdentity(0x046D,0x0825); + + CameraCalibration camCal = camCalManager.getCalibration( + null, new Size(640, 480) + ); + processor = new AprilTagProcessor.Builder() .setTagLibrary(library) + .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + .setLensIntrinsics(camCal.focalLengthY, camCal.focalLengthY, camCal.principalPointX, camCal.principalPointY) .build(); + WebcamName webcamname = hardwareMap.get(WebcamName.class, "webcam"); portal = VisionPortal.easyCreateWithDefaults(webcamname, processor); } @@ -56,11 +83,9 @@ public void scanGoalTag() { // If camera is facing to the right of the center of the cam (if it needs to move to the left) the bearing is positive. List detectionList = processor.getDetections(); for (AprilTagDetection detection : detectionList) { + cameraScannedId = detection.id; // goalTagID should be gotten before round/during auto if (detection.id == goalTagID && detection.ftcPose != null) { - range = detection.ftcPose.range; // distance from camera to center of tag - bearing = detection.ftcPose.bearing; // the angle (left/right) the camera must turn to directly point at the tag center - elevation = detection.ftcPose.elevation; // the angle (up/down) the camera must turn to directly point at the tag center id = detection.id; break; } @@ -70,6 +95,7 @@ public void scanGoalTag() { public void setGoalTagID(int allianceTagID) { goalTagID = allianceTagID; } + public int getCurrentId() { return cameraScannedId;} public int getObeliskId(){ return obeliskId; } diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml index 22ae7a86ba33..3972936b26cb 100644 --- a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml +++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml @@ -147,14 +147,14 @@ - + From c06baa83e0270b59f37b3eb90143f95d2de8eea5 Mon Sep 17 00:00:00 2001 From: tango8 Date: Wed, 5 Nov 2025 17:30:27 -0500 Subject: [PATCH 21/67] auto aim kinda works, sdk update --- .../external/samples/SensorOctoQuad.java | 141 ------------------ .../external/samples/SensorOctoQuadAdv.java | 1 - TeamCode/.project | 28 ++++ .../org.eclipse.buildship.core.prefs | 13 ++ .../ftc/teamcode/subsystems/AprilTag.java | 31 +--- .../teamcode/subsystems/AprilTagAimer.java | 8 +- .../ftc/teamcode/subsystems/Movement.java | 6 +- .../ftc/teamcode/testing/AprilTagTester.java | 17 +-- .../main/res/xml/teamwebcamcalibrations.xml | 2 +- build.dependencies.gradle | 16 +- 10 files changed, 69 insertions(+), 194 deletions(-) delete mode 100644 FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java create mode 100644 TeamCode/.project create mode 100644 TeamCode/.settings/org.eclipse.buildship.core.prefs diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java deleted file mode 100644 index f797c6b0dc72..000000000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (c) 2024 DigitalChickenLabs - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/* - * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module - * - * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs. - * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder. - * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, - * as can several sonar rangefinders such as the MaxBotix MB1000 series. - * - * This basic sample shows how an OctoQuad can be used to read the position three Odometry pods fitted - * with REV Thru-Bore encoders. For a more advanced example showing additional OctoQuad capabilities, see the SensorOctoQuadAdv sample. - * - * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. - * - * The code assumes the first three OctoQuad inputs are connected as follows - * - Chan 0: for measuring forward motion on the left side of the robot. - * - Chan 1: for measuring forward motion on the right side of the robot. - * - Chan 2: for measuring Lateral (strafing) motion. - * - * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1. - * - * This sample does not show how to interpret these readings, just how to obtain and display them. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list - * - * See the sensor's product page: https://www.tindie.com/products/35114/ - */ -@TeleOp(name = "OctoQuad Basic", group="OctoQuad") -@Disabled -public class SensorOctoQuad extends LinearOpMode { - - // Identify which encoder OctoQuad inputs are connected to each odometry pod. - private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion) - private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion) - private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion) - - // Declare the OctoQuad object and members to store encoder positions and velocities - private OctoQuad octoquad; - - private int posLeft; - private int posRight; - private int posPerp; - - /** - * This function is executed when this OpMode is selected from the Driver Station. - */ - @Override - public void runOpMode() { - - // Connect to OctoQuad by referring to its name in the Robot Configuration. - octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); - - // Read the Firmware Revision number from the OctoQuad and display it as telemetry. - telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); - - // Reverse the count-direction of any encoder that is not what you require. - // e.g. if you push the robot forward and the left encoder counts down, then reverse it so it counts up. - octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); - octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); - octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); - - // Any changes that are made should be saved in FLASH just in case there is a sensor power glitch. - octoquad.saveParametersToFlash(); - - telemetry.addLine("\nPress START to read encoder values"); - telemetry.update(); - - waitForStart(); - - // Configure the telemetry for optimal display of data. - telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); - telemetry.setMsTransmissionInterval(50); - - // Set all the encoder inputs to zero. - octoquad.resetAllPositions(); - - // Loop while displaying the odometry pod positions. - while (opModeIsActive()) { - telemetry.addData(">", "Press X to Reset Encoders\n"); - - // Check for X button to reset encoders. - if (gamepad1.x) { - // Reset the position of all encoders to zero. - octoquad.resetAllPositions(); - } - - // Read all the encoder data. Load into local members. - readOdometryPods(); - - // Display the values. - telemetry.addData("Left ", "%8d counts", posLeft); - telemetry.addData("Right", "%8d counts", posRight); - telemetry.addData("Perp ", "%8d counts", posPerp); - telemetry.update(); - } - } - - private void readOdometryPods() { - // For best performance, we should only perform ONE transaction with the OctoQuad each cycle. - // Since this example only needs to read positions from a few channels, we could use either - // readPositionRange(idxFirst, idxLast) to get a select number of sequential channels - // or - // readAllPositions() to get all 8 encoder readings - // - // Since both calls take almost the same amount of time, and the actual channels may not end up - // being sequential, we will read all of the encoder positions, and then pick out the ones we need. - int[] positions = octoquad.readAllPositions(); - posLeft = positions[ODO_LEFT]; - posRight = positions[ODO_RIGHT]; - posPerp = positions[ODO_PERP]; - } -} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java index e763b9aac2da..2e807ce0ab7f 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java @@ -22,7 +22,6 @@ package org.firstinspires.ftc.robotcontroller.external.samples; import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; -import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; diff --git a/TeamCode/.project b/TeamCode/.project new file mode 100644 index 000000000000..81437c6517d8 --- /dev/null +++ b/TeamCode/.project @@ -0,0 +1,28 @@ + + + TeamCode + Project TeamCode created by Buildship. + + + + + org.eclipse.buildship.core.gradleprojectbuilder + + + + + + org.eclipse.buildship.core.gradleprojectnature + + + + 1759929503127 + + 30 + + org.eclipse.core.resources.regexFilterMatcher + node_modules|\.git|__CREATED_BY_JAVA_LANGUAGE_SERVER__ + + + + diff --git a/TeamCode/.settings/org.eclipse.buildship.core.prefs b/TeamCode/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000000..824d16fef8d7 --- /dev/null +++ b/TeamCode/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,13 @@ +arguments=--init-script /home/etangos/.cache/nvim/jdtls/TeamCode/config/org.eclipse.osgi/58/0/.cp/gradle/init/init.gradle +auto.sync=false +build.scans.enabled=false +connection.gradle.distribution=GRADLE_DISTRIBUTION(VERSION(8.9)) +connection.project.dir= +eclipse.preferences.version=1 +gradle.user.home= +java.home=/usr/lib/jvm/java-24-openjdk +jvm.arguments= +offline.mode=false +override.workspace.settings=true +show.console.view=true +show.executions.view=true diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java index 3c0c0b0a6240..730defc100be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java @@ -2,23 +2,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.android.util.Size; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibrationIdentity; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.VendorProductCalibrationIdentity; -import org.firstinspires.ftc.teamcode.R; import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibrationManager; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.xmlpull.v1.XmlPullParser; - -import java.util.ArrayList; import java.util.List; // TODO IMPORTANT NOTES: For goalTagID, just have separate teleops one for red alliance one for blue where blue teleop can setGoalTagID(20) and red teleop can setGoalTagID(24) @@ -37,22 +26,8 @@ public class AprilTag { public AprilTag(HardwareMap hardwareMap) { AprilTagLibrary library = AprilTagGameDatabase.getCurrentGameTagLibrary(); - XmlPullParser parser = hardwareMap.appContext.getResources().getXml(R.xml.teamwebcamcalibrations); - List parsers = new ArrayList<>(); - parsers.add(parser); - - CameraCalibrationManager camCalManager = new CameraCalibrationManager(parsers); - - CameraCalibrationIdentity identity = new VendorProductCalibrationIdentity(0x046D,0x0825); - - CameraCalibration camCal = camCalManager.getCalibration( - null, new Size(640, 480) - ); - processor = new AprilTagProcessor.Builder() .setTagLibrary(library) - .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) - .setLensIntrinsics(camCal.focalLengthY, camCal.focalLengthY, camCal.principalPointX, camCal.principalPointY) .build(); WebcamName webcamname = hardwareMap.get(WebcamName.class, "webcam"); @@ -87,6 +62,9 @@ public void scanGoalTag() { // goalTagID should be gotten before round/during auto if (detection.id == goalTagID && detection.ftcPose != null) { id = detection.id; + bearing = detection.ftcPose.bearing; + elevation = detection.ftcPose.elevation; + range = detection.ftcPose.range; break; } } @@ -108,4 +86,7 @@ public double getRange(){ public double getBearing(){ return bearing; } + public void setCurrentCameraScannedId(int i) { + cameraScannedId = i; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index f997c4519a1d..26576a35bd5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -2,14 +2,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; -import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; - public class AprilTagAimer { - private final double kP = 0.01; + private final double kP = 0.03; private final double kI = 0.001; - private final double kD = 0.005; + private final double kD = 0.0025; private final double kF = 0.05; private final double maxIntegral = 1.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java index 15cc076e08c8..10cf4ca0a612 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java @@ -18,7 +18,7 @@ public class Movement { private final DcMotor leftFront, leftBack, rightFront, rightBack; private final IMU imu; - private final double STRAFE_MULTIPLIER = 0.6, ROTATION_MULTIPLIER = 0.5; + private final double STRAFE_MULTIPLIER = 1.0, ROTATION_MULTIPLIER = 0.8; /** * Initializes a Movement instance. @@ -51,7 +51,7 @@ public Movement(@NonNull HardwareMap map){ public void teleopTick(double leftStickX, double leftStickY, double rightStickX, double turnCorrection){ double axial = -leftStickY * STRAFE_MULTIPLIER; double lateral = -leftStickX * STRAFE_MULTIPLIER; - double yaw = -(rightStickX * ROTATION_MULTIPLIER) + turnCorrection; + double yaw = -(rightStickX * ROTATION_MULTIPLIER + turnCorrection); double leftFrontPower = axial + lateral + yaw; double rightFrontPower = axial - lateral - yaw; @@ -76,7 +76,7 @@ public void teleopTick(double leftStickX, double leftStickY, double rightStickX, public void teleopTickFieldCentric(double leftStickX, double leftStickY, double rightStickX, double turnCorrection, boolean start){ double axial = -leftStickY * STRAFE_MULTIPLIER; double lateral = -leftStickX * STRAFE_MULTIPLIER; - double yaw = -(rightStickX * ROTATION_MULTIPLIER) + turnCorrection; + double yaw = -(rightStickX * ROTATION_MULTIPLIER + turnCorrection); // This button choice was made so that it is hard to hit on accident, // it can be freely changed based on preference. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index 1bc50e1343dc..f08de582825f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -5,13 +5,10 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.subsystems.AprilTag; import org.firstinspires.ftc.teamcode.subsystems.AprilTagAimer; import org.firstinspires.ftc.teamcode.subsystems.Movement; -import org.opencv.core.Mat; -import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.*; @TeleOp(name = "AprilTagTester", group = "AA_main") @@ -47,6 +44,12 @@ public void runOpMode() throws InterruptedException { } else { turnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); } + telemetry.addData("Continuously locked in on", "apriltag"); + telemetry.addData("Last detected tag ID", aprilTag.getCurrentId()); + telemetry.addData("Goal tag bearing", aprilTag.getBearing()); + telemetry.addData("Goal tag elevation", aprilTag.getElevation()); + telemetry.addData("Goal tag range", aprilTag.getRange()); + aprilTag.setCurrentCameraScannedId(0); } else { turnCorrection = 0; @@ -62,12 +65,6 @@ public void runOpMode() throws InterruptedException { if (gamePadTwo.wasJustPressed(GamepadKeys.Button.A)) { continuousAprilTagLock = true; - - telemetry.addData("Continuously locked in on", "apriltag"); - telemetry.addData("Last detected tag ID", aprilTag.getCurrentId()); - telemetry.addData("Goal tag bearing", aprilTag.getBearing()); - telemetry.addData("Goal tag elevation", aprilTag.getElevation()); - telemetry.addData("Goal tag range", aprilTag.getRange()); } if (gamePadTwo.wasJustPressed(GamepadKeys.Button.B)) { @@ -87,6 +84,7 @@ public void runOpMode() throws InterruptedException { telemetry.update(); } + /* //vibe code stuff for the camera stream // Get webcam from config WebcamName webcamName = hardwareMap.get(WebcamName.class, "webcam"); @@ -135,6 +133,7 @@ public void onError(int errorCode) { } camera.stopStreaming(); + */ } } diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml index 3972936b26cb..eb71d94f7d3b 100644 --- a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml +++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml @@ -147,7 +147,7 @@ - + Date: Wed, 5 Nov 2025 18:33:13 -0500 Subject: [PATCH 22/67] correction calculations in apriltagtester teleop updates only set time, optimizations to hopefully get cleaner autoaim --- .../teamcode/subsystems/AprilTagAimer.java | 31 ++++++++----- .../ftc/teamcode/testing/AprilTagTester.java | 44 +++++++++++++------ 2 files changed, 51 insertions(+), 24 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java index 26576a35bd5c..8359924eebbb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagAimer.java @@ -3,13 +3,14 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class AprilTagAimer { - private final double kP = 0.03; - private final double kI = 0.001; - private final double kD = 0.0025; - private final double kF = 0.05; + private final double kP = 0.025; + private final double kI = 0.0005; + private final double kD = 0.002; + private final double kF = 0.04; + private final double alpha = 0.8; // smoothing factor (0 = no filtering, 1 = very heavy smoothing) private final double maxIntegral = 1.0; - private double integral = 0; + private double lastDerivative = 0.0; private double lastError = 0; private long lastTimestamp = 0; @@ -31,31 +32,41 @@ public double calculateTurnPowerToBearing(double bearing) { if (Double.isNaN(bearing)) { integral = 0; lastError = 0; + lastDerivative = 0; lastTimestamp = 0; return 0.0; } double error = -angleWrapDegrees(bearing); + if (Math.abs(error) < 1.0) { + integral = 0; + lastError = error; + lastDerivative = 0; + return 0; + } // Time diff in seconds long currentTime = System.currentTimeMillis(); - double deltaTime = (currentTime - lastTimestamp) / 1000.0; + double deltaTime; if (lastTimestamp == 0) { - deltaTime = 0.01; // assume 10ms for first call + deltaTime = 0.01; // assume 10 ms on first loop } else { deltaTime = (currentTime - lastTimestamp) / 1000.0; } - lastTimestamp = currentTime; + // Don't consider > 100 ms or < 5ms + deltaTime = Math.max(Math.min(deltaTime, 0.1), .005); // Integral accumulation integral += error * deltaTime; integral = Math.max(-maxIntegral, Math.min(maxIntegral, integral)); - // Derivative term - double derivative = (error - lastError) / deltaTime; + // Derivative with smoothing + double rawDerivative = (error - lastError) / deltaTime; + double derivative = alpha * lastDerivative + (1 - alpha) * rawDerivative; + lastDerivative = derivative; lastError = error; // Gets proper direction diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index f08de582825f..df72a2a0ddb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -14,6 +14,10 @@ @TeleOp(name = "AprilTagTester", group = "AA_main") public class AprilTagTester extends LinearOpMode { OpenCvCamera camera; + private long lastAimUpdateTime = 0; + private double lastTurnCorrection = 0; + private static final long AIM_UPDATE_INTERVAL_MS = 50; // update every 50 ms (~20 Hz) + @Override public void runOpMode() throws InterruptedException { AprilTag aprilTag = new AprilTag(hardwareMap); @@ -37,21 +41,25 @@ public void runOpMode() throws InterruptedException { double turnCorrection; if (continuousAprilTagLock) { - aprilTag.scanGoalTag(); - double bearing = aprilTag.getBearing(); - if (Double.isNaN(bearing)) { - turnCorrection = 0; - } else { - turnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); + long currentTime = System.currentTimeMillis(); + + // Run scan + PID only every AIM_UPDATE_INTERVAL_MS + if (currentTime - lastAimUpdateTime >= AIM_UPDATE_INTERVAL_MS) { + lastAimUpdateTime = currentTime; + + aprilTag.scanGoalTag(); + double bearing = aprilTag.getBearing(); + + if (Double.isNaN(bearing)) { + lastTurnCorrection = 0; + } else { + lastTurnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); + } } - telemetry.addData("Continuously locked in on", "apriltag"); - telemetry.addData("Last detected tag ID", aprilTag.getCurrentId()); - telemetry.addData("Goal tag bearing", aprilTag.getBearing()); - telemetry.addData("Goal tag elevation", aprilTag.getElevation()); - telemetry.addData("Goal tag range", aprilTag.getRange()); - aprilTag.setCurrentCameraScannedId(0); - } - else { + + // Use the last computed correction between updates, but slowly decay it + turnCorrection = 0.9 * lastTurnCorrection; + } else { turnCorrection = 0; } @@ -65,6 +73,14 @@ public void runOpMode() throws InterruptedException { if (gamePadTwo.wasJustPressed(GamepadKeys.Button.A)) { continuousAprilTagLock = true; + + telemetry.addData("Button A to update", "telemetry"); + telemetry.addData("Continuously locked in on", "apriltag"); + telemetry.addData("Last detected tag ID", aprilTag.getCurrentId()); + telemetry.addData("Goal tag bearing", aprilTag.getBearing()); + telemetry.addData("Goal tag elevation", aprilTag.getElevation()); + telemetry.addData("Goal tag range", aprilTag.getRange()); + aprilTag.setCurrentCameraScannedId(0); } if (gamePadTwo.wasJustPressed(GamepadKeys.Button.B)) { From ce287c7240604fca8b461032df906d1b2ae5c37b Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Fri, 7 Nov 2025 16:05:11 -0500 Subject: [PATCH 23/67] added voltage tester thingy --- .../ftc/teamcode/testing/VoltageTester.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java new file mode 100644 index 000000000000..9e8716603e89 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.testing; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.HardwareMap; + +@TeleOp(name = "Voltage Tester") +public class VoltageTester extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + AnalogInput signal = hardwareMap.get(AnalogInput.class, "Encoder"); + waitForStart(); + while (opModeIsActive()) + { + telemetry.addData("voltage", signal.getVoltage()); + telemetry.update(); + } + } +} From 97f38ab983091a80b45907a41434e65eadab4c96 Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Sat, 8 Nov 2025 17:13:40 -0500 Subject: [PATCH 24/67] rewrote indexer to work with CR Servo and added custom CRServo Position control class to help us move to certian positons using CR Servo the control class is PIDF vibe code so dont expect it to work first time --- .../ftc/teamcode/subsystems/CRServoPositionControl.java | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java new file mode 100644 index 000000000000..35d16a786ea4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +public class CRServoPositionControl { +} From 336d6ebca59fa66c3de45f70ba7a274a9a0d55e4 Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Sat, 8 Nov 2025 17:17:38 -0500 Subject: [PATCH 25/67] rewrote indexer to work with CR Servo and added custom CRServo Position control class to help us move to certian positons using CR Servo the control class is PIDF vibe code so dont expect it to work first time --- .../subsystems/CRServoPositionControl.java | 51 ++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 35d16a786ea4..084d64558c4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -1,4 +1,53 @@ package org.firstinspires.ftc.teamcode.subsystems; - +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.util.ElapsedTime; +// TODO: this is vibe coded so probably needs testing and adjustments public class CRServoPositionControl { + private CRServo crServo; + private double kp = 1.0; // Proportional gain + private double ki = 0.0; // Integral gain + private double kd = 0.1; // Derivative gain + private double kf = 0.1; // Feedforward gain + + private double integral = 0.0; + private double lastError = 0.0; + private ElapsedTime timer = new ElapsedTime(); + + public CRServoPositionControl(CRServo servo) { + this.crServo = servo; + timer.reset(); + } + + // Read encoder voltage from hardware (0 to 3.3V) + + + // Convert angle (0 to 360 degrees) to voltage (0 to 3.3V) + private double angleToVoltage(double angleDegrees) { + angleDegrees = Math.max(0, Math.min(360, angleDegrees)); // Clamp angle + return (angleDegrees / 360.0) * 3.3; + } + + // Move to target angle in degrees using PIDF control + public void moveToAngle(double targetAngleDegrees) { + double targetVoltage = angleToVoltage(targetAngleDegrees); + double currentVoltage = crServo.getPower(); + + double error = targetVoltage - currentVoltage; + + double deltaTime = timer.seconds(); + timer.reset(); + + integral += error * deltaTime; + double derivative = (error - lastError) / deltaTime; + + // PIDF output for power command [-1, 1] + double output = kp * error + ki * integral + kd * derivative + kf * targetVoltage; + output = Math.max(-1.0, Math.min(1.0, output)); + + crServo.setPower(output); + + lastError = error; + } + + } From 0e27c508a855f40fd8136e2fcfe0c1e2f35a993b Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Sat, 8 Nov 2025 17:19:14 -0500 Subject: [PATCH 26/67] rewrote indexer to work with CR Servo and added custom CRServo Position control class to help us move to certian positons using CR Servo the control class is PIDF vibe code so dont expect it to work first time and yes this is my third commit and push cuz i forgot to git add twice --- .../ftc/teamcode/subsystems/Indexer.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 457a4324887c..a6fc61751f19 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -3,6 +3,7 @@ import com.arcrobotics.ftclib.hardware.SimpleServo; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.hardware.CRServo; import java.util.concurrent.ConcurrentLinkedQueue; @@ -32,7 +33,7 @@ public enum IndexerState three, oneAlt } - private final SimpleServo indexerServo; + private final CRServo indexerServo; private final Actuator actuator; // Color sensor timing stuff @@ -43,10 +44,12 @@ public enum IndexerState private final double minWait = 100; private final double maxWait = 300; private double lastAngle = 0; + CRServoPositionControl crServoPositionControl; public Indexer (HardwareMap hardwareMap) { state = IndexerState.one; - indexerServo = new SimpleServo(hardwareMap, "index",0,360); + indexerServo = hardwareMap.get(CRServo.class,"indexer"); + crServoPositionControl = new CRServoPositionControl(indexerServo); colorSensor = new ColorSensorSystem(hardwareMap); actuator = new Actuator(hardwareMap); } @@ -161,10 +164,10 @@ public void moveTo(IndexerState newState) if(intaking) { newAngle = ((stateToNum(newState)-1)*120+60)%360; // intake angle calculation - indexerServo.turnToAngle(newAngle); + crServoPositionControl.moveToAngle(newAngle); } else { newAngle = (stateToNum(newState) - 1) * 120; // outtake angle calculation - indexerServo.turnToAngle(newAngle); + crServoPositionControl.moveToAngle(newAngle); } double angleDelta = Math.abs(newAngle - oldAngle); From cd8844e741a6ca9a35afeebe9102b3f61f2b16f7 Mon Sep 17 00:00:00 2001 From: tango8 Date: Mon, 10 Nov 2025 16:43:53 -0500 Subject: [PATCH 27/67] small changes --- .../ftc/teamcode/testing/SpindexerTester.java | 36 ++++++++++++------- 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index 08d5a6f6ca64..3cdcf58eddfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -9,12 +9,14 @@ import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.OpticalDistanceSensor; +import org.firstinspires.ftc.teamcode.subsystems.Actuator; import org.firstinspires.ftc.teamcode.subsystems.Indexer; import org.firstinspires.ftc.teamcode.subsystems.Intake; @TeleOp(name = "SpindexerTest", group = "Teleop") public class SpindexerTester extends LinearOpMode { - //ColorSensor colorSensor; + ColorSensor colorSensor; + Actuator actuator; Indexer indexer; Intake intake; GamepadEx gp2; @@ -22,8 +24,9 @@ public class SpindexerTester extends LinearOpMode { @Override // Make sure to check if busy every time you do an indexer move public void runOpMode(){ - // colorSensor = hardwareMap.get(ColorSensor.class, "color"); + colorSensor = hardwareMap.get(ColorSensor.class, "color"); indexer = new Indexer(hardwareMap); + actuator = new Actuator(hardwareMap); intake = new Intake(hardwareMap); gp2 = new GamepadEx(gamepad2); @@ -51,23 +54,32 @@ public void runOpMode(){ indexer.setIntaking(false); } } - if(gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01){ + if(gp2.getButton(GamepadKeys.Button.X)) { + actuator.set(true); + } + else if(gp2.getButton(GamepadKeys.Button.Y)) { + actuator.set(false); + } + + if(gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01) + { intake.run(true); } else { intake.run(false); } - //telemetry.addData("Light Detected:",((OpticalDistanceSensor) colorSensor).getLightDetected()); - //telemetry.addData("Red", colorSensor.red()); - //telemetry.addData("Green", colorSensor.green()); - //telemetry.addData("Blue", colorSensor.blue()); - //int r = colorSensor.red(); - //int g = colorSensor.green(); - //int b = colorSensor.blue(); + telemetry.addData("Light Detected:",((OpticalDistanceSensor) colorSensor).getLightDetected()); + telemetry.addData("Red", colorSensor.red()); + telemetry.addData("Green", colorSensor.green()); + telemetry.addData("Blue", colorSensor.blue()); + + int r = colorSensor.red(); + int g = colorSensor.green(); + int b = colorSensor.blue(); - //String mainColor = getMainColor(r, g, b); - //telemetry.addData("Color Detected overall:",mainColor); + String mainColor = getMainColor(r, g, b); + telemetry.addData("Color Detected overall:",mainColor); indexer.updateColorScanning(); telemetry.update(); From bfc19e0d8a82ec7e8f286709c25d9b8b15f0deb3 Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Mon, 10 Nov 2025 18:20:53 -0500 Subject: [PATCH 28/67] 'fixed' crservopositoncontrol issues and aded logic in teleOP --- .../subsystems/CRServoPositionControl.java | 43 +++-- .../ftc/teamcode/subsystems/Indexer.java | 170 ++++++++---------- 2 files changed, 101 insertions(+), 112 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 084d64558c4a..d55e4b4b014b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -1,53 +1,60 @@ package org.firstinspires.ftc.teamcode.subsystems; import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.util.ElapsedTime; -// TODO: this is vibe coded so probably needs testing and adjustments + public class CRServoPositionControl { - private CRServo crServo; - private double kp = 1.0; // Proportional gain - private double ki = 0.0; // Integral gain - private double kd = 0.1; // Derivative gain - private double kf = 0.1; // Feedforward gain + private final CRServo crServo; + private final AnalogInput encoder; // Analog input for position from 4th wire + + private double kp = 1.0; + private double ki = 0.0; + private double kd = 0.1; + private double kf = 0.1; private double integral = 0.0; private double lastError = 0.0; private ElapsedTime timer = new ElapsedTime(); - public CRServoPositionControl(CRServo servo) { + public CRServoPositionControl(CRServo servo, AnalogInput encoder) { this.crServo = servo; + this.encoder = encoder; timer.reset(); } - // Read encoder voltage from hardware (0 to 3.3V) - + private double getCurrentPositionVoltage() { + return encoder.getVoltage(); // Read analog voltage from 4th wire + } - // Convert angle (0 to 360 degrees) to voltage (0 to 3.3V) private double angleToVoltage(double angleDegrees) { - angleDegrees = Math.max(0, Math.min(360, angleDegrees)); // Clamp angle + angleDegrees = Math.max(0, Math.min(360, angleDegrees)); // Clamp return (angleDegrees / 360.0) * 3.3; } - // Move to target angle in degrees using PIDF control + private double voltageToAngle(double voltage) { + voltage = Math.max(0, Math.min(3.3, voltage)); + return (voltage / 3.3) * 360.0; + } + public void moveToAngle(double targetAngleDegrees) { double targetVoltage = angleToVoltage(targetAngleDegrees); - double currentVoltage = crServo.getPower(); + double currentVoltage = getCurrentPositionVoltage(); double error = targetVoltage - currentVoltage; + // Shortest path wrap handling, for continuous rotation (optional) + if (error > 1.65) { error -= 3.3; } + if (error < -1.65) { error += 3.3; } + double deltaTime = timer.seconds(); timer.reset(); - integral += error * deltaTime; double derivative = (error - lastError) / deltaTime; - // PIDF output for power command [-1, 1] double output = kp * error + ki * integral + kd * derivative + kf * targetVoltage; output = Math.max(-1.0, Math.min(1.0, output)); - crServo.setPower(output); lastError = error; } - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index a6fc61751f19..22de5a8750bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -1,23 +1,41 @@ package org.firstinspires.ftc.teamcode.subsystems; -import com.arcrobotics.ftclib.hardware.SimpleServo; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.hardware.CRServo; - -import java.util.concurrent.ConcurrentLinkedQueue; public class Indexer { private IndexerState state; private boolean intaking = true; private final IndexerState COLOR_SENSOR_POSITION = IndexerState.one; + private ArtifactColor[] artifacts = { ArtifactColor.unknown, ArtifactColor.unknown, ArtifactColor.unknown }; private final ColorSensorSystem colorSensor; + private final CRServoPositionControl indexerServoControl; + private final ElapsedTime scanTimer = new ElapsedTime(); + private boolean scanPending = false; + private double scanDelay; + private final double msPerDegree = 0.6; + private final double minWait = 100; + private final double maxWait = 300; + private double lastAngle = 0; + private double targetAngle = 0; + Actuator actuator; + + public Indexer(HardwareMap hardwareMap) { + state = IndexerState.one; + CRServo indexerServo = hardwareMap.get(CRServo.class, "index"); + AnalogInput indexerAnalog = hardwareMap.get(AnalogInput.class, "indexAnalog"); + actuator = new Actuator(hardwareMap); + indexerServoControl = new CRServoPositionControl(indexerServo, indexerAnalog); + colorSensor = new ColorSensorSystem(hardwareMap); + } public enum ArtifactColor { unknown, @@ -25,34 +43,12 @@ public enum ArtifactColor { green } - public enum IndexerState - { - //i swear these names are temporary we'll do some color coding or sum + public enum IndexerState { one, two, three, oneAlt } - private final CRServo indexerServo; - private final Actuator actuator; - - // Color sensor timing stuff - private final ElapsedTime scanTimer = new ElapsedTime(); - private boolean scanPending = false; - private double scanDelay; - private final double msPerDegree = 0.6; // tune this - private final double minWait = 100; - private final double maxWait = 300; - private double lastAngle = 0; - CRServoPositionControl crServoPositionControl; - public Indexer (HardwareMap hardwareMap) - { - state = IndexerState.one; - indexerServo = hardwareMap.get(CRServo.class,"indexer"); - crServoPositionControl = new CRServoPositionControl(indexerServo); - colorSensor = new ColorSensorSystem(hardwareMap); - actuator = new Actuator(hardwareMap); - } public void setIntaking(boolean isIntaking) { if (this.intaking != isIntaking) { @@ -61,34 +57,27 @@ public void setIntaking(boolean isIntaking) { } } - public void startIntake() - { + public void startIntake() { setIntaking(true); - //intakes from a closer container instead of the one currently at outtake moveTo(nextState()); } - public void startOuttake() - { + public void startOuttake() { setIntaking(false); - //moves to a closer state instead of turning the full 180 degrees moveTo(nextState()); } - public boolean getIntaking() - { + public boolean getIntaking() { return intaking; - } public ArtifactColor stateToColor(IndexerState colorState) { - ArtifactColor color = ArtifactColor.unknown; int stateNum = stateToNum(colorState); if (stateNum == 4) stateNum = 1; stateNum -= 1; - color = artifacts[stateNum]; - return color; + return artifacts[stateNum]; } + public void scanArtifact() { ArtifactColor scannedColor = colorSensor.getColor(); int stateNum = stateToNum(COLOR_SENSOR_POSITION); @@ -96,6 +85,7 @@ public void scanArtifact() { stateNum -= 1; artifacts[stateNum] = scannedColor; } + public void shiftArtifacts(IndexerState oldState, IndexerState newState) { int oldNum = stateToNum(oldState) - 1; if (oldNum == 3) oldNum = 0; @@ -105,96 +95,95 @@ public void shiftArtifacts(IndexerState oldState, IndexerState newState) { int difference = newNum - oldNum; for (int i = 0; i < Math.abs(difference); i++) { if (difference < 0) { - artifacts = new ArtifactColor[]{artifacts[1],artifacts[2],artifacts[0]}; + artifacts = new ArtifactColor[]{artifacts[1], artifacts[2], artifacts[0]}; } else { - artifacts = new ArtifactColor[]{artifacts[2],artifacts[0],artifacts[1]}; + artifacts = new ArtifactColor[]{artifacts[2], artifacts[0], artifacts[1]}; } } - if (actuator.isActivated()) { // && !getIntaking() not ever set to false yet + if (actuator.isActivated()) { int stateNum = stateToNum(state); if (stateNum == 4) stateNum = 1; stateNum -= 1; artifacts[stateNum] = ArtifactColor.unknown; } } + public void moveToColor(ArtifactColor color) { - ArtifactColor checkingColor = stateToColor(IndexerState.one); - if (checkingColor == color) {moveTo(IndexerState.one); return;} - checkingColor = stateToColor(IndexerState.three); - if (checkingColor == color) {moveTo(IndexerState.three); return;} - checkingColor = stateToColor(IndexerState.two); - if (checkingColor == color) {moveTo(IndexerState.two);} - } - public void quickSpin() - { - switch(state) - { + if (stateToColor(IndexerState.one) == color) { + moveTo(IndexerState.one); + return; + } + if (stateToColor(IndexerState.three) == color) { + moveTo(IndexerState.three); + return; + } + if (stateToColor(IndexerState.two) == color) { + moveTo(IndexerState.two); + } + } + + public void quickSpin() { + switch (state) { case one: - moveInOrder(new int[]{1,2,3}); + moveInOrder(new int[]{1, 2, 3}); break; case two: - moveInOrder(new int[]{2,3,1}); + moveInOrder(new int[]{2, 3, 1}); break; case three: - moveInOrder(new int[]{3,2,1}); + moveInOrder(new int[]{3, 2, 1}); break; case oneAlt: - moveInOrder(new int[]{1,3,2}); + moveInOrder(new int[]{1, 3, 2}); break; } } - // TODO Either low key get rid of this or implement wait(short thread.sleep should be fine?) there is no wait in between so color sensing doesn't work public void moveInOrder(int[] arr) { Thread moveThread = new Thread(() -> { - for(int i : arr){ + for (int i : arr) { moveTo(numToState(i)); } }); moveThread.start(); } - //for state 1: will turn to 0 - //for state oneAlt: stateToNum returns 4, so turns to 360 - public void moveTo(IndexerState newState) - { + // Only sets targetAngle and updates state and timing + public void moveTo(IndexerState newState) { shiftArtifacts(state, newState); double oldAngle = lastAngle; - double newAngle; - if(intaking) - { - newAngle = ((stateToNum(newState)-1)*120+60)%360; // intake angle calculation - crServoPositionControl.moveToAngle(newAngle); + if (intaking) { + targetAngle = ((stateToNum(newState) - 1) * 120 + 60) % 360; } else { - newAngle = (stateToNum(newState) - 1) * 120; // outtake angle calculation - crServoPositionControl.moveToAngle(newAngle); + targetAngle = (stateToNum(newState) - 1) * 120; } - - double angleDelta = Math.abs(newAngle - oldAngle); - if (angleDelta > 180) angleDelta = 360 - angleDelta; // shortest path + double angleDelta = Math.abs(targetAngle - oldAngle); + if (angleDelta > 180) angleDelta = 360 - angleDelta; double waitTime = Math.min(maxWait, Math.max(minWait, angleDelta * msPerDegree)); - scanTimer.reset(); scanDelay = waitTime; scanPending = true; - lastAngle = newAngle; - + lastAngle = targetAngle; state = newState; } - public void updateColorScanning() { + // Call this repeatedly in OpMode loop + public void update() { + indexerServoControl.moveToAngle(targetAngle); + + // some stuff the Ai spit out seems right but idk if (scanPending && scanTimer.milliseconds() >= scanDelay) { scanArtifact(); scanPending = false; } + + } - public IndexerState numToState(int num) - { - switch (num) - { + public IndexerState numToState(int num) { + switch (num) { case 1: return closestZero(); case 2: @@ -205,11 +194,8 @@ public IndexerState numToState(int num) return null; } - //returns 4 for oneAlt (useful for turning functions as you can see in comments) - public int stateToNum(IndexerState newState) - { - switch (newState) - { + public int stateToNum(IndexerState newState) { + switch (newState) { case one: return 1; case two: @@ -222,19 +208,15 @@ public int stateToNum(IndexerState newState) return 0; } - //im not gonna bother explaining this because ur lowkey cooked if you dont understand this math - public IndexerState nextState() - { + public IndexerState nextState() { return numToState((stateToNum(state) % 3) + 1); } - //returns the closest 0 state - public IndexerState closestZero() - { - if(state == IndexerState.two) { + public IndexerState closestZero() { + if (state == IndexerState.two) { return IndexerState.one; } - if(state == IndexerState.three) { + if (state == IndexerState.three) { return IndexerState.oneAlt; } return state; From 24e4b836a82b285c95c35ebe7888ba11b74af6a8 Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Mon, 10 Nov 2025 18:21:35 -0500 Subject: [PATCH 29/67] fixed the helper cals thing and added teleop logic --- .../ftc/teamcode/testing/SpindexerTester.java | 72 +++++++------------ 1 file changed, 25 insertions(+), 47 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index 3cdcf58eddfd..97543062e8d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -6,110 +6,88 @@ import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.ColorSensor; -import com.qualcomm.robotcore.hardware.OpticalDistanceSensor; -import org.firstinspires.ftc.teamcode.subsystems.Actuator; import org.firstinspires.ftc.teamcode.subsystems.Indexer; import org.firstinspires.ftc.teamcode.subsystems.Intake; @TeleOp(name = "SpindexerTest", group = "Teleop") public class SpindexerTester extends LinearOpMode { - ColorSensor colorSensor; - Actuator actuator; + Indexer indexer; Intake intake; GamepadEx gp2; @Override - // Make sure to check if busy every time you do an indexer move - public void runOpMode(){ - colorSensor = hardwareMap.get(ColorSensor.class, "color"); + public void runOpMode() { indexer = new Indexer(hardwareMap); - actuator = new Actuator(hardwareMap); intake = new Intake(hardwareMap); gp2 = new GamepadEx(gamepad2); waitForStart(); + indexer.startIntake(); - while(opModeIsActive()){ + + while (opModeIsActive()) { gp2.readButtons(); + telemetry.addData("CurrentState: ", indexer.getState()); telemetry.addData("NextState: ", indexer.nextState()); - if(gp2.wasJustPressed(GamepadKeys.Button.DPAD_UP)){ + if (gp2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { if (!indexer.isBusy()) { indexer.moveTo(indexer.nextState()); } } - if(gp2.wasJustPressed(GamepadKeys.Button.A)) - { + + if (gp2.wasJustPressed(GamepadKeys.Button.A)) { if (!indexer.isBusy()) { indexer.setIntaking(true); } } - if(gp2.wasJustPressed(GamepadKeys.Button.B)) - { + + if (gp2.wasJustPressed(GamepadKeys.Button.B)) { if (!indexer.isBusy()) { indexer.setIntaking(false); } } - if(gp2.getButton(GamepadKeys.Button.X)) { - actuator.set(true); - } - else if(gp2.getButton(GamepadKeys.Button.Y)) { - actuator.set(false); - } - if(gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER)>0.01) - { + if (gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { intake.run(true); - } - else { + } else { intake.run(false); } - telemetry.addData("Light Detected:",((OpticalDistanceSensor) colorSensor).getLightDetected()); - telemetry.addData("Red", colorSensor.red()); - telemetry.addData("Green", colorSensor.green()); - telemetry.addData("Blue", colorSensor.blue()); + // Call update each loop to continuously control the servo position + indexer.update(); - int r = colorSensor.red(); - int g = colorSensor.green(); - int b = colorSensor.blue(); + // Update color scanning timing and sensor reading + //indexer.updateColorScanning(); - String mainColor = getMainColor(r, g, b); - telemetry.addData("Color Detected overall:",mainColor); - - indexer.updateColorScanning(); telemetry.update(); } } @NonNull private static String getMainColor(int r, int g, int b) { - double total = r+g+b; + double total = r + g + b; double rRatio = r / total; double gRatio = g / total; double bRatio = b / total; - String mainColor; - if (rRatio > 0.45 && gRatio < 0.35 && bRatio < 0.35) { - mainColor = "Red"; + return "Red"; } else if (rRatio > 0.45 && gRatio > 0.25 && bRatio < 0.20) { - mainColor = "Orange"; + return "Orange"; } else if (rRatio > 0.38 && gRatio > 0.38 && bRatio < 0.25) { - mainColor = "Yellow"; + return "Yellow"; } else if (gRatio > 0.45 && rRatio < 0.35 && bRatio < 0.35) { - mainColor = "Green"; + return "Green"; } else if (bRatio > 0.45 && rRatio < 0.35 && gRatio < 0.35) { - mainColor = "Blue"; + return "Blue"; } else if (rRatio > 0.35 && bRatio > 0.35 && gRatio < 0.30) { - mainColor = "Purple"; + return "Purple"; } else { - mainColor = "Unclear"; + return "Unclear"; } - return mainColor; } } From 7549e7856b2897f55ba67253ed9231361e2e580d Mon Sep 17 00:00:00 2001 From: tango8 Date: Mon, 10 Nov 2025 18:31:47 -0500 Subject: [PATCH 30/67] small changes --- .../org/firstinspires/ftc/teamcode/subsystems/Movement.java | 5 ++++- .../firstinspires/ftc/teamcode/testing/AprilTagTester.java | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java index 10cf4ca0a612..3f448234a0f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java @@ -38,7 +38,10 @@ public Movement(@NonNull HardwareMap map){ imu.initialize(parameters); - leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + rightBack.setDirection(DcMotorSimple.Direction.REVERSE); + rightFront.setDirection(DcMotorSimple.Direction.REVERSE); + //leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + //leftFront.setDirection(DcMotorSimple.Direction.REVERSE); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index df72a2a0ddb4..962b251fd5e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -51,7 +51,7 @@ public void runOpMode() throws InterruptedException { double bearing = aprilTag.getBearing(); if (Double.isNaN(bearing)) { - lastTurnCorrection = 0; + lastTurnCorrection = 0; } else { lastTurnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); } From a297961f181975e0781615690f905a8fa06d833a Mon Sep 17 00:00:00 2001 From: andrewcodes28 <180860358+andrewcodes28@users.noreply.github.com> Date: Mon, 10 Nov 2025 19:08:15 -0500 Subject: [PATCH 31/67] fix the motors aaaaaaaaaaaaa : --- .../org/firstinspires/ftc/teamcode/subsystems/Movement.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java index 3f448234a0f6..40a77b9e3b3a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Movement.java @@ -38,9 +38,7 @@ public Movement(@NonNull HardwareMap map){ imu.initialize(parameters); - rightBack.setDirection(DcMotorSimple.Direction.REVERSE); - rightFront.setDirection(DcMotorSimple.Direction.REVERSE); - //leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + leftBack.setDirection(DcMotorSimple.Direction.REVERSE); //leftFront.setDirection(DcMotorSimple.Direction.REVERSE); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); From 425fa3ce25f5667b8ffba4b218fdb083dc315aa8 Mon Sep 17 00:00:00 2001 From: tango8 Date: Mon, 10 Nov 2025 19:55:22 -0500 Subject: [PATCH 32/67] field centric on apriltag tester --- .../ftc/teamcode/testing/AprilTagTester.java | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java index 962b251fd5e9..7b142aa25823 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/AprilTagTester.java @@ -16,6 +16,7 @@ public class AprilTagTester extends LinearOpMode { OpenCvCamera camera; private long lastAimUpdateTime = 0; private double lastTurnCorrection = 0; + private boolean fieldCentric = false; private static final long AIM_UPDATE_INTERVAL_MS = 50; // update every 50 ms (~20 Hz) @Override @@ -63,7 +64,19 @@ public void runOpMode() throws InterruptedException { turnCorrection = 0; } - movement.teleopTick(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection); + if (fieldCentric) { + movement.teleopTickFieldCentric(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection, true); + } + else { + movement.teleopTick(gamePadOne.getLeftX(), gamePadOne.getLeftY(), gamePadOne.getRightX(), turnCorrection); + } + + if (gamePadOne.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) { + fieldCentric = true; + } + if (gamePadOne.getButton(GamepadKeys.Button.RIGHT_STICK_BUTTON)) { + fieldCentric = false; + } if (gamePadTwo.wasJustPressed(GamepadKeys.Button.Y)) { aprilTag.scanObeliskTag(); From 72d7605d1d507b83ec169892d40cc6b0dac9312c Mon Sep 17 00:00:00 2001 From: tango8 Date: Wed, 12 Nov 2025 16:19:30 -0500 Subject: [PATCH 33/67] push --- .../ftc/teamcode/subsystems/Indexer.java | 7 ++++- .../testing/ContinuousValueTester.java | 26 +++++++++++++++++++ .../ftc/teamcode/testing/SpindexerTester.java | 1 + .../ftc/teamcode/testing/VoltageTester.java | 2 +- 4 files changed, 34 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 22de5a8750bc..171882cd1dc2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -27,11 +27,12 @@ public class Indexer { private double lastAngle = 0; private double targetAngle = 0; Actuator actuator; + AnalogInput indexerAnalog; public Indexer(HardwareMap hardwareMap) { state = IndexerState.one; CRServo indexerServo = hardwareMap.get(CRServo.class, "index"); - AnalogInput indexerAnalog = hardwareMap.get(AnalogInput.class, "indexAnalog"); + indexerAnalog = hardwareMap.get(AnalogInput.class, "indexAnalog"); actuator = new Actuator(hardwareMap); indexerServoControl = new CRServoPositionControl(indexerServo, indexerAnalog); colorSensor = new ColorSensorSystem(hardwareMap); @@ -222,6 +223,10 @@ public IndexerState closestZero() { return state; } + public double getVoltageAnalog() { + return indexerAnalog.getVoltage(); + } + public IndexerState getState() { return state; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java new file mode 100644 index 000000000000..e5113d56651e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode.testing; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.AnalogInput; + +import org.firstinspires.ftc.teamcode.subsystems.CRServoPositionControl; + + +public class ContinuousValueTester extends LinearOpMode { + AnalogInput indexerAnalog; + + @TeleOp(name = "ContinuousServoTester") + @Overrider + public void runOpMode() throws InterruptedException { + AnalogInput signal = hardwareMap.get(AnalogInput.class, "indexAnalog"); + CRServo crServo = new CRServoPositionControl(indexerServo, indexerAnalog); + waitForStart(); + while (opModeIsActive()) + { + crServo.setPower(1); + telemetry.addData("voltage", signal.getVoltage()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index 97543062e8d3..a425d3f576eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -59,6 +59,7 @@ public void runOpMode() { // Call update each loop to continuously control the servo position indexer.update(); + telemetry.addData("Indexer Voltage: ", indexer.getVoltageAnalog()); // Update color scanning timing and sensor reading //indexer.updateColorScanning(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java index 9e8716603e89..8c6051796c90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java @@ -9,7 +9,7 @@ public class VoltageTester extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - AnalogInput signal = hardwareMap.get(AnalogInput.class, "Encoder"); + AnalogInput signal = hardwareMap.get(AnalogInput.class, "indexAnalog"); waitForStart(); while (opModeIsActive()) { From 4b6f12240c5f51f559b2a0f90a6deb19e861641b Mon Sep 17 00:00:00 2001 From: naushadwaqar Date: Wed, 12 Nov 2025 17:14:49 -0500 Subject: [PATCH 34/67] add dashboard support and tweak PIDF --- TeamCode/build.gradle | 4 ++-- .../subsystems/CRServoPositionControl.java | 20 ++++++++----------- .../ftc/teamcode/subsystems/Indexer.java | 4 +++- .../testing/ContinuousValueTester.java | 11 ++++++---- 4 files changed, 20 insertions(+), 19 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 4d9e12629cb4..1836d242f183 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -23,8 +23,8 @@ android { } compileOptions { - sourceCompatibility JavaVersion.VERSION_1_8 - targetCompatibility JavaVersion.VERSION_1_8 + sourceCompatibility JavaVersion.VERSION_16 + targetCompatibility JavaVersion.VERSION_16 } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index d55e4b4b014b..781b213d025f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -1,16 +1,18 @@ package org.firstinspires.ftc.teamcode.subsystems; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.util.ElapsedTime; +@Config public class CRServoPositionControl { private final CRServo crServo; private final AnalogInput encoder; // Analog input for position from 4th wire - private double kp = 1.0; - private double ki = 0.0; - private double kd = 0.1; - private double kf = 0.1; + public static double kp = 0.15; + public static double ki = 0.0; + public static double kd = 0.02; + public static double kf = 0.2; private double integral = 0.0; private double lastError = 0.0; @@ -28,12 +30,7 @@ private double getCurrentPositionVoltage() { private double angleToVoltage(double angleDegrees) { angleDegrees = Math.max(0, Math.min(360, angleDegrees)); // Clamp - return (angleDegrees / 360.0) * 3.3; - } - - private double voltageToAngle(double voltage) { - voltage = Math.max(0, Math.min(3.3, voltage)); - return (voltage / 3.3) * 360.0; + return (angleDegrees / 360.0) * 3.2; } public void moveToAngle(double targetAngleDegrees) { @@ -51,10 +48,9 @@ public void moveToAngle(double targetAngleDegrees) { integral += error * deltaTime; double derivative = (error - lastError) / deltaTime; - double output = kp * error + ki * integral + kd * derivative + kf * targetVoltage; + double output = kp * error + ki * integral + kd * derivative + kf * Math.signum(error); output = Math.max(-1.0, Math.min(1.0, output)); crServo.setPower(output); - lastError = error; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 171882cd1dc2..7b4147d1e803 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -1,10 +1,12 @@ package org.firstinspires.ftc.teamcode.subsystems; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; +@Config public class Indexer { private IndexerState state; private boolean intaking = true; @@ -25,7 +27,7 @@ public class Indexer { private final double minWait = 100; private final double maxWait = 300; private double lastAngle = 0; - private double targetAngle = 0; + public static double targetAngle = 0; Actuator actuator; AnalogInput indexerAnalog; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java index e5113d56651e..35179fae7d93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java @@ -1,24 +1,27 @@ package org.firstinspires.ftc.teamcode.testing; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.AnalogInput; + import org.firstinspires.ftc.teamcode.subsystems.CRServoPositionControl; +@TeleOp(name = "ContinuousServoTester") public class ContinuousValueTester extends LinearOpMode { AnalogInput indexerAnalog; - @TeleOp(name = "ContinuousServoTester") - @Overrider + @Override public void runOpMode() throws InterruptedException { AnalogInput signal = hardwareMap.get(AnalogInput.class, "indexAnalog"); - CRServo crServo = new CRServoPositionControl(indexerServo, indexerAnalog); + CRServo indexerServo = hardwareMap.get(CRServo.class, "index"); + CRServoPositionControl crServo = new CRServoPositionControl(indexerServo, indexerAnalog); waitForStart(); while (opModeIsActive()) { - crServo.setPower(1); + indexerServo.setPower(1); telemetry.addData("voltage", signal.getVoltage()); telemetry.update(); } From f81aba7360e803db015a965eac35b13c5e811d7a Mon Sep 17 00:00:00 2001 From: tango8 Date: Fri, 14 Nov 2025 13:52:45 -0500 Subject: [PATCH 35/67] fixes CR pidf issues --- .../subsystems/CRServoPositionControl.java | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 781b213d025f..222a9ed96dc3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -12,10 +12,11 @@ public class CRServoPositionControl { public static double kp = 0.15; public static double ki = 0.0; public static double kd = 0.02; - public static double kf = 0.2; + public static double kf = 0.1; private double integral = 0.0; private double lastError = 0.0; + private double filteredVoltage = 0; private ElapsedTime timer = new ElapsedTime(); public CRServoPositionControl(CRServo servo, AnalogInput encoder) { @@ -24,28 +25,34 @@ public CRServoPositionControl(CRServo servo, AnalogInput encoder) { timer.reset(); } - private double getCurrentPositionVoltage() { - return encoder.getVoltage(); // Read analog voltage from 4th wire + private double getFilteredVoltage() { + filteredVoltage = 0.9 * filteredVoltage + 0.1 * encoder.getVoltage(); + return filteredVoltage; } + private double angleToVoltage(double angleDegrees) { angleDegrees = Math.max(0, Math.min(360, angleDegrees)); // Clamp return (angleDegrees / 360.0) * 3.2; + // REV Through-Bore analog encoders output 0–3.3V, not 0–3.2V apparently but we measured 3.2 so we will try } public void moveToAngle(double targetAngleDegrees) { double targetVoltage = angleToVoltage(targetAngleDegrees); - double currentVoltage = getCurrentPositionVoltage(); + double currentVoltage = getFilteredVoltage(); double error = targetVoltage - currentVoltage; // Shortest path wrap handling, for continuous rotation (optional) - if (error > 1.65) { error -= 3.3; } - if (error < -1.65) { error += 3.3; } + if (error > 1.6) { error -= 3.2; } + if (error < -1.6) { error += 3.2; } double deltaTime = timer.seconds(); timer.reset(); + if (deltaTime <= 0.0001) deltaTime = 0.0001; + integral += error * deltaTime; + integral = Math.max(-2, Math.min(2, integral)); double derivative = (error - lastError) / deltaTime; double output = kp * error + ki * integral + kd * derivative + kf * Math.signum(error); From 8d47fc2a069cad4205ffaac9881a311a76724111 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 16:07:50 -0500 Subject: [PATCH 36/67] Sigma teleop made (combines 3 other teleops) --- .../ftc/teamcode/subsystems/Outtake.java | 2 +- .../ftc/teamcode/teleop/SigmaTeleop.java | 180 ++++++++++++++++++ 2 files changed, 181 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java index 07f767bf5b43..3434e804caa2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java @@ -11,7 +11,7 @@ public class Outtake { public Outtake(HardwareMap hardwareMap) { motor = new MotorEx(hardwareMap, "outtake"); - + power = 1; } public void stop() diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java new file mode 100644 index 000000000000..1774e1442fe5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java @@ -0,0 +1,180 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.*; + +@TeleOp(name = "UnifiedTeleOp", group = "AA_main") +public class SigmaTeleop extends LinearOpMode { + + private Intake intake; + private Indexer indexer; + private Actuator actuator; + private Outtake outtake; + private Movement movement; + + private AprilTag aprilTag; + private AprilTagAimer aprilAimer; + + private long lastAimUpdate = 0; + private double lastTurnCorrection = 0; + + private boolean continuousAprilTagLock = false; + private boolean fieldCentric = false; + + private static final long AIM_UPDATE_INTERVAL_MS = 50; + + @Override + public void runOpMode() throws InterruptedException { + intake = new Intake(hardwareMap); + indexer = new Indexer(hardwareMap); + actuator = new Actuator(hardwareMap); + outtake = new Outtake(hardwareMap); + movement = new Movement(hardwareMap); + + aprilTag = new AprilTag(hardwareMap); + aprilAimer = new AprilTagAimer(hardwareMap); + + GamepadEx gp1 = new GamepadEx(gamepad1); + GamepadEx gp2 = new GamepadEx(gamepad2); + + waitForStart(); + + indexer.startIntake(); + + while (opModeIsActive()) { + gp1.readButtons(); + gp2.readButtons(); + + teleopTick(gp1, gp2, telemetry); + } + } + + //teleop type shift + public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { + + //apriltag turn correction + double turnCorrection = 0; + + if (continuousAprilTagLock) { + long now = System.currentTimeMillis(); + + if (now - lastAimUpdate >= AIM_UPDATE_INTERVAL_MS) { + lastAimUpdate = now; + + aprilTag.scanGoalTag(); + double bearing = aprilTag.getBearing(); + + if (!Double.isNaN(bearing)) { + lastTurnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); + } else { + lastTurnCorrection = 0; + } + } + + turnCorrection = 0.9 * lastTurnCorrection; // smooth decay + } + + //drivetrain control + if (fieldCentric) { + movement.teleopTickFieldCentric( + g1.getLeftX(), + g1.getLeftY(), + g1.getRightX(), + turnCorrection, + true + ); + } else { + movement.teleopTick( + g1.getLeftX(), + g1.getLeftY(), + g1.getRightX(), + turnCorrection + ); + } + + // Toggle field centric + if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = true; + if (g1.getButton(GamepadKeys.Button.RIGHT_STICK_BUTTON)) fieldCentric = false; + + + + // intake control + if (g2.wasJustPressed(GamepadKeys.Button.A)) { + intake.run(!intake.isRunning()); + } + + + + // spindexer control + // Advance state + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + if (!indexer.isBusy()) indexer.moveTo(indexer.nextState()); + } + + // Set intaking ON + if (g2.wasJustPressed(GamepadKeys.Button.B)) { + if (!indexer.isBusy()) indexer.setIntaking(true); + } + + // Set intaking OFF + if (g2.wasJustPressed(GamepadKeys.Button.Y)) { + if (!indexer.isBusy()) indexer.setIntaking(false); + } + + indexer.update(); + + + //outtake control + if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { + outtake.run(); + } else { + outtake.stop(); + } + + + //actuator control + if (g2.wasJustPressed(GamepadKeys.Button.X)) { + actuator.set(!actuator.isActivated()); + } + + // Scan obelisk + if (g2.wasJustPressed(GamepadKeys.Button.Y)) { + aprilTag.scanObeliskTag(); + telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); + } + + // Begin continuous lock + if (g2.wasJustPressed(GamepadKeys.Button.A)) { + continuousAprilTagLock = true; + aprilTag.setCurrentCameraScannedId(0); + } + + // Stop continuous lock + if (g2.wasJustPressed(GamepadKeys.Button.B)) { + continuousAprilTagLock = false; + } + + // Alliance selection + if (g2.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) + aprilTag.setGoalTagID(20); + if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) + aprilTag.setGoalTagID(24); + + + + // ========== TELEMETRY ========== + telemetry.addData("Field Centric", fieldCentric); + telemetry.addData("April Lock", continuousAprilTagLock); + telemetry.addData("Turn Correction", turnCorrection); + telemetry.addData("Indexer State", indexer.getState()); + telemetry.addData("Next State", indexer.nextState()); + telemetry.addData("Indexer Voltage", indexer.getVoltageAnalog()); + telemetry.addData("Outtake Power", outtake.getPower()); + telemetry.update(); + } +} From 39705a34c35df7416525a2b4e4cb08b19be2c7eb Mon Sep 17 00:00:00 2001 From: andrewcodes28 <180860358+andrewcodes28@users.noreply.github.com> Date: Fri, 14 Nov 2025 16:34:06 -0500 Subject: [PATCH 37/67] being outdated = bad --- TeamCode/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 1836d242f183..01b5e675538e 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -47,5 +47,5 @@ dependencies { implementation "com.acmerobotics.roadrunner:ftc:0.1.23" implementation "com.acmerobotics.roadrunner:core:1.0.1" implementation "com.acmerobotics.roadrunner:actions:1.0.1" - implementation "com.acmerobotics.dashboard:dashboard:0.4.17" + implementation "com.acmerobotics.dashboard:dashboard:0.5.1" } \ No newline at end of file From 761db141f8ecfa45cd173d798f62fa231201bfd9 Mon Sep 17 00:00:00 2001 From: andrewcodes28 <180860358+andrewcodes28@users.noreply.github.com> Date: Fri, 14 Nov 2025 18:49:21 -0500 Subject: [PATCH 38/67] add some actuator positions --- .../firstinspires/ftc/teamcode/subsystems/Actuator.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java index ceb3c755b176..02df7b0f1f93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java @@ -4,8 +4,8 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Actuator { - private final int DOWN = 0; // flush with the floor of platform - private final int UP = 180; // raised to push it into the flywheel + private final double DOWN = 0.08; // flush with the floor of platform + private final double UP = 1; // raised to push it into the flywheel private boolean activated; @@ -16,12 +16,12 @@ public Actuator(HardwareMap hardwareMap) { } public void down() { - servo.turnToAngle(DOWN); + servo.setPosition(DOWN); activated = false; } public void up() { - servo.turnToAngle(UP); + servo.setPosition(UP); activated = true; } From c03e7ff06d76a55d0e88645e02ef62de3cc09fe2 Mon Sep 17 00:00:00 2001 From: andrewcodes28 <180860358+andrewcodes28@users.noreply.github.com> Date: Fri, 14 Nov 2025 18:49:30 -0500 Subject: [PATCH 39/67] add angle offset logic --- .../firstinspires/ftc/teamcode/subsystems/Indexer.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 7b4147d1e803..0da8fbcb8484 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -26,8 +26,9 @@ public class Indexer { private final double msPerDegree = 0.6; private final double minWait = 100; private final double maxWait = 300; - private double lastAngle = 0; - public static double targetAngle = 0; + public static double offsetAngle = 0; + public static double targetAngle = offsetAngle; + private double lastAngle = targetAngle; Actuator actuator; AnalogInput indexerAnalog; @@ -156,10 +157,11 @@ public void moveTo(IndexerState newState) { shiftArtifacts(state, newState); double oldAngle = lastAngle; if (intaking) { - targetAngle = ((stateToNum(newState) - 1) * 120 + 60) % 360; + targetAngle = (stateToNum(newState) - 1) * 120 + 60; } else { targetAngle = (stateToNum(newState) - 1) * 120; } + targetAngle = (targetAngle + offsetAngle) % 360; double angleDelta = Math.abs(targetAngle - oldAngle); if (angleDelta > 180) angleDelta = 360 - angleDelta; From fb09961d2fd8822135681e4285a544257bc6194a Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 21:40:39 -0500 Subject: [PATCH 40/67] This is theoretically a much better CR control but I'm not gonna try to make it work rn --- .../teamcode/subsystems/BetterCRControl.java | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java new file mode 100644 index 000000000000..f4c23233bb94 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.subsystems; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Config +public class BetterCRControl { + private final CRServo crServo; + private final AnalogInput encoder; + + public static double kp = 0.45; + public static double ki = 0.0; + public static double kd = 0.04; + + public static double deadband = 1.5; // degrees + public static double minPower = 0.08; + public static double holdPower = 0.05; + + private double integral = 0.0; + private double lastError = 0.0; + private double filteredVoltage = 0; + + private ElapsedTime timer = new ElapsedTime(); + + public BetterCRControl(CRServo servo, AnalogInput encoder) { + this.crServo = servo; + this.encoder = encoder; + timer.reset(); + } + + private double getFilteredVoltage() { + filteredVoltage = 0.6 * filteredVoltage + 0.4 * encoder.getVoltage(); + return filteredVoltage; + } + + private double voltageToAngle(double v) { + return (v / 3.2) * 360.0; + } + + private double wrapDegrees(double angle) { + angle %= 360; + if (angle > 180) angle -= 360; + if (angle < -180) angle += 360; + return angle; + } + + public void moveToAngle(double target) { + double current = voltageToAngle(getFilteredVoltage()); + double error = wrapDegrees(target - current); + + // Hold state behavior + if (Math.abs(error) < deadband) { + crServo.setPower(holdPower * Math.signum(lastError)); + lastError = error; + return; + } + + double dt = timer.seconds(); + timer.reset(); + if (dt < 0.0001) dt = 0.0001; + + integral += error * dt; + integral = Math.max(-2, Math.min(2, integral)); + + double derivative = (error - lastError) / dt; + + double output = kp * error + ki * integral + kd * derivative; + + // Minimum power to break stiction + if (Math.abs(output) < minPower) + output = minPower * Math.signum(output); + + output = Math.max(-1.0, Math.min(1.0, output)); + + crServo.setPower(output); + + lastError = error; + } +} From 3eb93194118e4f276d0870fe6ad7b26af9a2edad Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 21:41:02 -0500 Subject: [PATCH 41/67] Ive been tuning so much im gonna jump --- .../ftc/teamcode/subsystems/CRServoPositionControl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 222a9ed96dc3..b895aef19eb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -9,10 +9,10 @@ public class CRServoPositionControl { private final CRServo crServo; private final AnalogInput encoder; // Analog input for position from 4th wire - public static double kp = 0.15; + public static double kp = 0.; public static double ki = 0.0; public static double kd = 0.02; - public static double kf = 0.1; + public static double kf = 0.0; private double integral = 0.0; private double lastError = 0.0; From fc1650e1b4a2e934d0d8a6543e3be7f427a4c0bb Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 21:41:35 -0500 Subject: [PATCH 42/67] Tried to fix this test opmode but whoever made it forgot you need to engage a axon before it gets properly recorded for some reason --- .../firstinspires/ftc/teamcode/testing/VoltageTester.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java index 8c6051796c90..c320d0b25bdd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java @@ -5,15 +5,17 @@ import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + @TeleOp(name = "Voltage Tester") public class VoltageTester extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - AnalogInput signal = hardwareMap.get(AnalogInput.class, "indexAnalog"); + Indexer indexer = new Indexer(hardwareMap); waitForStart(); while (opModeIsActive()) { - telemetry.addData("voltage", signal.getVoltage()); + telemetry.addData("voltage", indexer.getVoltageAnalog()); telemetry.update(); } } From 97117a8f813157739b9464240cca33de16ad0ca8 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 21:41:47 -0500 Subject: [PATCH 43/67] Much better power for crservotest --- .../ftc/teamcode/testing/ContinuousValueTester.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java index 35179fae7d93..d20597c34449 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java @@ -21,7 +21,7 @@ public void runOpMode() throws InterruptedException { waitForStart(); while (opModeIsActive()) { - indexerServo.setPower(1); + indexerServo.setPower(0.1); telemetry.addData("voltage", signal.getVoltage()); telemetry.update(); } From fa1bfc0df2d55e373d749e6b7dd4666557dce947 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 21:42:20 -0500 Subject: [PATCH 44/67] unlimited power --- .../java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java index 3434e804caa2..c2e62c869fac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Outtake.java @@ -11,7 +11,7 @@ public class Outtake { public Outtake(HardwareMap hardwareMap) { motor = new MotorEx(hardwareMap, "outtake"); - power = 1; + power = -1; } public void stop() From 8b9002b995776d26337966acb4cb8f759a8c688e Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Fri, 14 Nov 2025 21:42:29 -0500 Subject: [PATCH 45/67] skib --- .../firstinspires/ftc/teamcode/subsystems/Actuator.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java index ceb3c755b176..02df7b0f1f93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java @@ -4,8 +4,8 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Actuator { - private final int DOWN = 0; // flush with the floor of platform - private final int UP = 180; // raised to push it into the flywheel + private final double DOWN = 0.08; // flush with the floor of platform + private final double UP = 1; // raised to push it into the flywheel private boolean activated; @@ -16,12 +16,12 @@ public Actuator(HardwareMap hardwareMap) { } public void down() { - servo.turnToAngle(DOWN); + servo.setPosition(DOWN); activated = false; } public void up() { - servo.turnToAngle(UP); + servo.setPosition(UP); activated = true; } From 894414ea9e6f0132bfaf9bfc6ed2257a90a4cac7 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sat, 15 Nov 2025 08:13:26 -0500 Subject: [PATCH 46/67] PIDF values --- .../teamcode/subsystems/CRServoPositionControl.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 222a9ed96dc3..76ebcccd19e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -9,11 +9,11 @@ public class CRServoPositionControl { private final CRServo crServo; private final AnalogInput encoder; // Analog input for position from 4th wire - public static double kp = 0.15; + public static double kp = 0.41; public static double ki = 0.0; - public static double kd = 0.02; - public static double kf = 0.1; - + public static double kd = 0.0; + public static double kf = 0.0; + public static double filterAlpha = 0.9; private double integral = 0.0; private double lastError = 0.0; private double filteredVoltage = 0; @@ -26,7 +26,7 @@ public CRServoPositionControl(CRServo servo, AnalogInput encoder) { } private double getFilteredVoltage() { - filteredVoltage = 0.9 * filteredVoltage + 0.1 * encoder.getVoltage(); + filteredVoltage = (1 - filterAlpha) * filteredVoltage + filterAlpha * encoder.getVoltage(); return filteredVoltage; } From aadcbeb7a10d2ace98954a23227ab8504b688df8 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sat, 15 Nov 2025 08:21:44 -0500 Subject: [PATCH 47/67] PIDF values --- .../ftc/teamcode/subsystems/CRServoPositionControl.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 76ebcccd19e4..057d8f5fa61f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -9,9 +9,9 @@ public class CRServoPositionControl { private final CRServo crServo; private final AnalogInput encoder; // Analog input for position from 4th wire - public static double kp = 0.41; - public static double ki = 0.0; - public static double kd = 0.0; + public static double kp = 0.6; + public static double ki = 0.01; + public static double kd = 0.067; public static double kf = 0.0; public static double filterAlpha = 0.9; private double integral = 0.0; From 1191e319825308a8e84b1d28aedf9b129f12cb93 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Sat, 15 Nov 2025 08:44:32 -0500 Subject: [PATCH 48/67] last stuff, tuning from last night, etc,etc --- .../ftc/teamcode/subsystems/CRServoPositionControl.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 9fe95eaf607e..5e6a07b1b5a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -10,7 +10,7 @@ public class CRServoPositionControl { private final AnalogInput encoder; // Analog input for position from 4th wire public static double kp = 0.6; - public static double ki = 0.01; + public static double ki = 0.0; public static double kd = 0.067; public static double kf = 0.0; public static double filterAlpha = 0.2; From b9fe6151b9f723c82a9367d776ac99fa345caf8b Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Sat, 15 Nov 2025 08:45:20 -0500 Subject: [PATCH 49/67] skipping offset angle for now, also making intake -> outtake a 180 degree turn for intuition of colors without color sensro --- .../firstinspires/ftc/teamcode/subsystems/Indexer.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 0da8fbcb8484..c746c616965d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -26,9 +26,8 @@ public class Indexer { private final double msPerDegree = 0.6; private final double minWait = 100; private final double maxWait = 300; - public static double offsetAngle = 0; - public static double targetAngle = offsetAngle; - private double lastAngle = targetAngle; + private double lastAngle = 0; + public static double targetAngle = 0; Actuator actuator; AnalogInput indexerAnalog; @@ -57,7 +56,7 @@ public enum IndexerState { public void setIntaking(boolean isIntaking) { if (this.intaking != isIntaking) { this.intaking = isIntaking; - moveTo(nextState()); + moveTo(state); } } @@ -157,11 +156,10 @@ public void moveTo(IndexerState newState) { shiftArtifacts(state, newState); double oldAngle = lastAngle; if (intaking) { - targetAngle = (stateToNum(newState) - 1) * 120 + 60; + targetAngle = ((stateToNum(newState) - 1) * 120 + 180) % 360; } else { targetAngle = (stateToNum(newState) - 1) * 120; } - targetAngle = (targetAngle + offsetAngle) % 360; double angleDelta = Math.abs(targetAngle - oldAngle); if (angleDelta > 180) angleDelta = 360 - angleDelta; From e24c8668bd0baa855ab7e07b8d3aaf81daf64ae1 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Sat, 15 Nov 2025 08:45:37 -0500 Subject: [PATCH 50/67] tuned intaking power --- .../java/org/firstinspires/ftc/teamcode/subsystems/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index f4482f7f1747..c7e8877e3a08 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -7,7 +7,7 @@ public class Intake { // ARC Thunder Vortex - private final double INTAKING_POWER = -1.0; + private final double INTAKING_POWER = -0.8; private MotorEx intakeMotor; private boolean running; From b50eb9324dd520dddddb2f4ee9a49a141d71a744 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sat, 15 Nov 2025 11:55:48 -0500 Subject: [PATCH 51/67] fix maintele --- .../subsystems/CRServoPositionControl.java | 6 +-- .../ftc/teamcode/subsystems/Indexer.java | 4 +- .../ftc/teamcode/subsystems/Intake.java | 20 +++++----- .../ftc/teamcode/teleop/Prototyping.java | 8 ++-- .../ftc/teamcode/teleop/SigmaTeleop.java | 39 +++++++++---------- .../ftc/teamcode/testing/SpindexerTester.java | 4 +- 6 files changed, 41 insertions(+), 40 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 5e6a07b1b5a2..76ebcccd19e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -9,11 +9,11 @@ public class CRServoPositionControl { private final CRServo crServo; private final AnalogInput encoder; // Analog input for position from 4th wire - public static double kp = 0.6; + public static double kp = 0.41; public static double ki = 0.0; - public static double kd = 0.067; + public static double kd = 0.0; public static double kf = 0.0; - public static double filterAlpha = 0.2; + public static double filterAlpha = 0.9; private double integral = 0.0; private double lastError = 0.0; private double filteredVoltage = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index c746c616965d..d8ea1ef5c61d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -26,8 +26,9 @@ public class Indexer { private final double msPerDegree = 0.6; private final double minWait = 100; private final double maxWait = 300; - private double lastAngle = 0; public static double targetAngle = 0; + public static double offsetAngle = 106.5; + private double lastAngle = offsetAngle; Actuator actuator; AnalogInput indexerAnalog; @@ -160,6 +161,7 @@ public void moveTo(IndexerState newState) { } else { targetAngle = (stateToNum(newState) - 1) * 120; } + targetAngle = (targetAngle + offsetAngle) % 360; double angleDelta = Math.abs(targetAngle - oldAngle); if (angleDelta > 180) angleDelta = 360 - angleDelta; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java index c7e8877e3a08..c6f0618a935f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java @@ -16,20 +16,18 @@ public Intake(HardwareMap hardwareMap) intakeMotor = new MotorEx(hardwareMap, "intake"); } - public void run(boolean startMotor) + public void stop() { - running = startMotor; - if(startMotor) - { - intakeMotor.set(INTAKING_POWER); - } - else { - intakeMotor.stopMotor(); - } + intakeMotor.stopMotor(); } - public boolean isRunning() + public void run() { - return running; + intakeMotor.set(INTAKING_POWER); + } + + public void setPower(double newPower) + { + intakeMotor.set(INTAKING_POWER); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java index d49989f3ac9b..774720b91441 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Prototyping.java @@ -48,9 +48,11 @@ public void teleopTick(GamepadEx padOne, GamepadEx padTwo, Telemetry telemetry) movement.teleopTick(padOne.getLeftX(),padOne.getLeftY(),padOne.getRightX(), 0);//,padOne.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER),telemetry); telemetry.addData("Outtake Power: ",outtake.getPower()); - if(padTwo.wasJustPressed(GamepadKeys.Button.A)) - { - intake.run(!intake.isRunning()); + if(padTwo.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ + intake.run(); + } + else { + intake.stop(); } if(padTwo.wasJustPressed(GamepadKeys.Button.B)) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java index 1774e1442fe5..ea272d3f28db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java @@ -104,11 +104,19 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { // intake control - if (g2.wasJustPressed(GamepadKeys.Button.A)) { - intake.run(!intake.isRunning()); + if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ + intake.run(); + } + else { + intake.stop(); } - + //outtake control + if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { + outtake.run(); + } else { + outtake.stop(); + } // spindexer control // Advance state @@ -117,53 +125,44 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { } // Set intaking ON - if (g2.wasJustPressed(GamepadKeys.Button.B)) { + if (g2.wasJustPressed(GamepadKeys.Button.A)) { if (!indexer.isBusy()) indexer.setIntaking(true); } // Set intaking OFF - if (g2.wasJustPressed(GamepadKeys.Button.Y)) { + if (g2.wasJustPressed(GamepadKeys.Button.B)) { if (!indexer.isBusy()) indexer.setIntaking(false); } indexer.update(); - - //outtake control - if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { - outtake.run(); - } else { - outtake.stop(); - } - - //actuator control - if (g2.wasJustPressed(GamepadKeys.Button.X)) { + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { actuator.set(!actuator.isActivated()); } // Scan obelisk - if (g2.wasJustPressed(GamepadKeys.Button.Y)) { + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { aprilTag.scanObeliskTag(); telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); } // Begin continuous lock - if (g2.wasJustPressed(GamepadKeys.Button.A)) { + if (g2.wasJustPressed(GamepadKeys.Button.X)) { continuousAprilTagLock = true; aprilTag.setCurrentCameraScannedId(0); } // Stop continuous lock - if (g2.wasJustPressed(GamepadKeys.Button.B)) { + if (g2.wasJustPressed(GamepadKeys.Button.Y)) { continuousAprilTagLock = false; } // Alliance selection if (g2.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) - aprilTag.setGoalTagID(20); + aprilTag.setGoalTagID(20); // blue if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) - aprilTag.setGoalTagID(24); + aprilTag.setGoalTagID(24); // red diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java index a425d3f576eb..095c650fb1e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -52,9 +52,9 @@ public void runOpMode() { } if (gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { - intake.run(true); + intake.run(); } else { - intake.run(false); + intake.stop(); } // Call update each loop to continuously control the servo position From d05a251b21a1016b088b351a2977e009727df78d Mon Sep 17 00:00:00 2001 From: tango8 Date: Sat, 15 Nov 2025 12:54:06 -0500 Subject: [PATCH 52/67] aautos --- .../auto/roadrunner/AutoPathNavigator.java | 563 ++++++++++++++++++ .../teamcode/auto/roadrunner/BotActions.java | 51 ++ .../auto/roadrunner/opmodes/BlueFar.java | 14 + .../auto/roadrunner/opmodes/BlueNear.java | 14 + .../auto/roadrunner/opmodes/BlueParkFar.java | 14 + .../auto/roadrunner/opmodes/BlueParkNear.java | 14 + .../auto/roadrunner/opmodes/RedFar.java | 14 + .../auto/roadrunner/opmodes/RedNear.java | 14 + .../auto/roadrunner/opmodes/RedParkFar.java | 14 + .../auto/roadrunner/opmodes/RedParkNear.java | 14 + .../ftc/teamcode/subsystems/Actuator.java | 2 +- .../ftc/teamcode/subsystems/Indexer.java | 2 +- .../ftc/teamcode/teleop/SigmaTeleop.java | 27 +- 13 files changed, 743 insertions(+), 14 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java new file mode 100644 index 000000000000..8f5c39996fa3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java @@ -0,0 +1,563 @@ +/* + * Autonomous code + * Teleop and action methods + * Uses RoadRunner + * Setup and integration of RR in miscRR/MecanumDrive.java + */ + +package org.firstinspires.ftc.teamcode.auto.roadrunner; + +// Road Runner imports +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.acmerobotics.roadrunner.Vector2d; + +// Hardware imports +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +// Other +import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + +import java.util.ArrayList; + + +/* --useful info-- + * Naming scheme - far means the side with both goals and near is opposite of it + * + * Robot starts at 1 of 2 locations: + * 1. touching goal or far wall and touching far launch zone (Far opmodes) + * 2. touching near wall and touching near launch zone (Near opmodes) + * And they must be on your alliance's side + * + * tiles are 24 * 24 inches + */ + +// We will make it for all 4 positions, just because allied teams may only have certain auto positions +// If solo full auto is undoable and alliance partner has auto, we shall see how we want to do dividing work with the other team. +// red is right side, blue is left side (perspective of facing obelisk) +// Might/probably will split up this file for easier navigation and organization +public class AutoPathNavigator { + + // Positions numbered from bottom(nearest) rows up, and furthest from your alliance's wall to closest + private static final Vector2d[][] artifact_positions = new Vector2d[3][3]; + + // All of these positions could be redundant except the last of each if indexer works quick enough + public static void runOpModeRedFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + int x_val_pose_0 = -20; + int x_val_pose_1 = -25; + int x_val_pose_2 = -30; + + int y_val_col_0 = 48; + int y_val_col_1 = 72; + int y_val_col_2 = 96; + + Vector2d shootPos = new Vector2d(-10, 10); + + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(180)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(180)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(180)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(180)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(180)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(180)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(180)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(180)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(180)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(180)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(0))); + + int x_val_pose_0 = 20; + int x_val_pose_1 = 25; + int x_val_pose_2 = 30; + + int y_val_col_0 = 48; + int y_val_col_1 = 72; + int y_val_col_2 = 96; + + Vector2d shootPos = new Vector2d(10, 10); + + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(0))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeRedNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(0))); + + int x_val_pose_0 = 20; + int x_val_pose_1 = 25; + int x_val_pose_2 = 30; + + int y_val_col_0 = 24; + int y_val_col_1 = 48; + int y_val_col_2 = 72; + + Vector2d shootPos = new Vector2d(0, 0); + + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(0))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + int x_val_pose_0 = -20; + int x_val_pose_1 = -25; + int x_val_pose_2 = -30; + + int y_val_col_0 = 24; + int y_val_col_1 = 48; + int y_val_col_2 = 72; + + Vector2d shootPos = new Vector2d(0, 0); + + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(-24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeRedParkFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueParkFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeRedParkNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueParkNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(-24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java new file mode 100644 index 000000000000..a55cfd6fe44b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java @@ -0,0 +1,51 @@ +/* + * Action methods for use in Auto + */ + +package org.firstinspires.ftc.teamcode.auto.roadrunner; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.ParallelAction; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + + +public class BotActions { + private final Intake intake; + private final Outtake outtake; + private final Indexer indexer; + + public BotActions(@NonNull HardwareMap hardwareMap, @NonNull Telemetry telemetry) { + intake = new Intake(hardwareMap); + + indexer = new Indexer(hardwareMap); + + outtake = new Outtake(hardwareMap); + } + + public Action actionIntake() { + // Intakes and rotates indexer + return new SequentialAction( + + ); + } + + public Action actionOuttake() { + // Rotates Indexer and outtakes all 3 + return new SequentialAction( + ); + } + + public Action actionPark() { + // vert slides + return new ParallelAction( + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java new file mode 100644 index 000000000000..739c16f36d52 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Blue-Far", group = "Autonomous") +public class BlueFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java new file mode 100644 index 000000000000..f3cdc0228295 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Blue-Near", group = "Autonomous") +public class BlueNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java new file mode 100644 index 000000000000..96ad6f19f1a7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "BluePark-Far", group = "Autonomous") +public class BlueParkFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueParkFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java new file mode 100644 index 000000000000..9993c28f1a4f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "BluePark-Near", group = "Autonomous") +public class BlueParkNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueParkNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java new file mode 100644 index 000000000000..2c94a98eeea1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Red-Far", group = "Autonomous") +public class RedFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java new file mode 100644 index 000000000000..71d19dec0faf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Red-Close", group = "Autonomous") +public class RedNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java new file mode 100644 index 000000000000..e259656f8aea --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "RedPark-Far", group = "Autonomous") +public class RedParkFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedParkFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java new file mode 100644 index 000000000000..fe43569d0449 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "RedPark-Near", group = "Autonomous") +public class RedParkNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedParkNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java index 02df7b0f1f93..b2777a3b369d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java @@ -4,7 +4,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Actuator { - private final double DOWN = 0.08; // flush with the floor of platform + private final double DOWN = 0.0; // flush with the floor of platform private final double UP = 1; // raised to push it into the flywheel private boolean activated; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index d8ea1ef5c61d..5d99eb081a87 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -27,7 +27,7 @@ public class Indexer { private final double minWait = 100; private final double maxWait = 300; public static double targetAngle = 0; - public static double offsetAngle = 106.5; + public static double offsetAngle = 105; private double lastAngle = offsetAngle; Actuator actuator; AnalogInput indexerAnalog; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java index ea272d3f28db..b91834a39a24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java @@ -120,10 +120,24 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { // spindexer control // Advance state - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { if (!indexer.isBusy()) indexer.moveTo(indexer.nextState()); } + //actuator control + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + actuator.up(); + } + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { + actuator.down(); + } + + // Scan obelisk + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { + aprilTag.scanObeliskTag(); + telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); + } + // Set intaking ON if (g2.wasJustPressed(GamepadKeys.Button.A)) { if (!indexer.isBusy()) indexer.setIntaking(true); @@ -136,17 +150,6 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { indexer.update(); - //actuator control - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { - actuator.set(!actuator.isActivated()); - } - - // Scan obelisk - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { - aprilTag.scanObeliskTag(); - telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); - } - // Begin continuous lock if (g2.wasJustPressed(GamepadKeys.Button.X)) { continuousAprilTagLock = true; From 153c320ea0f2dc94d7a5c3730a16582892278a11 Mon Sep 17 00:00:00 2001 From: tango8 Date: Sat, 15 Nov 2025 15:02:46 -0500 Subject: [PATCH 53/67] adjustments --- .../firstinspires/ftc/teamcode/subsystems/Actuator.java | 2 +- .../ftc/teamcode/subsystems/CRServoPositionControl.java | 8 ++++---- .../firstinspires/ftc/teamcode/subsystems/Indexer.java | 1 + .../firstinspires/ftc/teamcode/teleop/SigmaTeleop.java | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java index b2777a3b369d..c8f6dcb1817b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Actuator.java @@ -5,7 +5,7 @@ public class Actuator { private final double DOWN = 0.0; // flush with the floor of platform - private final double UP = 1; // raised to push it into the flywheel + private final double UP = .27; // raised to push it into the flywheel private boolean activated; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java index 76ebcccd19e4..d1a27d7faa8d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/CRServoPositionControl.java @@ -12,7 +12,7 @@ public class CRServoPositionControl { public static double kp = 0.41; public static double ki = 0.0; public static double kd = 0.0; - public static double kf = 0.0; + public static double kf = 0.01; public static double filterAlpha = 0.9; private double integral = 0.0; private double lastError = 0.0; @@ -33,7 +33,7 @@ private double getFilteredVoltage() { private double angleToVoltage(double angleDegrees) { angleDegrees = Math.max(0, Math.min(360, angleDegrees)); // Clamp - return (angleDegrees / 360.0) * 3.2; + return (angleDegrees / 360.0) * 3.3; // REV Through-Bore analog encoders output 0–3.3V, not 0–3.2V apparently but we measured 3.2 so we will try } @@ -44,8 +44,8 @@ public void moveToAngle(double targetAngleDegrees) { double error = targetVoltage - currentVoltage; // Shortest path wrap handling, for continuous rotation (optional) - if (error > 1.6) { error -= 3.2; } - if (error < -1.6) { error += 3.2; } + if (error > 1.65) { error -= 3.3; } + if (error < -1.65) { error += 3.3; } double deltaTime = timer.seconds(); timer.reset(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java index 5d99eb081a87..1ead26ca3b02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Indexer.java @@ -28,6 +28,7 @@ public class Indexer { private final double maxWait = 300; public static double targetAngle = 0; public static double offsetAngle = 105; + public static double outtakeOffsetAngle = 5; private double lastAngle = offsetAngle; Actuator actuator; AnalogInput indexerAnalog; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java index b91834a39a24..a4c3bccf68f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java @@ -139,7 +139,7 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { } // Set intaking ON - if (g2.wasJustPressed(GamepadKeys.Button.A)) { + if (g2.wasJustPressed(GamepadKeys.Button.A) && !actuator.isActivated()) { if (!indexer.isBusy()) indexer.setIntaking(true); } From c5e0d3993a2de3f9636cf6d24b5512a556e0f2dd Mon Sep 17 00:00:00 2001 From: andrewcodes28 <180860358+andrewcodes28@users.noreply.github.com> Date: Mon, 17 Nov 2025 15:32:57 -0500 Subject: [PATCH 54/67] restrict intake and outtake-related features to the indexer state --- .../ftc/teamcode/teleop/SigmaTeleop.java | 34 +++++++++++-------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java index a4c3bccf68f5..6c1333a706a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java @@ -104,18 +104,22 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { // intake control - if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ - intake.run(); - } - else { - intake.stop(); + if (indexer.getIntaking()) { + if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ + intake.run(); + } + else { + intake.stop(); + } } //outtake control - if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { - outtake.run(); - } else { - outtake.stop(); + if (!indexer.getIntaking()) { + if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { + outtake.run(); + } else { + outtake.stop(); + } } // spindexer control @@ -125,11 +129,13 @@ public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { } //actuator control - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { - actuator.up(); - } - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { - actuator.down(); + if (!indexer.getIntaking()) { + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + actuator.up(); + } + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { + actuator.down(); + } } // Scan obelisk From 14746a62999967565a78afe516f79e88d6ed4513 Mon Sep 17 00:00:00 2001 From: andrewcodes28 <180860358+andrewcodes28@users.noreply.github.com> Date: Mon, 17 Nov 2025 15:44:42 -0500 Subject: [PATCH 55/67] create bot class and refactor teleop --- .../ftc/teamcode/teleop/Bot.java | 187 ++++++++++++++++++ .../ftc/teamcode/teleop/SigmaTeleop.java | 172 +--------------- 2 files changed, 190 insertions(+), 169 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java new file mode 100644 index 000000000000..7bba828f91d6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -0,0 +1,187 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.Actuator; +import org.firstinspires.ftc.teamcode.subsystems.AprilTag; +import org.firstinspires.ftc.teamcode.subsystems.AprilTagAimer; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Movement; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; + +public class Bot { + private final Intake intake; + private final Indexer indexer; + private final Actuator actuator; + private final Outtake outtake; + private final Movement movement; + + private final AprilTag aprilTag; + private final AprilTagAimer aprilAimer; + private final GamepadEx g1; + private final GamepadEx g2; + private final Telemetry telemetry; + + private long lastAimUpdate = 0; + private double lastTurnCorrection = 0; + + private boolean continuousAprilTagLock = false; + private boolean fieldCentric = false; + + private static final long AIM_UPDATE_INTERVAL_MS = 50; + + public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad gamepad2) { + intake = new Intake(hardwareMap); + indexer = new Indexer(hardwareMap); + actuator = new Actuator(hardwareMap); + outtake = new Outtake(hardwareMap); + movement = new Movement(hardwareMap); + + aprilTag = new AprilTag(hardwareMap); + aprilAimer = new AprilTagAimer(hardwareMap); + + g1 = new GamepadEx(gamepad1); + g2 = new GamepadEx(gamepad2); + telemetry = tele; + } + public void teleopInit() { + indexer.startIntake(); + } + public void teleopTick() { + g1.readButtons(); + g2.readButtons(); + + //apriltag turn correction + double turnCorrection = 0; + + if (continuousAprilTagLock) { + long now = System.currentTimeMillis(); + + if (now - lastAimUpdate >= AIM_UPDATE_INTERVAL_MS) { + lastAimUpdate = now; + + aprilTag.scanGoalTag(); + double bearing = aprilTag.getBearing(); + + if (!Double.isNaN(bearing)) { + lastTurnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); + } else { + lastTurnCorrection = 0; + } + } + + turnCorrection = 0.9 * lastTurnCorrection; // smooth decay + } + + //drivetrain control + if (fieldCentric) { + movement.teleopTickFieldCentric( + g1.getLeftX(), + g1.getLeftY(), + g1.getRightX(), + turnCorrection, + true + ); + } else { + movement.teleopTick( + g1.getLeftX(), + g1.getLeftY(), + g1.getRightX(), + turnCorrection + ); + } + + // Toggle field centric + if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = true; + if (g1.getButton(GamepadKeys.Button.RIGHT_STICK_BUTTON)) fieldCentric = false; + + + + // intake control + if (indexer.getIntaking()) { + if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ + intake.run(); + } + else { + intake.stop(); + } + } + + //outtake control + if (!indexer.getIntaking()) { + if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { + outtake.run(); + } else { + outtake.stop(); + } + } + + // spindexer control + // Advance state + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { + if (!indexer.isBusy()) indexer.moveTo(indexer.nextState()); + } + + //actuator control + if (!indexer.getIntaking()) { + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + actuator.up(); + } + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { + actuator.down(); + } + } + + // Scan obelisk + if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { + aprilTag.scanObeliskTag(); + telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); + } + + // Set intaking ON + if (g2.wasJustPressed(GamepadKeys.Button.A) && !actuator.isActivated()) { + if (!indexer.isBusy()) indexer.setIntaking(true); + } + + // Set intaking OFF + if (g2.wasJustPressed(GamepadKeys.Button.B)) { + if (!indexer.isBusy()) indexer.setIntaking(false); + } + + indexer.update(); + + // Begin continuous lock + if (g2.wasJustPressed(GamepadKeys.Button.X)) { + continuousAprilTagLock = true; + aprilTag.setCurrentCameraScannedId(0); + } + + // Stop continuous lock + if (g2.wasJustPressed(GamepadKeys.Button.Y)) { + continuousAprilTagLock = false; + } + + // Alliance selection + if (g2.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) + aprilTag.setGoalTagID(20); // blue + if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) + aprilTag.setGoalTagID(24); // red + + + + // ========== TELEMETRY ========== + telemetry.addData("Field Centric", fieldCentric); + telemetry.addData("April Lock", continuousAprilTagLock); + telemetry.addData("Turn Correction", turnCorrection); + telemetry.addData("Indexer State", indexer.getState()); + telemetry.addData("Next State", indexer.nextState()); + telemetry.addData("Indexer Voltage", indexer.getVoltageAnalog()); + telemetry.addData("Outtake Power", outtake.getPower()); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java index 6c1333a706a1..a2781d054c2a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/SigmaTeleop.java @@ -10,179 +10,13 @@ @TeleOp(name = "UnifiedTeleOp", group = "AA_main") public class SigmaTeleop extends LinearOpMode { - - private Intake intake; - private Indexer indexer; - private Actuator actuator; - private Outtake outtake; - private Movement movement; - - private AprilTag aprilTag; - private AprilTagAimer aprilAimer; - - private long lastAimUpdate = 0; - private double lastTurnCorrection = 0; - - private boolean continuousAprilTagLock = false; - private boolean fieldCentric = false; - - private static final long AIM_UPDATE_INTERVAL_MS = 50; - @Override public void runOpMode() throws InterruptedException { - intake = new Intake(hardwareMap); - indexer = new Indexer(hardwareMap); - actuator = new Actuator(hardwareMap); - outtake = new Outtake(hardwareMap); - movement = new Movement(hardwareMap); - - aprilTag = new AprilTag(hardwareMap); - aprilAimer = new AprilTagAimer(hardwareMap); - - GamepadEx gp1 = new GamepadEx(gamepad1); - GamepadEx gp2 = new GamepadEx(gamepad2); - + Bot bot = new Bot(hardwareMap, telemetry, gamepad1, gamepad2); waitForStart(); - - indexer.startIntake(); - + bot.teleopInit(); while (opModeIsActive()) { - gp1.readButtons(); - gp2.readButtons(); - - teleopTick(gp1, gp2, telemetry); + bot.teleopTick(); } } - - //teleop type shift - public void teleopTick(GamepadEx g1, GamepadEx g2, Telemetry telemetry) { - - //apriltag turn correction - double turnCorrection = 0; - - if (continuousAprilTagLock) { - long now = System.currentTimeMillis(); - - if (now - lastAimUpdate >= AIM_UPDATE_INTERVAL_MS) { - lastAimUpdate = now; - - aprilTag.scanGoalTag(); - double bearing = aprilTag.getBearing(); - - if (!Double.isNaN(bearing)) { - lastTurnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); - } else { - lastTurnCorrection = 0; - } - } - - turnCorrection = 0.9 * lastTurnCorrection; // smooth decay - } - - //drivetrain control - if (fieldCentric) { - movement.teleopTickFieldCentric( - g1.getLeftX(), - g1.getLeftY(), - g1.getRightX(), - turnCorrection, - true - ); - } else { - movement.teleopTick( - g1.getLeftX(), - g1.getLeftY(), - g1.getRightX(), - turnCorrection - ); - } - - // Toggle field centric - if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = true; - if (g1.getButton(GamepadKeys.Button.RIGHT_STICK_BUTTON)) fieldCentric = false; - - - - // intake control - if (indexer.getIntaking()) { - if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ - intake.run(); - } - else { - intake.stop(); - } - } - - //outtake control - if (!indexer.getIntaking()) { - if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { - outtake.run(); - } else { - outtake.stop(); - } - } - - // spindexer control - // Advance state - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { - if (!indexer.isBusy()) indexer.moveTo(indexer.nextState()); - } - - //actuator control - if (!indexer.getIntaking()) { - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { - actuator.up(); - } - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { - actuator.down(); - } - } - - // Scan obelisk - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { - aprilTag.scanObeliskTag(); - telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); - } - - // Set intaking ON - if (g2.wasJustPressed(GamepadKeys.Button.A) && !actuator.isActivated()) { - if (!indexer.isBusy()) indexer.setIntaking(true); - } - - // Set intaking OFF - if (g2.wasJustPressed(GamepadKeys.Button.B)) { - if (!indexer.isBusy()) indexer.setIntaking(false); - } - - indexer.update(); - - // Begin continuous lock - if (g2.wasJustPressed(GamepadKeys.Button.X)) { - continuousAprilTagLock = true; - aprilTag.setCurrentCameraScannedId(0); - } - - // Stop continuous lock - if (g2.wasJustPressed(GamepadKeys.Button.Y)) { - continuousAprilTagLock = false; - } - - // Alliance selection - if (g2.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) - aprilTag.setGoalTagID(20); // blue - if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) - aprilTag.setGoalTagID(24); // red - - - - // ========== TELEMETRY ========== - telemetry.addData("Field Centric", fieldCentric); - telemetry.addData("April Lock", continuousAprilTagLock); - telemetry.addData("Turn Correction", turnCorrection); - telemetry.addData("Indexer State", indexer.getState()); - telemetry.addData("Next State", indexer.nextState()); - telemetry.addData("Indexer Voltage", indexer.getVoltageAnalog()); - telemetry.addData("Outtake Power", outtake.getPower()); - telemetry.update(); - } } From 41d0bbca98cb6dd06cc5f06fe8d0a9142accb7a7 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 24 Nov 2025 16:36:36 -0500 Subject: [PATCH 56/67] Don't use two buttons when you can use a toggle --- .../org/firstinspires/ftc/teamcode/teleop/Bot.java | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index 7bba828f91d6..1acb83814e25 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -155,15 +155,10 @@ public void teleopTick() { indexer.update(); - // Begin continuous lock + // Toggle continuous lock if (g2.wasJustPressed(GamepadKeys.Button.X)) { - continuousAprilTagLock = true; - aprilTag.setCurrentCameraScannedId(0); - } - - // Stop continuous lock - if (g2.wasJustPressed(GamepadKeys.Button.Y)) { - continuousAprilTagLock = false; + continuousAprilTagLock = !continuousAprilTagLock; + if (continuousAprilTagLock) aprilTag.setCurrentCameraScannedId(0); } // Alliance selection @@ -172,8 +167,6 @@ public void teleopTick() { if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) aprilTag.setGoalTagID(24); // red - - // ========== TELEMETRY ========== telemetry.addData("Field Centric", fieldCentric); telemetry.addData("April Lock", continuousAprilTagLock); From b04bc41f31e0c1967e67aee352fd34618e63365d Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 24 Nov 2025 16:42:06 -0500 Subject: [PATCH 57/67] Commented out april tag stuff cause its gonna get reworked --- .../ftc/teamcode/teleop/Bot.java | 29 +++++++++++-------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index 1acb83814e25..e1783baed0f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -21,19 +21,19 @@ public class Bot { private final Outtake outtake; private final Movement movement; - private final AprilTag aprilTag; - private final AprilTagAimer aprilAimer; + //private final AprilTag aprilTag; + //private final AprilTagAimer aprilAimer; private final GamepadEx g1; private final GamepadEx g2; private final Telemetry telemetry; - private long lastAimUpdate = 0; - private double lastTurnCorrection = 0; + //private long lastAimUpdate = 0; + //private double lastTurnCorrection = 0; - private boolean continuousAprilTagLock = false; + //private boolean continuousAprilTagLock = false; private boolean fieldCentric = false; - private static final long AIM_UPDATE_INTERVAL_MS = 50; + //private static final long AIM_UPDATE_INTERVAL_MS = 50; public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad gamepad2) { intake = new Intake(hardwareMap); @@ -42,8 +42,8 @@ public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad ga outtake = new Outtake(hardwareMap); movement = new Movement(hardwareMap); - aprilTag = new AprilTag(hardwareMap); - aprilAimer = new AprilTagAimer(hardwareMap); + //aprilTag = new AprilTag(hardwareMap); + //aprilAimer = new AprilTagAimer(hardwareMap); g1 = new GamepadEx(gamepad1); g2 = new GamepadEx(gamepad2); @@ -56,6 +56,7 @@ public void teleopTick() { g1.readButtons(); g2.readButtons(); + /* //apriltag turn correction double turnCorrection = 0; @@ -77,6 +78,7 @@ public void teleopTick() { turnCorrection = 0.9 * lastTurnCorrection; // smooth decay } + */ //drivetrain control if (fieldCentric) { @@ -84,7 +86,7 @@ public void teleopTick() { g1.getLeftX(), g1.getLeftY(), g1.getRightX(), - turnCorrection, + 0, true ); } else { @@ -92,7 +94,7 @@ public void teleopTick() { g1.getLeftX(), g1.getLeftY(), g1.getRightX(), - turnCorrection + 0 ); } @@ -138,6 +140,7 @@ public void teleopTick() { } // Scan obelisk + /* if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { aprilTag.scanObeliskTag(); telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); @@ -155,6 +158,7 @@ public void teleopTick() { indexer.update(); + /* // Toggle continuous lock if (g2.wasJustPressed(GamepadKeys.Button.X)) { continuousAprilTagLock = !continuousAprilTagLock; @@ -166,11 +170,12 @@ public void teleopTick() { aprilTag.setGoalTagID(20); // blue if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) aprilTag.setGoalTagID(24); // red + */ // ========== TELEMETRY ========== telemetry.addData("Field Centric", fieldCentric); - telemetry.addData("April Lock", continuousAprilTagLock); - telemetry.addData("Turn Correction", turnCorrection); + //telemetry.addData("April Lock", continuousAprilTagLock); + //telemetry.addData("Turn Correction", turnCorrection); telemetry.addData("Indexer State", indexer.getState()); telemetry.addData("Next State", indexer.nextState()); telemetry.addData("Indexer Voltage", indexer.getVoltageAnalog()); From 6e5a4267e05dae7e9a6dc49d949e6f792d5d2a2a Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 24 Nov 2025 17:05:18 -0500 Subject: [PATCH 58/67] we gon do fsm --- .../java/org/firstinspires/ftc/teamcode/teleop/Bot.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index e1783baed0f4..853f11a10bb2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -35,6 +35,12 @@ public class Bot { //private static final long AIM_UPDATE_INTERVAL_MS = 50; + public enum FSM{ + SortOuttake, + QuickOuttake, + Intake, + Endgame + } public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad gamepad2) { intake = new Intake(hardwareMap); indexer = new Indexer(hardwareMap); From 6957bf94aee56b540038a5967f2524ec785ec5d3 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 24 Nov 2025 17:11:52 -0500 Subject: [PATCH 59/67] Sum of these toggles just arent toggles --- .../main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index 853f11a10bb2..072fc80a016a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -105,8 +105,7 @@ public void teleopTick() { } // Toggle field centric - if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = true; - if (g1.getButton(GamepadKeys.Button.RIGHT_STICK_BUTTON)) fieldCentric = false; + if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = !fieldCentric; From e890fd66f5d26e4663b511e76a1e15e548ce6fa9 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 24 Nov 2025 19:53:25 -0500 Subject: [PATCH 60/67] Misc stuff (Comments, telemetry, unnecessary code, more FSM) --- .../ftc/teamcode/teleop/Bot.java | 99 +++---------------- 1 file changed, 11 insertions(+), 88 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index 072fc80a016a..7cbe8ee6dba8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -21,39 +21,31 @@ public class Bot { private final Outtake outtake; private final Movement movement; - //private final AprilTag aprilTag; - //private final AprilTagAimer aprilAimer; private final GamepadEx g1; private final GamepadEx g2; private final Telemetry telemetry; - //private long lastAimUpdate = 0; - //private double lastTurnCorrection = 0; - - //private boolean continuousAprilTagLock = false; private boolean fieldCentric = false; - //private static final long AIM_UPDATE_INTERVAL_MS = 50; - public enum FSM{ SortOuttake, QuickOuttake, Intake, Endgame } + + public FSM state; + public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad gamepad2) { intake = new Intake(hardwareMap); indexer = new Indexer(hardwareMap); actuator = new Actuator(hardwareMap); outtake = new Outtake(hardwareMap); movement = new Movement(hardwareMap); - - //aprilTag = new AprilTag(hardwareMap); - //aprilAimer = new AprilTagAimer(hardwareMap); - g1 = new GamepadEx(gamepad1); g2 = new GamepadEx(gamepad2); telemetry = tele; + state = FSM.Intake; } public void teleopInit() { indexer.startIntake(); @@ -61,32 +53,6 @@ public void teleopInit() { public void teleopTick() { g1.readButtons(); g2.readButtons(); - - /* - //apriltag turn correction - double turnCorrection = 0; - - if (continuousAprilTagLock) { - long now = System.currentTimeMillis(); - - if (now - lastAimUpdate >= AIM_UPDATE_INTERVAL_MS) { - lastAimUpdate = now; - - aprilTag.scanGoalTag(); - double bearing = aprilTag.getBearing(); - - if (!Double.isNaN(bearing)) { - lastTurnCorrection = aprilAimer.calculateTurnPowerToBearing(bearing); - } else { - lastTurnCorrection = 0; - } - } - - turnCorrection = 0.9 * lastTurnCorrection; // smooth decay - } - */ - - //drivetrain control if (fieldCentric) { movement.teleopTickFieldCentric( g1.getLeftX(), @@ -103,23 +69,22 @@ public void teleopTick() { 0 ); } - - // Toggle field centric if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = !fieldCentric; - - // intake control if (indexer.getIntaking()) { - if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01){ + if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01) + { intake.run(); } - else { + else + { intake.stop(); } } //outtake control + //should be replaced with better automation i'll do that eventually if (!indexer.getIntaking()) { if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { outtake.run(); @@ -128,13 +93,7 @@ public void teleopTick() { } } - // spindexer control - // Advance state - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) { - if (!indexer.isBusy()) indexer.moveTo(indexer.nextState()); - } - - //actuator control + //should be replaced with better automation i'll do that eventually if (!indexer.getIntaking()) { if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { actuator.up(); @@ -143,48 +102,12 @@ public void teleopTick() { actuator.down(); } } - - // Scan obelisk - /* - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) { - aprilTag.scanObeliskTag(); - telemetry.addData("Obelisk ID", aprilTag.getObeliskId()); - } - - // Set intaking ON - if (g2.wasJustPressed(GamepadKeys.Button.A) && !actuator.isActivated()) { - if (!indexer.isBusy()) indexer.setIntaking(true); - } - - // Set intaking OFF - if (g2.wasJustPressed(GamepadKeys.Button.B)) { - if (!indexer.isBusy()) indexer.setIntaking(false); - } - - indexer.update(); - - /* - // Toggle continuous lock - if (g2.wasJustPressed(GamepadKeys.Button.X)) { - continuousAprilTagLock = !continuousAprilTagLock; - if (continuousAprilTagLock) aprilTag.setCurrentCameraScannedId(0); - } - - // Alliance selection - if (g2.wasJustPressed(GamepadKeys.Button.LEFT_BUMPER)) - aprilTag.setGoalTagID(20); // blue - if (g2.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) - aprilTag.setGoalTagID(24); // red - */ - // ========== TELEMETRY ========== telemetry.addData("Field Centric", fieldCentric); - //telemetry.addData("April Lock", continuousAprilTagLock); - //telemetry.addData("Turn Correction", turnCorrection); telemetry.addData("Indexer State", indexer.getState()); telemetry.addData("Next State", indexer.nextState()); telemetry.addData("Indexer Voltage", indexer.getVoltageAnalog()); - telemetry.addData("Outtake Power", outtake.getPower()); + telemetry.addData("Actuator up?: ",actuator.isActivated()); telemetry.update(); } } From 2525c60c1642a7883121d661ee800ae979b4be5c Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 24 Nov 2025 20:24:15 -0500 Subject: [PATCH 61/67] Wrote it into fsm, left todos for automation (i'll get to it chat) --- .../ftc/teamcode/teleop/Bot.java | 55 +++++++++---------- 1 file changed, 26 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index 7cbe8ee6dba8..86c08051f5ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -53,6 +53,7 @@ public void teleopInit() { public void teleopTick() { g1.readButtons(); g2.readButtons(); + if (fieldCentric) { movement.teleopTickFieldCentric( g1.getLeftX(), @@ -71,36 +72,32 @@ public void teleopTick() { } if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = !fieldCentric; - // intake control - if (indexer.getIntaking()) { - if(g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER)>0.01) - { - intake.run(); - } - else - { - intake.stop(); - } - } - - //outtake control - //should be replaced with better automation i'll do that eventually - if (!indexer.getIntaking()) { - if (g2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { - outtake.run(); - } else { - outtake.stop(); - } - } + switch(state) { - //should be replaced with better automation i'll do that eventually - if (!indexer.getIntaking()) { - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { - actuator.up(); - } - if (g2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) { - actuator.down(); - } + case Intake: + if (g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.01) { + intake.run(); + } else { + intake.stop(); + } + if (g2.wasJustPressed(GamepadKeys.Button.A)){ + state = FSM.QuickOuttake; + } + //TODO: manage spinning while intaking + break; + case QuickOuttake: + if (g2.wasJustPressed(GamepadKeys.Button.X)){ + //TODO: quickspin + } + if (g2.wasJustPressed(GamepadKeys.Button.A)){ + state = FSM.Intake; + } + break; + case SortOuttake: + //TODO: figure this out + break; + case Endgame: + //TODO: Ts more of a build todo than a code one } // ========== TELEMETRY ========== telemetry.addData("Field Centric", fieldCentric); From 009e9a7f11fbc0ba6bfb5f6a1512876e45c4880b Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 1 Dec 2025 20:58:02 -0500 Subject: [PATCH 62/67] refactor Bot.java to improve state handling and movement logic --- .../ftc/teamcode/teleop/Bot.java | 134 ++++++++++++------ 1 file changed, 87 insertions(+), 47 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java index 86c08051f5ba..cf95713edb48 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -7,8 +7,6 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.subsystems.Actuator; -import org.firstinspires.ftc.teamcode.subsystems.AprilTag; -import org.firstinspires.ftc.teamcode.subsystems.AprilTagAimer; import org.firstinspires.ftc.teamcode.subsystems.Indexer; import org.firstinspires.ftc.teamcode.subsystems.Intake; import org.firstinspires.ftc.teamcode.subsystems.Movement; @@ -27,84 +25,126 @@ public class Bot { private boolean fieldCentric = false; - public enum FSM{ - SortOuttake, - QuickOuttake, + public enum FSM { Intake, + QuickOuttake, + SortOuttake, Endgame } public FSM state; + private static final double TRIGGER_DEADZONE = 0.05; + private boolean quickSpinRequested = false; + public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad gamepad2) { intake = new Intake(hardwareMap); indexer = new Indexer(hardwareMap); actuator = new Actuator(hardwareMap); - outtake = new Outtake(hardwareMap); + outtake = new Outtake(hardwareMap, Outtake.Mode.RPM); movement = new Movement(hardwareMap); g1 = new GamepadEx(gamepad1); g2 = new GamepadEx(gamepad2); telemetry = tele; state = FSM.Intake; } + + private void enterState(FSM newState) { + if (state != newState) { + state = newState; + quickSpinRequested = false; + } + } + public void teleopInit() { indexer.startIntake(); + enterState(FSM.Intake); } + public void teleopTick() { g1.readButtons(); g2.readButtons(); - if (fieldCentric) { - movement.teleopTickFieldCentric( - g1.getLeftX(), - g1.getLeftY(), - g1.getRightX(), - 0, - true - ); - } else { - movement.teleopTick( - g1.getLeftX(), - g1.getLeftY(), - g1.getRightX(), - 0 - ); - } - if (g1.getButton(GamepadKeys.Button.LEFT_STICK_BUTTON)) fieldCentric = !fieldCentric; + handleMovement(); - switch(state) { + if (g1.wasJustPressed(GamepadKeys.Button.LEFT_STICK_BUTTON)) { + fieldCentric = !fieldCentric; + } + switch (state) { case Intake: - if (g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.01) { - intake.run(); - } else { - intake.stop(); - } - if (g2.wasJustPressed(GamepadKeys.Button.A)){ - state = FSM.QuickOuttake; - } - //TODO: manage spinning while intaking + handleIntakeState(); break; case QuickOuttake: - if (g2.wasJustPressed(GamepadKeys.Button.X)){ - //TODO: quickspin - } - if (g2.wasJustPressed(GamepadKeys.Button.A)){ - state = FSM.Intake; - } + handleQuickOuttakeState(); break; case SortOuttake: - //TODO: figure this out + handleSortOuttakeState(); break; case Endgame: - //TODO: Ts more of a build todo than a code one + handleEndgameState(); + break; } - // ========== TELEMETRY ========== - telemetry.addData("Field Centric", fieldCentric); - telemetry.addData("Indexer State", indexer.getState()); - telemetry.addData("Next State", indexer.nextState()); - telemetry.addData("Indexer Voltage", indexer.getVoltageAnalog()); - telemetry.addData("Actuator up?: ",actuator.isActivated()); + + // Telem (most sigma ever) + telemetry.addData("Field Centric:", fieldCentric); + telemetry.addData("Indexer States:", "Current State: %s , Next State: %s",indexer.getState(), indexer.nextState()); + telemetry.addData("Indexer Voltages:", "Target: %.3f , Actual: %.3f" , indexer.getTargetVoltage(), indexer.getVoltageAnalog()); + telemetry.addData("Actuator up?: ", actuator.isActivated()); telemetry.update(); } + + private void handleMovement() { + double lx = g1.getLeftX(); + double ly = g1.getLeftY(); + double rx = g1.getRightX(); + + if (fieldCentric) movement.teleopTickFieldCentric(lx, ly, rx, 0, true); + else movement.teleopTick(lx, ly, rx, 0); + } + + private void handleIntakeState() { + double leftTrigger = g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); + if (leftTrigger > TRIGGER_DEADZONE) intake.run(); + else intake.stop(); + + if (g2.wasJustPressed(GamepadKeys.Button.A)) enterState(FSM.QuickOuttake); + if (g2.wasJustPressed(GamepadKeys.Button.B)) enterState(FSM.SortOuttake); + if (g2.wasJustPressed(GamepadKeys.Button.Y)) enterState(FSM.Endgame); + } + + private void handleQuickOuttakeState() { + if (g2.wasJustPressed(GamepadKeys.Button.X)) { + quickSpinRequested = !quickSpinRequested; + if (quickSpinRequested) { + // TODO: start shooter quickly (e.g. outtake.startHighSpeed()) + } else { + // TODO: outtake.stop() + } + } + + if (quickSpinRequested) { + // TODO: do ts ig + // Example: call indexer.advanceOne() with your own timing logic + } + + if (g2.wasJustPressed(GamepadKeys.Button.A)) enterState(FSM.Intake); + } + + private void handleSortOuttakeState() { + // TODO: implement sorted shoot sequence: + // 1) spin shooter to RPM + // 2) wait for stable RPM (ideally ts is in a roadronere action) + // 3) feed one ball at a time with small delays + if (g2.wasJustPressed(GamepadKeys.Button.A)) { + // cancel sort and return to intake + enterState(FSM.Intake); + } + } + + private void handleEndgameState() { + // TODO: activate actuator to open slides or perform endgame actions + // Example: actuator.extendSlides(); + if (g2.wasJustPressed(GamepadKeys.Button.A)) enterState(FSM.Intake); + } } From 41d9b7ccde5037570cfd2fc3e8dfc1fc1259a00c Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Sun, 21 Dec 2025 23:21:17 +0000 Subject: [PATCH 63/67] Adding github actions for PRs --- .github/workflows/ci.yml | 87 ++++++++++++++++++++++ .github/workflows/format-java.yml | 47 ++++++++++++ .github/workflows/spotbugs-teamcode.yml | 96 +++++++++++++++++++++++++ 3 files changed, 230 insertions(+) create mode 100644 .github/workflows/ci.yml create mode 100644 .github/workflows/format-java.yml create mode 100644 .github/workflows/spotbugs-teamcode.yml diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 000000000000..6a6f413e798f --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,87 @@ +name: CI (Build + Lint + Tests) + +on: + pull_request: + paths: + - "TeamCode/**" + - "FtcRobotController/**" + - "libs/**" + - "gradle/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/**" + push: + # Keep this narrow so you don't burn minutes on every branch. + # Add/remove branches to match how your team works. + branches: + - "Quali-1" + paths: + - "TeamCode/**" + - "FtcRobotController/**" + - "libs/**" + - "gradle/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/**" + +concurrency: + group: ci-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +jobs: + build_lint_test: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Validate Gradle Wrapper + uses: gradle/wrapper-validation-action@v3 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + distribution: temurin + java-version: "17" + + - name: Set up Android SDK + uses: android-actions/setup-android@v3 + + - name: Set up Gradle (with caching) + uses: gradle/actions/setup-gradle@v4 + + - name: Make gradlew executable + run: chmod +x ./gradlew + + - name: Build (Debug) + run: ./gradlew --no-daemon --stacktrace assembleDebug + + - name: Android Lint + run: ./gradlew --no-daemon --stacktrace lint + + - name: Unit tests (if present) + run: ./gradlew --no-daemon --stacktrace test + + - name: Upload reports (lint/tests) + if: always() + uses: actions/upload-artifact@v4 + with: + name: reports + if-no-files-found: warn + path: | + **/build/reports/** + **/build/test-results/** diff --git a/.github/workflows/format-java.yml b/.github/workflows/format-java.yml new file mode 100644 index 000000000000..d5841be1e3aa --- /dev/null +++ b/.github/workflows/format-java.yml @@ -0,0 +1,47 @@ +name: Format (TeamCode Java) + +on: + pull_request: + paths: + - "TeamCode/**/*.java" + - ".github/workflows/**" + push: + branches: + - "Quali-1" + paths: + - "TeamCode/**/*.java" + - ".github/workflows/**" + +concurrency: + group: format-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +jobs: + google_java_format: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + distribution: temurin + java-version: "17" + + # This action can either “check” or “format then show diff”. + # We choose “format then diff” so the PR shows exactly what would change. + - name: Run google-java-format (TeamCode) + uses: axel-op/googlejavaformat-action@v4 + with: + files: "TeamCode/**/*.java" + args: "--replace" + skip-commit: true + + - name: Fail if formatting changed (prints diff) + run: git --no-pager diff --exit-code + diff --git a/.github/workflows/spotbugs-teamcode.yml b/.github/workflows/spotbugs-teamcode.yml new file mode 100644 index 000000000000..679f4f52e465 --- /dev/null +++ b/.github/workflows/spotbugs-teamcode.yml @@ -0,0 +1,96 @@ +name: SpotBugs (TeamCode) + +on: + pull_request: + paths: + - "TeamCode/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradle/**" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/spotbugs-teamcode.yml" + push: + branches: ["Quali-1"] + paths: + - "TeamCode/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradle/**" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/spotbugs-teamcode.yml" + +concurrency: + group: spotbugs-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +jobs: + spotbugs_teamcode: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Validate Gradle Wrapper + uses: gradle/wrapper-validation-action@v3 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + distribution: temurin + java-version: "17" + + # FTC/Android Gradle builds typically need the Android SDK present even for analysis tasks + - name: Set up Android SDK + uses: android-actions/setup-android@v3 + + - name: Set up Gradle (with caching) + uses: gradle/actions/setup-gradle@v4 + + - name: Make gradlew executable + run: chmod +x ./gradlew + + - name: Run SpotBugs on TeamCode + shell: bash + run: | + set -euo pipefail + + echo "Trying SpotBugs tasks for :TeamCode (Android projects differ by plugin/config)..." + + # Try a few common task names. Use the first one that exists and succeeds. + if ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugsDebug(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugsDebug + elif ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugsRelease(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugsRelease + elif ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugsMain(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugsMain + elif ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugs(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugs + else + echo "::error::No SpotBugs task found for :TeamCode. Make sure the SpotBugs Gradle plugin is applied/configured for TeamCode." + echo "Here are any spotbugs-related tasks Gradle sees:" + ./gradlew -q :TeamCode:tasks --all | grep -i spotbugs || true + exit 1 + fi + + - name: Upload SpotBugs reports + if: always() + uses: actions/upload-artifact@v4 + with: + name: spotbugs-teamcode-reports + if-no-files-found: warn + path: | + TeamCode/build/reports/spotbugs/** + **/build/reports/spotbugs/** + From 0eb5e067cfe7d5dc01a0b64157a8c5335a0a8aaf Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 22 Dec 2025 00:22:35 +0000 Subject: [PATCH 64/67] Gradle changes for SpotBug --- TeamCode/build.gradle | 54 ++++++++++++++++++++++++++++++++++++++++++- build.gradle | 5 ++++ 2 files changed, 58 insertions(+), 1 deletion(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 01b5e675538e..e220e14f2f8f 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -14,6 +14,7 @@ // Include common definitions from above. apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' +apply plugin: "com.github.spotbugs" android { namespace = 'org.firstinspires.ftc.teamcode' @@ -48,4 +49,55 @@ dependencies { implementation "com.acmerobotics.roadrunner:core:1.0.1" implementation "com.acmerobotics.roadrunner:actions:1.0.1" implementation "com.acmerobotics.dashboard:dashboard:0.5.1" -} \ No newline at end of file +} + +import com.github.spotbugs.snom.SpotBugsTask + +// Create a stable task name we can call from CI: :TeamCode:spotbugsTeamCodeDebug +afterEvaluate { + def variants = + (project.plugins.hasPlugin("com.android.library") && android.hasProperty("libraryVariants")) ? android.libraryVariants : + (project.plugins.hasPlugin("com.android.application") && android.hasProperty("applicationVariants")) ? android.applicationVariants : + null + + if (variants == null) { + logger.lifecycle("SpotBugs: No Android variants found in :TeamCode (is it an Android module?)") + return + } + + variants.all { variant -> + if (variant.name != "debug") return + + def javaCompile = variant.hasProperty("javaCompileProvider") ? variant.javaCompileProvider.get() : variant.javaCompile + + // AGP versions differ on where compiled classes live: + def classesDir = javaCompile.hasProperty("destinationDirectory") + ? javaCompile.destinationDirectory.asFile.get() + : javaCompile.destinationDir + + tasks.register("spotbugsTeamCodeDebug", SpotBugsTask) { + group = "verification" + description = "Run SpotBugs on TeamCode (debug variant)" + dependsOn javaCompile + + // SpotBugs inputs + classes = files(classesDir) + // TeamCode Java sources (variant source sets) + sourceDirs = files(variant.sourceSets.collectMany { it.java.srcDirs }) + // Classpath is optional for many checks; keep it empty to avoid Android classpath pain. + classpath = files() + + reports.create("html") { + required = true + outputLocation = file("$buildDir/reports/spotbugs/teamcode-debug.html") + } + reports.create("xml") { + required = true + outputLocation = file("$buildDir/reports/spotbugs/teamcode-debug.xml") + } + } + + // Make `./gradlew check` run it too (optional but handy) + tasks.named("check").configure { dependsOn("spotbugsTeamCodeDebug") } + } +} diff --git a/build.gradle b/build.gradle index 118d5e459d52..f1085eadb26b 100644 --- a/build.gradle +++ b/build.gradle @@ -24,6 +24,11 @@ allprojects { } } +plugins { + id "com.github.spotbugs" version "6.4.4" apply false +} + + repositories { mavenCentral() } From 2b7b87882f7b6f7c358f3f22afbed1922aeb2902 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 22 Dec 2025 00:25:12 +0000 Subject: [PATCH 65/67] fixed position of plugin block --- build.gradle | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/build.gradle b/build.gradle index f1085eadb26b..d979eaef0b24 100644 --- a/build.gradle +++ b/build.gradle @@ -15,6 +15,10 @@ buildscript { } } +plugins { + id "com.github.spotbugs" version "6.4.4" apply false +} + // This is now required because aapt2 has to be downloaded from the // google() repository beginning with version 3.2 of the Android Gradle Plugin allprojects { @@ -24,10 +28,6 @@ allprojects { } } -plugins { - id "com.github.spotbugs" version "6.4.4" apply false -} - repositories { mavenCentral() From 3de3c1a3d58c3d431123d382e8e86d8dc6288f57 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 22 Dec 2025 00:29:30 +0000 Subject: [PATCH 66/67] Remove classpath assignment --- TeamCode/build.gradle | 2 -- 1 file changed, 2 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index e220e14f2f8f..75539453d1c5 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -84,8 +84,6 @@ afterEvaluate { classes = files(classesDir) // TeamCode Java sources (variant source sets) sourceDirs = files(variant.sourceSets.collectMany { it.java.srcDirs }) - // Classpath is optional for many checks; keep it empty to avoid Android classpath pain. - classpath = files() reports.create("html") { required = true From 62d2ec3717ef1bdc99f100abe0a1cf19fc4cc198 Mon Sep 17 00:00:00 2001 From: arnavjosh <78702246+arnavjosh@users.noreply.github.com> Date: Mon, 22 Dec 2025 00:31:27 +0000 Subject: [PATCH 67/67] Fix formatter workflow --- .github/workflows/format-java.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/format-java.yml b/.github/workflows/format-java.yml index d5841be1e3aa..40c3366e83bc 100644 --- a/.github/workflows/format-java.yml +++ b/.github/workflows/format-java.yml @@ -27,12 +27,13 @@ jobs: - name: Checkout uses: actions/checkout@v4 - - name: Set up JDK 17 + - name: Set up JDK 21 uses: actions/setup-java@v4 with: distribution: temurin - java-version: "17" - + java-version: "21" + - name: Show java version + run: java -version # This action can either “check” or “format then show diff”. # We choose “format then diff” so the PR shows exactly what would change. - name: Run google-java-format (TeamCode)