ripark2/FTC-Code
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

a

Browse Source
This commit is contained in:
Anirudh Narayanan
2026-02-19 19:52:09 -06:00
parent 045ef98262
commit 3319e934c1
3 changed files with 308 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

303
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java
View File

@@ -1,110 +1,285 @@
package org.firstinspires.ftc.teamcode.Auton;
package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
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.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config
@Autonomous(name = "Clean Red Depot")
@Autonomous(name = "Blue Depot Cycle")
public class redDepot extends LinearOpMode {
Hware robot = new Hware();
public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
// --- Action Helpers ---
public void setTurret(int degrees) {
robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE));
//turret
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
public static double target = 0;
public static double ticksperdegree = 9.738888 * 0.29969;
public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (75));
PersonalPID pid;
Limelight3A limelight;
LLResult result;
boolean lock = true;
public void turretPower(double power) {
robot.turret.setPower(power);
}
public void turretPos(int position) {
turretPower(0);
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.turret.setPower(1.0);
turretPower(1);
}
public void startShooter(double velocity) {
robot.topFly.setVelocity(velocity);
robot.bottomFly.setVelocity(velocity);
robot.launchHood.setPosition(0.54);
public void spintakeStart(){
robot.intake.set(1);
}
public void stopEverything() {
// robot.intake.set(0);
// robot.topFly.setVelocity(0);
// robot.bottomFly.setVelocity(0);
// robot.activeTransfer.setPosition(0.8);
public void spintakeEnd(){
robot.intake.set(0);
}
@Override
public void shoot(){
robot.topFly.setVelocity(1800);
robot.bottomFly.setVelocity(1800);
robot.launchHood.setPosition(0.55);
}
public void reset(){
robot.intake.set(0);
// robot.topFly.setVelocity(1200);
// robot.bottomFly.setVelocity(1200);
// robot.topFly.set(0);
// robot.bottomFly.set(0);
// robot.launchHood.setPosition(0.6);
robot.activeTransfer.setPosition(1);
}
public void runOpMode() throws InterruptedException {
//webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
pid = new PersonalPID(p, i, d, f);
limelight.pipelineSwitch(1);
limelight.start();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
pid = new PersonalPID(p, i, d, f);
Pose2d startPose = new Pose2d(54, -46, Math.toRadians(-52));
Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205));
drive.setPoseEstimate(startPose);
// --- TRAJECTORY 1: Initial Score ---
TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose)
.addDisplacementMarker(() -> {
setTurret(-5);
startShooter(1660);
robot.intake.set(.8);
TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
// turretPos(5);
})
.strafeRight(35)
.addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
.UNSTABLE_addTemporalMarkerOffset(.3, () -> {
shoot();
})
.back(47)
.waitSeconds(0.5)
.UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
})
// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
// robot.launchHood.setPosition(0.5);
// })
.waitSeconds(1.5)
.addDisplacementMarker(this::stopEverything)
.build();
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
// --- TRAJECTORY 2: Intake 6-Ball ---
TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end())
.lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265)))
.addDisplacementMarker(() -> robot.intake.set(1.0))
.forward(35)
.addDisplacementMarker(() -> robot.intake.set(0))
.build();
//6ball
.lineToLinearHeading(new Pose2d(8,-28, Math.toRadians(-90)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
shoot();
})
.lineToLinearHeading(new Pose2d(24,-14, Math.toRadians(-52)))
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
})
// --- TRAJECTORY 3: Score 6-Ball ---
TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end())
.lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
.addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through
.addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
// robot.launchHood.setPosition(0.5);
// })
.waitSeconds(1.5)
.addDisplacementMarker(this::stopEverything)
.build();
// --- TRAJECTORY 4: Intake 9-Ball ---
TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end())
.lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265)))
.addDisplacementMarker(() -> robot.intake.set(1.0))
.forward(35)
.addDisplacementMarker(() -> robot.intake.set(0))
.build();
// --- TRAJECTORY 5: Score 9-Ball & Park ---
TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end())
.lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
.addDisplacementMarker(() -> {
startShooter(1660);
robot.activeTransfer.setPosition(1.0);
//9ball
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-19,-27, Math.toRadians(-90)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(-12,-54, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(26,-14, Math.toRadians(-52)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.85);
})
.waitSeconds(1.5)
.addDisplacementMarker(this::stopEverything)
.lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90)))
//12ball
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-40,-22, Math.toRadians(-90)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(35,-14, Math.toRadians(-52)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
shoot();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.8);
robot.activeTransfer.setPosition(0.85);
})
.waitSeconds(1.5)
// .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
// turretPos(84);
// })
.waitSeconds(1)
.build();
// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
// .strafeLeft(25)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot();
// })
// .waitSeconds(1)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot();
// })
// .waitSeconds(1)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .forward(10)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .strafeLeft(10)
// .forward(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot();
// })
// .waitSeconds(1)
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart();
// })
// .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot();
// })
// .waitSeconds(1)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// })
// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .build();
waitForStart();
if (opModeIsActive() && !isStopRequested()) {
// while (opModeIsActive()) {
drive.followTrajectorySequenceAsync(threeBall);
PoseStorage.currentPose = drive.getPoseEstimate();
if (isStopRequested()) return;
while(opModeIsActive()){
robot.leftPTO.setPosition(0.2);
robot.rightPTO.setPosition(0.14);
drive.update();
result = limelight.getLatestResult();
double vision = result.getTx() + 3;
// Execute sequentially
drive.followTrajectorySequence(traj1);
drive.followTrajectorySequence(traj2);
drive.followTrajectorySequence(traj3);
drive.followTrajectorySequence(traj4);
drive.followTrajectorySequence(traj5);
double target = ticksperdegree * vision;
double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
pid.setPIDF(p, i, d, f);
if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
targetTicks = targetTicks;
} else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
targetTicks = targetTicks;
} else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
targetTicks = 0;
}
PoseStorage.currentPose = drive.getPoseEstimate();
robot.turret.setPower(- targetTicks);
}
// }
}
}
}

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Blue.java
View File

