From b89cca22af8ef68ff504d426979e46721774c8bc Mon Sep 17 00:00:00 2001 From: sanjeevgedela <145948668+sanjeevgedela@users.noreply.github.com> Date: Sat, 14 Feb 2026 01:22:52 -0600 Subject: [PATCH] added redtele, minor tweaks on TurretLock --- .../ftc/teamcode/DriverControlsV1.java | 9 +- .../ftc/teamcode/DriverControlsV1Red.java | 198 ++++++++++++++++++ .../ftc/teamcode/TeleOpTurretLock.java | 2 +- .../ftc/teamcode/TurretLock.java | 55 +++-- 4 files changed, 242 insertions(+), 22 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java index 08f8640..7c17aa5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java @@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PE import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.fasterxml.jackson.databind.node.POJONode; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -15,9 +16,10 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; @Config -@TeleOp +@TeleOp (name = "TeleOp BLUE") public class DriverControlsV1 extends LinearOpMode { enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET } Limelight3A limelight; @@ -37,7 +39,8 @@ public class DriverControlsV1 extends LinearOpMode { Boolean shooting = false; SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - drive.setPoseEstimate(new Pose2d(0,0,0)); + drive.setPoseEstimate(PoseStorage.currentPose); + drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose); Pose2d driveDirection; @@ -49,7 +52,7 @@ public class DriverControlsV1 extends LinearOpMode { PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14); drive.setPoseEstimate(new Pose2d(0, 0, 0)); - TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry); + TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java new file mode 100644 index 0000000..06e1d5b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java @@ -0,0 +1,198 @@ +package org.firstinspires.ftc.teamcode; + +import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +@Config +@TeleOp (name = "TeleOp RED") +public class DriverControlsV1Red extends LinearOpMode { + enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET } + Limelight3A limelight; + LLResult result = null; + public static double visionMult = 1.5; + + @Override + public void runOpMode() throws InterruptedException { + Hware robot = new Hware(); + robot.initialize(hardwareMap); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + + double SPEEDCONTROL = 1; + + Boolean adjust = false; + Boolean shooting = false; + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + drive.setPoseEstimate(new Pose2d(0,0,0)); + + Pose2d driveDirection; + + ShooterSubsystem shooterMap = new ShooterSubsystem(); + + ShootState shootStatus = ShootState.IDLE; + ElapsedTime shootTimer = new ElapsedTime(); + + PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14); + + drive.setPoseEstimate(new Pose2d(0, 0, 0)); + TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED); + + waitForStart(); + + limelight.pipelineSwitch(1); + limelight.start(); + + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); + robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + while (opModeIsActive()) { + robot.leftPTO.setPosition(0.2); + robot.rightPTO.setPosition(0.2); + + double currentVoltage = robot.batteryVoltageSensor.getVoltage(); + if (currentVoltage > 13.5) { + pidf = new PIDFCoefficients(3, 0, 0, 13.8 + ); + } else if (currentVoltage > 12.7) { + pidf = new PIDFCoefficients(3, 0, 0, 14); + } else if (currentVoltage > 11.2) { + pidf = new PIDFCoefficients(3, 0, 0, 14.5 ); + } else { + pidf = new PIDFCoefficients(3, 0, 0, 14 ); + + } + Pose2d myPose = drive.getPoseEstimate(); + double turrPos = robot.turret.getCurrentPosition(); + + result = limelight.getLatestResult(); + double vision = result.getTx() * visionMult; +// if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){ +// drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos))); +// } + double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger); + if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg); + if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim +// telemetry.update();\ +// + double targetTicks = turretLock.update(myPose, result, vision, turrPos); + robot.turret.setTargetPosition((int) targetTicks); + robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.turret.setPower(1); + + if (gamepad1.right_bumper) { + SPEEDCONTROL= 0.5; + } + if (!shooting) { + if (gamepad1.right_trigger > 0.3) { + robot.intake.set(0.8); + robot.activeTransfer.setPosition(0.7); + } else if (gamepad1.left_trigger > 0.5) { + robot.intake.set(-0.8); + robot.activeTransfer.setPosition(0.7); + } else { + robot.intake.set(0); + robot.activeTransfer.setPosition(0.7); + } + } + + driveDirection = new Pose2d( + -gamepad1.left_stick_y * SPEEDCONTROL, + -gamepad1.left_stick_x * SPEEDCONTROL, + -gamepad1.right_stick_x * SPEEDCONTROL + ); + + // Arm + + + // 2. Get Targets based on current X, Y + double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY()); + double targetVelocity = targets[0]; + double targetHood = targets[1]; + + + if (shooting) { + robot.launchHood.setPosition(targetHood); + } + + if(gamepad2.dpadUpWasPressed()) { + adjust = true; + } else if (gamepad2.dpadDownWasPressed()){ + adjust = false; + } + + if (adjust) { + robot.bottomFly.setVelocity(targetVelocity); + robot.topFly.setVelocity(targetVelocity); + } else { + robot.bottomFly.setVelocity(0); + robot.topFly.setVelocity(0); + } + + + // 1. TRIGGER: Start the sequence + if (gamepad2.xWasPressed()) { + shooting = true; + shootStatus = ShootState.START_SHOOT; + shootTimer.reset(); + } + + switch (shootStatus) { + case START_SHOOT: + // Initial actions + robot.activeTransfer.setPosition(0.95); + robot.intake.set(0.55); + robot.launchHood.setPosition(targetHood); + + // Wait 250ms for the shot to physically fire before moving the hood + if (shootTimer.milliseconds() > 100) { + shootStatus = ShootState.LOWER_HOOD; + } + break; + + case LOWER_HOOD: + targetHood = Math.max(0.34, targetHood - 0.2); + targetVelocity = targetVelocity + 100; + robot.launchHood.setPosition(targetHood); + robot.bottomFly.setVelocity(targetVelocity); + robot.topFly.setVelocity(targetVelocity); + shootStatus = ShootState.RESET; + break; + + case RESET: + if (shootTimer.milliseconds() > 3000) { + shootStatus = ShootState.IDLE; + shooting = false; + } + break; + } + + if (gamepad2.yWasPressed()) { + shooting = false; + robot.activeTransfer.setPosition(.7); + } + + drive.setWeightedDrivePower(driveDirection); + drive.update(); + + telemetry.addData("Vision", vision); + telemetry.addData("Voltage Sensor", currentVoltage); + telemetry.update(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java index f55231e..137b222 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java @@ -70,7 +70,7 @@ public class TeleOpTurretLock extends LinearOpMode { // Start pose (you said 0,0) drive.setPoseEstimate(new Pose2d(0, 0, 0)); - TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry); + TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE); // limelight diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java index e9ca896..d86955c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java @@ -19,7 +19,7 @@ import java.util.List; public class TurretLock { - public enum MODE{ + public enum MODE { LIME, ODOMETRY } @@ -40,7 +40,7 @@ public class TurretLock { // Mechanical limits (ticks). Keep your existing numbers. public int minTicks = (int) (ticksPerDegree * (-90)); - public int maxTicks = (int) (ticksPerDegree * ( 90 )); + public int maxTicks = (int) (ticksPerDegree * (90)); public double finalDeg = 0; @@ -50,6 +50,11 @@ public class TurretLock { public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle public static double cameraBearingEqual = 0.5; // Deadband + public enum SIDE { + RED, + BLUE + } + // TODO: tune these values for limelight Hware robot; Limelight3A webcam; @@ -76,25 +81,35 @@ public class TurretLock { Telemetry tele; - // private final ElapsedTime tagTimer = new ElapsedTime(); + // private final ElapsedTime tagTimer = new ElapsedTime(); - public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele) { + public TurretLock(DcMotorEx turretMotor, Limelight3A cam, SIDE side) { this.turret = turretMotor; this.webcam = cam; - this.tele = tele; + switch (side) { + case RED: + goalX = -75; + goalY = -75; + break; + case BLUE: + goalX = 75; + goalY = 75; + break; + } } public double getTurrPos() { return turret.getCurrentPosition(); } - public void addManualTrimDeg(double deltaDeg){ + public void addManualTrimDeg(double deltaDeg) { manualTrimDeg += deltaDeg; } - public void resetManualTrim(){ + public void resetManualTrim() { manualTrimDeg = 0; } + private void limelightRead(LLResult result) { // only for tracking purposes, not general reads if (result != null) { if (result.isValid()) { @@ -112,7 +127,7 @@ public class TurretLock { } public double getBearing() { - // tx = 1000; + // tx = 1000; LLResult result = webcam.getLatestResult(); return result.getBotpose().getOrientation().getYaw(); } @@ -151,23 +166,25 @@ public class TurretLock { private double bearingAlign(LLResult llResult) { double targetTx = 0; - if(llResult != null && llResult.isValid()){ + if (llResult != null && llResult.isValid()) { targetTx = llResult.getTx(); // How far left or right the target is (degrees) } return llResult.getBotpose().getOrientation().getYaw(); } - public void Init(){ + public void Init() { pid = new PersonalPID(p, i, d, f); } - public String getStatus(){ + public String getStatus() { return String.valueOf(webcam.getStatus()); } - /** Main update: call every loop. */ + /** + * Main update: call every loop. + */ public double update(Pose2d robotPose, LLResult result, double visionCorrect, double turrPos) { // 1) ODOM: compute turret angle needed to face goal (in degrees) double dx = goalX - robotPose.getX(); @@ -191,7 +208,7 @@ public class TurretLock { finalDeg = manualTrimDeg + turretCmdDeg; limelightRead(result); filteredVisionTrimDeg = -visionCorrect; - switch (TYPE){ + switch (TYPE) { case LIME: finalDeg = manualTrimDeg + turrPos / ticksPerDegree + filteredVisionTrimDeg; if (result != null && (finalDeg > 20 || finalDeg < -20)) { @@ -221,13 +238,13 @@ public class TurretLock { // turret.setTargetPosition(targetTicks); // turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); // turret.setPower(1.0); - } + } public double updateHeading(Pose2d robotPose, LLResult result, double turrPos) { double robotHeadingDeg = Math.toDegrees(robotPose.getHeading()); - if(result.getTx() < 1 || result.getTy() > -1){ - robotHeadingDeg = - (turrPos / ticksPerDegree - 45); + if (result.getTx() < 1 || result.getTy() > -1) { + robotHeadingDeg = -(turrPos / ticksPerDegree - 45); } return robotHeadingDeg; @@ -237,11 +254,13 @@ public class TurretLock { return TYPE; } - public double getFinalDeg(){ + public double getFinalDeg() { return finalDeg; } - /** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */ + /** + * Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. + */ private static double normalizeDeg(double deg) { while (deg > 180) deg -= 360;