From 1c292e77c774ff04de1080efd44765cbcf1861f2 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sat, 17 Jan 2026 13:50:58 -0600 Subject: [PATCH] Working red auto apparently...blue is theoretial atp --- .../autonomous/ProtoAutoClose_V3.java | 16 +- .../ftc/teamcode/constants/Poses.java | 12 +- .../teamcode/constants/ServoPositions.java | 8 +- .../ftc/teamcode/constants/ShooterVars.java | 2 +- .../ftc/teamcode/teleop/TeleopV3.java | 193 ++++++++++++------ .../ftc/teamcode/tests/LimelightTest.java | 2 +- 6 files changed, 154 insertions(+), 79 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java index fe72189..a03f24d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java @@ -233,6 +233,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode { teleStart = drive.localizer.getPose(); + robot.intake.setPower(-0.3); + if (ticker == 1) { stamp = getRuntime(); } @@ -240,7 +242,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { robot.intake.setPower(0); - if (getRuntime() - stamp < 3) { + if (getRuntime() - stamp < 2.7) { robot.transferServo.setPosition(transferServo_in); @@ -265,6 +267,8 @@ public class ProtoAutoClose_V3 extends LinearOpMode { @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + ticker++; if (ticker == 1) { @@ -283,7 +287,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { if (getRuntime() - stamp > jamTime+0.4) { - robot.intake.setPower(0); + robot.intake.setPower(0.5); return false; } @@ -506,6 +510,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; + } double turretPID; @@ -562,6 +567,13 @@ public class ProtoAutoClose_V3 extends LinearOpMode { shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + pickup3 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx4a, by4a), bh4a) + .strafeToLinearHeading(new Vector2d(bx4b, by4b), bh4b, + new TranslationalVelConstraint(slowSpeed)); + shoot3 = drive.actionBuilder(new Pose2d(bx4b, by4b, bh4b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); } robot.turr1.setPosition(turretPID); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 12af346..d2a1382 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -15,16 +15,16 @@ public class Poses { public static Pose2d goalPose = new Pose2d(-15, 0, 0); public static double rx1 = 40, ry1 = -7, rh1 = 0; - public static double rx2a = 45, ry2a = 18, rh2a = Math.toRadians(140); - public static double rx2b = 25, ry2b = 38, rh2b = Math.toRadians(140); + public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); + public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140); public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); - public static double rx3a = 55, ry3a = 43, rh3a = Math.toRadians(140); - public static double rx3b = 37, ry3b = 61, rh3b = Math.toRadians(140); + public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); + public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140); - public static double rx4b = 50, ry4b = 77, rh4b = Math.toRadians(140); + public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140); public static double bx1 = 40, by1 = 7, bh1 = 0; public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); @@ -35,7 +35,7 @@ public class Poses { public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140); public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140); - public static double bx4b = 50, by4b = -77, bh4b = Math.toRadians(-140); + public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index d1a358e..1c3d091 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -11,10 +11,10 @@ public class ServoPositions { public static double spindexer_intakePos3 = 0.66; - public static double spindexer_outtakeBall3 = 0.42; + public static double spindexer_outtakeBall3 = 0.47; - public static double spindexer_outtakeBall2 = 0.74; - public static double spindexer_outtakeBall1 = 0.58; + public static double spindexer_outtakeBall2 = 0.31; + public static double spindexer_outtakeBall1 = 0.15; public static double transferServo_out = 0.15; @@ -24,7 +24,7 @@ public class ServoPositions { public static double hoodDefault = 0.6; - public static double hoodAuto = 0.43; + public static double hoodAuto = 0.25; public static double hoodAutoFar = 0.5; //TODO: change this; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index 237ed0c..30e5dc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -19,6 +19,6 @@ public class ShooterVars { public static double maxStep = 0.06; // prevents sudden jumps // VELOCITY CONSTANTS - public static int AUTO_CLOSE_VEL = 3000; //3300; + public static int AUTO_CLOSE_VEL = 3300; //3300; public static int AUTO_FAR_VEL = 4000; //TODO: test this } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index fee0829..37bc223 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -21,6 +21,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -45,10 +46,10 @@ public class TeleopV3 extends LinearOpMode { public static double shootStamp2 = 0.0; public static double spinningPow = 0.2; public static double spindexPos = spindexer_intakePos1; - public static double spinPow = 0.08; - public static double bMult = -1, bDiv = 130000; + public static double spinPow = 0.09; + public static double bMult = 1, bDiv = 2200; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; - public static boolean manualTurret = false; + public static boolean manualTurret = true; public double vel = 3000; public boolean autoVel = true; public double manualOffset = 0.0; @@ -146,8 +147,6 @@ public class TeleopV3 extends LinearOpMode { tController.setTolerance(0.001); - - if (redAlliance) { robot.limelight.pipelineSwitch(3); } else { @@ -234,31 +233,24 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.right_bumper) { - robot.transferServo.setPosition(transferServo_out); intakeTicker++; - if (intakeTicker % 4 == 0) { - spinCurrentPos = servo.getSpinPos(); - if (Math.abs(spinCurrentPos - spinInitPos) < 0.02) { - reverse = true; - } else { - reverse = false; - } - spinInitPos = spinCurrentPos; - } - - if (intakeTicker % 12 < 3) { + if (intakeTicker % 20 < 2) { robot.spin1.setPower(-1); robot.spin2.setPower(1); - } else if (reverse) { + } else if (intakeTicker % 20 < 10) { + robot.spin1.setPower(-0.5); + robot.spin2.setPower(0.5); + } else if (intakeTicker % 20 < 12) { robot.spin1.setPower(1); robot.spin2.setPower(-1); } else { - robot.spin1.setPower(-spinningPow); - robot.spin2.setPower(spinningPow); + robot.spin1.setPower(0.5); + robot.spin2.setPower(-0.5); } + robot.intake.setPower(1); intakeStamp = getRuntime(); TELE.addData("Reverse?", reverse); @@ -386,7 +378,7 @@ public class TeleopV3 extends LinearOpMode { desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; - desiredTurretAngle += manualOffset; + desiredTurretAngle += manualOffset + error; offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); @@ -406,7 +398,6 @@ public class TeleopV3 extends LinearOpMode { pos = 0.83; } - //SHOOTER: double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); @@ -440,67 +431,56 @@ public class TeleopV3 extends LinearOpMode { //TODO: test the camera teleop code - TELE.addData("posS2", pos); -// if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving -// double bearing; -// -// LLResult result = robot.limelight.getLatestResult(); -// if (result != null) { -// if (result.isValid()) { -// bearing = result.getTx() * bMult; -// overrideTurr = true; -// -// double bearingCorrection = bearing / bDiv; -// -// // deadband: ignore tiny noise -// if (Math.abs(bearing) > 0.3 && camTicker < 8) { -// -// error += bearingCorrection; -// -// } -// camTicker++; -// TELE.addData("tx", bearing); -// TELE.addData("ty", result.getTy()); -// } -// } -// -// } else { -// camTicker = 0; -// overrideTurr = false; -// } + if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving + double bearing; + + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + bearing = result.getTx() * bMult; + + double bearingCorrection = bearing / bDiv; + + error += bearingCorrection; + + camTicker++; + TELE.addData("tx", bearingCorrection); + TELE.addData("ty", result.getTy()); + } + } + + } else { + camTicker = 0; + overrideTurr = false; + } if (!overrideTurr) { turretPos = pos; } - - - TELE.addData("posS3", turretPos); if (manualTurret) { - pos = turrDefault + (manualOffset / 100); + pos = turrDefault + (manualOffset / 100) + error; } if (!overrideTurr) { turretPos = pos; } - if (gamepad2.dpad_right || gamepad1.dpad_right) { - manualOffset -= 2; - } else if (gamepad2.dpad_left || gamepad1.dpad_left) { - manualOffset += 2; + if (Math.abs(gamepad2.left_stick_x)>0.2) { + manualOffset += 1.35 * gamepad2.left_stick_x; } robot.turr1.setPosition(pos); - robot.turr2.setPosition(1-pos); + robot.turr2.setPosition(1 - pos); //HOOD: if (autoHood) { - robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset); + robot.hood.setPosition(0.15 + hoodOffset); } else { robot.hood.setPosition(hoodDefaultPos + hoodOffset); } @@ -518,7 +498,7 @@ public class TeleopV3 extends LinearOpMode { if (gamepad2.cross) { manualOffset = 0; - fixedTurret = true; + overrideTurr = true; } if (gamepad2.squareWasPressed()) { @@ -552,21 +532,40 @@ public class TeleopV3 extends LinearOpMode { // } // } - if (gamepad1.leftBumperWasPressed()) { + if (gamepad1.left_bumper) { robot.transferServo.setPosition(transferServo_out); autoSpintake = false; - robot.spin1.setPower(1); - robot.spin2.setPower(-1); + intakeTicker++; + + if (intakeTicker % 10 < 1) { + + robot.spin1.setPower(-1); + robot.spin2.setPower(1); + + } else if (intakeTicker % 10 < 5) { + robot.spin1.setPower(-0.5); + robot.spin2.setPower(0.5); + } else if (intakeTicker % 10 < 6) { + robot.spin1.setPower(1); + robot.spin2.setPower(-1); + } else { + robot.spin1.setPower(0.5); + robot.spin2.setPower(-0.5); + } + + intake = false; + reject = false; + + robot.intake.setPower(0.5); } if (gamepad1.leftBumperWasReleased()) { shootStamp = getRuntime(); shootAll = true; - spindexPos = spindexer_intakePos1; shooterTicker = 0; } @@ -603,6 +602,70 @@ public class TeleopV3 extends LinearOpMode { } } +// +// if (shootAll) { +// +// TELE.addData("100% works", shootOrder); +// +// intake = false; +// reject = false; +// +// shooterTicker++; +// +// spindexPos = spindexer_intakePos1; +// +// if (getRuntime() - shootStamp < 1) { +// +// if (servo.spinEqual(spindexer_outtakeBall3) || ((getRuntime()-shootStamp)>0.4)){ +// robot.transferServo.setPosition(transferServo_in); +// +// } else { +// robot.transferServo.setPosition(transferServo_out); +// +// } +// +// +// autoSpintake = true; +// +// spindexPos = spindexer_outtakeBall3; +// robot.intake.setPower(0.5); +// +// } +// +// else if (getRuntime() - shootStamp < 1.8) { +// +// robot.transferServo.setPosition(transferServo_in); +// +// autoSpintake = true; +// robot.intake.setPower(0); +// +// spindexPos = spindexer_outtakeBall2; +// +// } +// else if (getRuntime() - shootStamp < 2.6) { +// +// robot.transferServo.setPosition(transferServo_in); +// +// autoSpintake = false; +// +// robot.spin1.setPower(1); +// robot.spin2.setPower(-1); +// +// } +// +// else { +// robot.transferServo.setPosition(transferServo_out); +// spindexPos = spindexer_intakePos1; +// +// shootAll = false; +// +// autoSpintake = true; +// +// robot.transferServo.setPosition(transferServo_out); +// } +// +// } + // if (gamepad1.squareWasPressed()) { // square = true; // shootStamp = getRuntime(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index d79a4f4..2ab1b16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -20,7 +20,7 @@ public class LimelightTest extends LinearOpMode { public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track @Override public void runOpMode() throws InterruptedException { - Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight"); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); limelight.pipelineSwitch(pipeline); waitForStart();