@@ -18,7 +18,7 @@ import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config
@TeleOp(name = "TeleOp BLUE - Final Optimized")
public class DriverControlsV1 extends LinearOpMode {
public class DriverControlsV1Blue extends LinearOpMode {
// --- DASHBOARD TUNING ---
public static double flywheelTuner = 1.0;
@@ -34,7 +34,7 @@ public class DriverControlsV1 extends LinearOpMode {
private double lastTuner = -1.0;
private double lastPTOPosition = -1.0;
enum ShootState { IDLE, START_SHOOT, RESET }
enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR }
//turret
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
@@ -44,6 +44,7 @@ public class DriverControlsV1 extends LinearOpMode {
public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (75));
public int offsetCam = 3;
PersonalPID pid;
Limelight3A limelight;
LLResult result;
@@ -81,7 +82,7 @@ public class DriverControlsV1 extends LinearOpMode {
while (opModeIsActive()) {
result = limelight.getLatestResult();
double vision = result.getTx()-3;
double vision = result.getTx()-offsetCam;
// 1. INPUTS & TURRET
// if (gamepad2.yWasPressed()) {
@@ -178,6 +179,12 @@ public class DriverControlsV1 extends LinearOpMode {
shootTimer.reset();
}
if (gamepad2.dpadLeftWasPressed()) {
shooting = true;
shootStatus = ShootState.START_SHOOT_FAR;
shootTimer.reset();
}
if (gamepad2.yWasPressed()) {
flywheelTuner+=.01;
}
@@ -186,6 +193,12 @@ public class DriverControlsV1 extends LinearOpMode {
flywheelTuner-=.01;
}
if (gamepad2.xWasPressed()) {
offsetCam = 0;
} else if (gamepad2.bWasPressed()) {
offsetCam = 3;
}
switch (shootStatus) {
case START_SHOOT:
robot.activeTransfer.setPosition(0.85);
@@ -195,6 +208,16 @@ public class DriverControlsV1 extends LinearOpMode {
shootStatus = ShootState.RESET;
}
break;
case START_SHOOT_FAR:
robot.activeTransfer.setPosition(0.85);
robot.intake.set(.6);
targetTicks = 0;
if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
}
break;
case RESET:
if (shootTimer.milliseconds() > 2500) {
shootStatus = ShootState.IDLE;

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java
View File

@@ -17,7 +17,7 @@ import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config
@TeleOp(name = "TeleOp RED - Final Optimized")
@TeleOp(name = "TeleOp BLUE - Final Optimized")
public class DriverControlsV1Red extends LinearOpMode {
// --- DASHBOARD TUNING ---
@@ -27,14 +27,14 @@ public class DriverControlsV1Red extends LinearOpMode {
public static double HOOD_MIN = 0.3;
public static double HOOD_MAX = 1.0;
public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
public static double HOOD_CORRECTION_SENSITIVITY = 0.01;
// -------------------------
// State tracking to prevent redundant hardware writes (The Fix)
private double lastTuner = -1.0;
private double lastPTOPosition = -1.0;
enum ShootState { IDLE, START_SHOOT, RESET }
enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR }
//turret
public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
@@ -44,6 +44,7 @@ public class DriverControlsV1Red extends LinearOpMode {
public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (75));
public int offsetCam = 3;
PersonalPID pid;
Limelight3A limelight;
LLResult result;
@@ -58,7 +59,7 @@ public class DriverControlsV1Red extends LinearOpMode {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
ShooterSubsystemRed shooterMap = new ShooterSubsystemRed();
ShooterSubsystem shooterMap = new ShooterSubsystem();
ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime();
@@ -81,7 +82,7 @@ public class DriverControlsV1Red extends LinearOpMode {
while (opModeIsActive()) {
result = limelight.getLatestResult();
double vision = result.getTx();
double vision = result.getTx()+offsetCam;
// 1. INPUTS & TURRET
// if (gamepad2.yWasPressed()) {
@@ -104,7 +105,7 @@ public class DriverControlsV1Red extends LinearOpMode {
// 2. PTO OPTIMIZATION (Only write if value is new)
double targetPTO = 0.2;
double targetPTO = 0.15;
if (targetPTO != lastPTOPosition) {
robot.leftPTO.setPosition(targetPTO);
robot.rightPTO.setPosition(targetPTO);
@@ -156,7 +157,7 @@ public class DriverControlsV1Red extends LinearOpMode {
double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
double velocityError = targetVelocity - currentVel;
double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY;
double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
if (gamepad2.dpad_up) adjust = true;
@@ -167,8 +168,8 @@ public class DriverControlsV1Red extends LinearOpMode {
robot.topFly.setVelocity(targetVelocity);
robot.launchHood.setPosition(correctedHood);
} else {
robot.bottomFly.setVelocity(0);
robot.topFly.setVelocity(0);
robot.bottomFly.setVelocity(1700);
robot.topFly.setVelocity(1700);
}
// 7. SHOOTING STATE MACHINE
@@ -178,15 +179,45 @@ public class DriverControlsV1Red extends LinearOpMode {
shootTimer.reset();
}
if (gamepad2.dpadLeftWasPressed()) {
shooting = true;
shootStatus = ShootState.START_SHOOT_FAR;
shootTimer.reset();
}
if (gamepad2.yWasPressed()) {
flywheelTuner+=.01;
}
if (gamepad2.aWasPressed()) {
flywheelTuner-=.01;
}
if (gamepad2.xWasPressed()) {
offsetCam = 0;
} else if (gamepad2.bWasPressed()) {
offsetCam = 3;
}
switch (shootStatus) {
case START_SHOOT:
robot.activeTransfer.setPosition(0.81);
robot.intake.set(1.0);
robot.activeTransfer.setPosition(0.85);
robot.intake.set(1);
targetTicks = 0;
if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
}
break;
case START_SHOOT_FAR:
robot.activeTransfer.setPosition(0.85);
robot.intake.set(.6);
targetTicks = 0;
if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
}
break;
case RESET:
if (shootTimer.milliseconds() > 2500) {
shootStatus = ShootState.IDLE;
@@ -219,6 +250,7 @@ public class DriverControlsV1Red extends LinearOpMode {
telemetry.addData("Status", "Running");
telemetry.addData("Vel Error", (int)velocityError);
telemetry.addData("Hood", "%.2f", correctedHood);
telemetry.addData("TunerMult", flywheelTuner);
telemetry.update();
}
}