From 9d29e0b56c1cc21611ff21f31962b1bc4abde2bd Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 4 Jun 2026 22:13:49 -0500 Subject: [PATCH] tweaks were made and hardware changes are needed --- .../ftc/teamcode/teleop/TeleopV4.java | 24 ++++++++++---- .../ftc/teamcode/tests/NewShooterTest.java | 33 ++++++++++++++----- .../ftc/teamcode/utilsv2/Shooter.java | 4 +++ .../ftc/teamcode/utilsv2/Turret.java | 23 ++++++------- 4 files changed, 57 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 87ba8a9..af8bbca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -55,6 +55,7 @@ public class TeleopV4 extends LinearOpMode { turret = new Turret(robot); parkTilter = new ParkTilter(robot); + parkTilter.unpark(); shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.TRACK_GOAL); @@ -65,7 +66,7 @@ public class TeleopV4 extends LinearOpMode { turret.switchPipeline(Turret.PipelineMode.TRACKING); robot.limelight.start(); - limelightUsed = false; + limelightUsed = true; waitForStart(); @@ -86,6 +87,20 @@ public class TeleopV4 extends LinearOpMode { follower.update(); + if (gamepad1.dpadLeftWasPressed()){ + shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); + } + + if (gamepad1.dpadRightWasPressed()){ + shooter.setState(Shooter.ShooterState.TRACK_GOAL); + } + + if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){ + shooter.setFlywheelVelocity(2500); + robot.setHoodPos(0.6); + robot.setTransferPower(-0.8); + } + shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); @@ -110,12 +125,9 @@ public class TeleopV4 extends LinearOpMode { SpindexerTransferIntake.RapidMode.HOLD_BALLS ); } - if (gamepad1.rightBumperWasPressed() - && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { - spindexerTransferIntake.setRapidMode( - SpindexerTransferIntake.RapidMode.INTAKE - ); + if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) { + robot.setIntakePower(1); } if (gamepad2.leftBumperWasPressed()){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index 39474b0..0830331 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -15,6 +15,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.teleop.TeleopV4; import org.firstinspires.ftc.teamcode.utilsv2.*; @TeleOp @@ -57,6 +58,7 @@ public class NewShooterTest extends LinearOpMode { turret = new Turret(robot); parkTilter = new ParkTilter(robot); + parkTilter.unpark(); shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); @@ -81,10 +83,28 @@ public class NewShooterTest extends LinearOpMode { gamepad1.left_stick_x ); + if (gamepad1.crossWasPressed()){ + follower.setPose(TeleopV4.relocalizePose); + gamepad1.rumble(100); + } + follower.update(); - shooter.setFlywheelVelocity(flywheelVelo); - robot.setHoodPos(hoodPos); + if (gamepad1.dpadLeftWasPressed()){ + shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); + } + + if (gamepad1.dpadRightWasPressed()){ + shooter.setState(Shooter.ShooterState.TRACK_GOAL); + } + + if (shooter.getState() == Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR || shooter.getState() == Shooter.ShooterState.MANUAL){ + shooter.setFlywheelVelocity(flywheelVelo); + robot.setHoodPos(hoodPos); + robot.setTransferPower(transferPower); + } + + // shooter.setTurretPosition(turretPos); shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); @@ -116,12 +136,8 @@ public class NewShooterTest extends LinearOpMode { SpindexerTransferIntake.RapidMode.HOLD_BALLS ); } - if (gamepad1.rightBumperWasPressed() - && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { - - spindexerTransferIntake.setRapidMode( - SpindexerTransferIntake.RapidMode.INTAKE - ); + if (gamepad1.right_bumper && state != SpindexerTransferIntake.RapidMode.OPEN_GATE && state != SpindexerTransferIntake.RapidMode.SHOOT) { + robot.setIntakePower(1); } if (gamepad1.dpad_down){ @@ -135,6 +151,7 @@ public class NewShooterTest extends LinearOpMode { TELE.addData("Transfer Power", commander.getTransferPow()); TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + TELE.addData("TX:", turret.getTX()); TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 3d39960..78a244c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -71,6 +71,10 @@ public class Shooter { this.state = shooterState; } + public ShooterState getState(){ + return state; + } + public void setTurretPosition(double input) { this.turretPosition = input; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index 06562e2..9ee6419 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -1,12 +1,10 @@ package org.firstinspires.ftc.teamcode.utilsv2; -import static java.lang.Math.abs; - import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.util.Range; @@ -23,18 +21,18 @@ public class Turret { private final double turretMin = 0.05; private final double turretMax = 0.95; public static boolean limelightUsed = true; - public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007; + public static double B_PID_P = 0.0003, B_PID_I = 0.0, B_PID_D = 0.00003; LLResult result; PIDController bearingPID; boolean bearingAligned = false; - private boolean lockOffset = false; public int LL_COAST_TICKS = 60; public static double TARGET_POSITION_TOLERANCE = 0.5; public static double alphaTX = 0.5; private double targetTx = 0; - private double bearingOffset = 0; + private double currentTrackOffset = 0; + private double llCoast = 0; + private double servoAngle = 0.51; double tx = 0.0; - double ty = 0.0; private final double hVelK = 0; // TODO: Tune private final double xVelK = 0; // TODO: Tune private final double xAccK = 0; // TODO: Tune @@ -67,6 +65,8 @@ public class Turret { } } + public double getTX(){return tx;} + public enum PipelineMode{ OBELISK, TRACKING @@ -137,13 +137,12 @@ public class Turret { robot.setTurretPos(pos); } - private double currentTrackOffset = 0; - private double llCoast = 0; - private double servoAngle = 0.51; public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) { // dx, dy, dz is target - robot // h is the raw heading where 0 degrees is positive x in the system of x, y + bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); // Keep when debugging/tuning, comment out when doing teleop + double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading @@ -162,7 +161,7 @@ public class Turret { targetTx = (tx*alphaTX)+(targetTx*(1-alphaTX)); bearingAligned = Math.abs(targetTx) < TARGET_POSITION_TOLERANCE; if (!bearingAligned){ - bearingOffset = -(bearingPID.calculate(targetTx, 0.0)); + bearingOffset = (bearingPID.calculate(targetTx, 0.0)); } } else { targetTx = 0; @@ -178,8 +177,6 @@ public class Turret { } } - currentTrackOffset += bearingOffset; - double servoTicksFromNeutral = (angleDelta+currentTrackOffset) * (2.0 * servoTicksPer180); servoAngle = neutralPosition + servoTicksFromNeutral;