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

2/11 changes

Browse Source
This commit is contained in:
Plano East Robotics
2026-02-18 18:30:20 -06:00
parent b89cca22af
commit 97f5332c71
16 changed files with 1208 additions and 947 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java
View File

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@@ -15,6 +16,7 @@ public class AutoShotTeleOp extends LinearOpMode {
@Override @Override
public void runOpMode() { public void runOpMode() {
// Initialization
drive = new SampleMecanumDrive(hardwareMap); drive = new SampleMecanumDrive(hardwareMap);
robot = new Hware(); robot = new Hware();
robot.initialize(hardwareMap); robot.initialize(hardwareMap);
@@ -27,25 +29,48 @@ public class AutoShotTeleOp extends LinearOpMode {
drive.update(); drive.update();
Pose2d myPose = drive.getPoseEstimate(); Pose2d myPose = drive.getPoseEstimate();
// 2. Get Targets based on current X, Y // 2. Get Base Targets from Interpolation Table
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY()); double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
double targetVelocity = targets[0]; double targetVelocity = targets[0];
double targetHood = targets[1]; double baseHoodPos = targets[1];
robot.bottomFly.setVelocity(targetVelocity); // 3. Calculate Velocity Error and Hood Correction
robot.topFly.setVelocity(targetVelocity); // Get current speed (averaging both flywheels for accuracy)
robot.launchHood.setPosition(targetHood); double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
double velocityError = targetVelocity - currentVel;
/* Logic: Decrease hood by 0.05 for every 100 ticks/sec the motor is off.
If velocityError is 200 (motor is too slow), adjustment is (200/100) * 0.05 = 0.10.
We subtract this from the base hood position.
*/
double hoodAdjustment = (velocityError / 100.0) * 0.03;
double finalHoodPos = baseHoodPos - hoodAdjustment;
// 4. Ensure the hood stays within physical servo limits [0.0 to 1.0]
finalHoodPos = Range.clip(finalHoodPos, 0.0, 1.0);
// 5. Apply Power and Positions
robot.bottomFly.setVelocity(targetVelocity);
robot.topFly.setVelocity(targetVelocity);
robot.launchHood.setPosition(finalHoodPos);
robot.intake.set(1);
if (gamepad1.dpadUpWasPressed()) {
robot.activeTransfer.setPosition(.95); robot.activeTransfer.setPosition(.95);
robot.intake.set(.65); }
if (gamepad1.dpadDownWasPressed()) {
robot.activeTransfer.setPosition(.7);
}
// Optional: Turret tracking (Angle to Goal) // Telemetry for real-time tuning
// double angleToGoal = Math.atan2(GOAL_Y - myPose.getY(), GOAL_X - myPose.getX()); telemetry.addData("Robot X", myPose.getX());
// robot.turret.setRotation(angleToGoal - myPose.getHeading()); telemetry.addData("Robot Y", myPose.getY());
telemetry.addData("Target Velocity", targetVelocity); telemetry.addData("Target Velocity", targetVelocity);
telemetry.addData("Target Hood", targetHood); telemetry.addData("Actual Velocity", currentVel);
telemetry.addData("Base Hood", baseHoodPos);
telemetry.addData("Corrected Hood", finalHoodPos);
telemetry.update(); telemetry.update();
} }
} }

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
View File

@@ -41,18 +41,18 @@ public class blueDepot extends LinearOpMode {
} }
public void shoot(){ public void shoot(){
robot.topFly.setVelocity(1750); robot.topFly.setVelocity(1610);
robot.bottomFly.setVelocity(1750); robot.bottomFly.setVelocity(1610);
robot.launchHood.setPosition(0.6); robot.launchHood.setPosition(0.6);
} }
public void reset(){ public void reset(){
robot.intake.set(0); robot.intake.set(0);
robot.topFly.setVelocity(1750); // robot.topFly.setVelocity(1500);
robot.bottomFly.setVelocity(1750); // robot.bottomFly.setVelocity(1500);
// robot.topFly.set(0); // robot.topFly.set(0);
// robot.bottomFly.set(0); // robot.bottomFly.set(0);
robot.launchHood.setPosition(0.65); // robot.launchHood.setPosition(0.6);
robot.activeTransfer.setPosition(0.8); robot.activeTransfer.setPosition(0.8);
} }
@@ -68,14 +68,14 @@ public class blueDepot extends LinearOpMode {
drive.setPoseEstimate(startPose); drive.setPoseEstimate(startPose);
TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
.UNSTABLE_addTemporalMarkerOffset(.3, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset(); reset();
turretPos(5); turretPos(5);
}) })
.strafeLeft(35) .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { shoot();
reset();
}) })
.strafeLeft(35)
.UNSTABLE_addTemporalMarkerOffset(.5, () -> { .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6); robot.intake.set(0.6);
robot.activeTransfer.setPosition(1); robot.activeTransfer.setPosition(1);
@@ -90,18 +90,19 @@ public class blueDepot extends LinearOpMode {
}) })
//6ball //6ball
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90))) .lineToLinearHeading(new Pose2d(11,28, Math.toRadians(90)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart(); spintakeStart();
}) })
.forward(35) .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd(); spintakeEnd();
}) })
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset(); shoot();
}) })
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.5, () -> { .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6); robot.intake.set(0.6);
robot.activeTransfer.setPosition(1); robot.activeTransfer.setPosition(1);
@@ -116,19 +117,15 @@ public class blueDepot extends LinearOpMode {
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset(); reset();
}) })
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) .lineToLinearHeading(new Pose2d(-14,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-16,65, Math.toRadians(55)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart(); spintakeStart();
}) })
.back(5, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.forward(7, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd(); spintakeEnd();
}) })
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset(); reset();
@@ -143,22 +140,18 @@ public class blueDepot extends LinearOpMode {
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset(); reset();
}) })
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) .lineToLinearHeading(new Pose2d(-33,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-18 ,65, Math.toRadians(55)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart(); spintakeStart();
}) })
.back(5, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.forward(7, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd(); spintakeEnd();
}) })
.lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> { .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset(); shoot();
}) })
.UNSTABLE_addTemporalMarkerOffset(.5, () -> { .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6); robot.intake.set(0.6);

184
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java
View File

@@ -1,80 +1,106 @@
package org.firstinspires.ftc.teamcode.Auton;//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.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
// import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
//@Config
//@Autonomous(name = "Blue Depot Far NO USE") @Config
//public class blueDepotFar extends LinearOpMode { @Autonomous(name = "Blue / Red Far")
// public class blueDepotFar extends LinearOpMode {
// Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888; Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888;
// public void turretPower(double power) {
// robot.turret.setPower(power); public void turretPower(double power) {
// } robot.turret.setPower(power);
// }
// public void turretPos(int position) {
// turretPower(0); public void turretPos(int position) {
// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); turretPower(0);
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
// turretPower(1); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// } turretPower(1);
// }
// public void spintakeStart(){
// robot.intake.set(1); public void spintakeStart(){
// } robot.intake.set(1);
// }
// public void spintakeEnd(){
// robot.intake.set(0); public void spintakeEnd(){
// } robot.intake.set(0);
// }
// public void shoot(){
// robot.topFly.set(1); public void shoot(){
// robot.bottomFly.set(1); robot.topFly.setVelocity(2200);
// robot.launchHood.setPosition(0.58); robot.bottomFly.setVelocity(2200);
// } robot.launchHood.setPosition(0.66);
// }
// public void reset(){
// robot.intake.set(0); public void reset(){
//// robot.topFly.set(0); robot.intake.set(0);
//// robot.bottomFly.set(0); robot.topFly.setVelocity(0);
// robot.launchHood.setPosition(0.35); robot.bottomFly.setVelocity(0);
// robot.activeTransfer.setPosition(0.8); robot.launchHood.setPosition(0.35);
// } robot.activeTransfer.setPosition(0.8);
// }
//
// public void runOpMode() throws InterruptedException {
// public void runOpMode() throws InterruptedException {
// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
// robot.initialize(hardwareMap); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// robot.initialize(hardwareMap);
// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
// Pose2d startPose = new Pose2d(-64, -20, Math.toRadians(25));
// drive.setPoseEstimate(startPose);
// drive.setPoseEstimate(startPose);
// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
// .waitSeconds(3) TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
// .forward(5) .forward(3)
// .build(); .addDisplacementMarker(()-> {
// shoot();
// waitForStart(); })
// if (opModeIsActive() && !isStopRequested()) { .build();
//// while (opModeIsActive()) {
// drive.followTrajectorySequence(threeBall); TrajectorySequence letGo = drive.trajectorySequenceBuilder(threeBall.end())
// PoseStorage.currentPose = drive.getPoseEstimate(); .forward(.01)
//// } .addDisplacementMarker(()-> {
// } robot.activeTransfer.setPosition(1);
// } robot.intake.set(.5);
//} })
.build();
TrajectorySequence waitTime = drive.trajectorySequenceBuilder(startPose)
.waitSeconds(4)
.build();
TrajectorySequence goOut = drive.trajectorySequenceBuilder(startPose)
.forward(4)
.build();
waitForStart();
if (opModeIsActive() && !isStopRequested()) {
while (opModeIsActive()) {
robot.leftPTO.setPosition(0.2);
robot.rightPTO.setPosition(0.2);
drive.followTrajectorySequence(threeBall);
drive.followTrajectorySequence(waitTime);
drive.followTrajectorySequence(letGo);
drive.followTrajectorySequence(waitTime);
drive.followTrajectorySequence(goOut);
PoseStorage.currentPose = drive.getPoseEstimate();
}
}
}
}

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

@@ -1,215 +1,110 @@
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.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
//
//import org.firstinspires.ftc.teamcode.OUTDATED.Hware; 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.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; @Config
// @Autonomous(name = "Clean Red Depot")
//@Config public class redDepot extends LinearOpMode {
//@Autonomous(name = "Red Depot Cycle")
//public class redDepot extends LinearOpMode { Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
// Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888; // --- Action Helpers ---
// public void setTurret(int degrees) {
// public void turretPower(double power) { robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE));
// robot.turret.setPower(power); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// } robot.turret.setPower(1.0);
// }
// public void turretPos(int position) {
// turretPower(0); public void startShooter(double velocity) {
// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robot.topFly.setVelocity(velocity);
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.bottomFly.setVelocity(velocity);
// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); robot.launchHood.setPosition(0.54);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); }
// turretPower(1);
// } public void stopEverything() {
//
// public void spintakeStart(){
// robot.intake.set(1);
// }
//
// public void spintakeEnd(){
// robot.intake.set(0); // robot.intake.set(0);
// } // robot.topFly.setVelocity(0);
// // robot.bottomFly.setVelocity(0);
// public void shoot(){
// robot.topFly.set(0.85);
// robot.bottomFly.set(0.85);
// robot.launchHood.setPosition(0.4);
// }
//
// public void reset(){
// robot.intake.set(0);
//// robot.topFly.set(0);
//// robot.bottomFly.set(0);
// robot.launchHood.setPosition(0.35);
// robot.activeTransfer.setPosition(0.8); // robot.activeTransfer.setPosition(0.8);
// } }
//
// @Override
// public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); robot.initialize(hardwareMap);
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.initialize(hardwareMap);
// Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205));
// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); drive.setPoseEstimate(startPose);
//
// drive.setPoseEstimate(startPose); // --- TRAJECTORY 1: Initial Score ---
// TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose)
// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) .addDisplacementMarker(() -> {
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { setTurret(-5);
// reset(); startShooter(1660);
// turretPos(-10); robot.intake.set(.8);
// }) })
// .strafeRight(35) .strafeRight(35)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
// shoot(); .waitSeconds(1.5)
// }) .addDisplacementMarker(this::stopEverything)
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { .build();
// robot.intake.set(0.9);
// }) // --- TRAJECTORY 2: Intake 6-Ball ---
// .UNSTABLE_addTemporalMarkerOffset(2, () -> { TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end())
// robot.activeTransfer.setPosition(1); .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265)))
// }) .addDisplacementMarker(() -> robot.intake.set(1.0))
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { .forward(35)
//// robot.launchHood.setPosition(0.5); .addDisplacementMarker(() -> robot.intake.set(0))
//// }) .build();
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // --- TRAJECTORY 3: Score 6-Ball ---
// reset(); TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end())
// }) .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
// .lineToLinearHeading(new Pose2d(12,-50, Math.toRadians(45))) .addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
// spintakeStart(); .waitSeconds(1.5)
// }) .addDisplacementMarker(this::stopEverything)
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), .build();
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // --- TRAJECTORY 4: Intake 9-Ball ---
// spintakeEnd(); TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end())
// }) .lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265)))
// .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0))) .addDisplacementMarker(() -> robot.intake.set(1.0))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { .forward(35)
// shoot(); .addDisplacementMarker(() -> robot.intake.set(0))
// }) .build();
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9); // --- TRAJECTORY 5: Score 9-Ball & Park ---
// }) TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end())
// .UNSTABLE_addTemporalMarkerOffset(2, () -> { .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
// robot.activeTransfer.setPosition(1); .addDisplacementMarker(() -> {
// }) startShooter(1660);
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { robot.activeTransfer.setPosition(1.0);
//// robot.launchHood.setPosition(0.5); })
//// }) .waitSeconds(1.5)
// .waitSeconds(3) .addDisplacementMarker(this::stopEverything)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90)))
// reset(); .build();
// })
// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45))) waitForStart();
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart(); if (isStopRequested()) return;
// })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), // Execute sequentially
// SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) drive.followTrajectorySequence(traj1);
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { drive.followTrajectorySequence(traj2);
// spintakeEnd(); drive.followTrajectorySequence(traj3);
// }) drive.followTrajectorySequence(traj4);
// .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0))) drive.followTrajectorySequence(traj5);
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { }
// shoot(); }
// })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
// robot.intake.set(0.9);
// })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> {
// robot.activeTransfer.setPosition(1);
// })
//// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
//// robot.launchHood.setPosition(0.5);
//// })
// .waitSeconds(3)
// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
// turretPos(-84);
// })
// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45)))
// // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
// .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.followTrajectorySequence(threeBall);
// PoseStorage.currentPose = drive.getPoseEstimate();
//// }
// }
// }
//}

252
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java
View File

@@ -1,10 +1,7 @@
package org.firstinspires.ftc.teamcode; 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.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.fasterxml.jackson.databind.node.POJONode;
import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -12,19 +9,45 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config @Config
@TeleOp (name = "TeleOp BLUE") @TeleOp(name = "TeleOp BLUE - Final Optimized")
public class DriverControlsV1 extends LinearOpMode { public class DriverControlsV1 extends LinearOpMode {
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
// --- DASHBOARD TUNING ---
public static double flywheelTuner = 1.0;
private final double P_BASE = 1.09;
private final double F_BASE = 14.12;
public static double HOOD_MIN = 0.3;
public static double HOOD_MAX = 1.0;
public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
// -------------------------
// 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 }
//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; Limelight3A limelight;
LLResult result = null; LLResult result;
public static double visionMult = 1.5;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -32,123 +55,124 @@ public class DriverControlsV1 extends LinearOpMode {
robot.initialize(hardwareMap); robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight"); limelight = hardwareMap.get(Limelight3A.class, "limelight");
double SPEEDCONTROL = 1;
Boolean adjust = false;
Boolean shooting = false;
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(PoseStorage.currentPose); if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
Pose2d driveDirection;
ShooterSubsystem shooterMap = new ShooterSubsystem(); ShooterSubsystem shooterMap = new ShooterSubsystem();
ShootState shootStatus = ShootState.IDLE; ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime(); ElapsedTime shootTimer = new ElapsedTime();
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14); boolean adjust = false;
boolean shooting = false;
drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
waitForStart();
// Initial Hardware Setup
// robot.turret.setTargetPosition(8);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// robot.turret.setPower(0.2);
pid = new PersonalPID(p, i, d, f);
limelight.pipelineSwitch(1); limelight.pipelineSwitch(1);
limelight.start(); limelight.start();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); pid = new PersonalPID(p, i, d, f);
robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
waitForStart();
while (opModeIsActive()) { while (opModeIsActive()) {
robot.leftPTO.setPosition(0.2); result = limelight.getLatestResult();
robot.rightPTO.setPosition(0.2); double vision = result.getTx();
double currentVoltage = robot.batteryVoltageSensor.getVoltage(); // 1. INPUTS & TURRET
if (currentVoltage > 13.5) { // if (gamepad2.yWasPressed()) {
pidf = new PIDFCoefficients(3, 0, 0, 13.8 // robot.turret.setTargetPosition(8);
// } else if (gamepad2.xWasPressed()) {
// robot.turret.setTargetPosition(88);
// } else if (gamepad2.bWasPressed()) {
// robot.turret.setTargetPosition(-88);
// }
double target = ticksperdegree * vision;
double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
pid.setPIDF(p, i, d, f);
//double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
// robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
telemetry.addData("PID power", - targetTicks);
telemetry.addData("Angle", result.getTx());
// 2. PTO OPTIMIZATION (Only write if value is new)
double targetPTO = 0.2;
if (targetPTO != lastPTOPosition) {
robot.leftPTO.setPosition(targetPTO);
robot.rightPTO.setPosition(targetPTO);
lastPTOPosition = targetPTO;
}
// 3. FLYWHEEL PIDF OPTIMIZATION (Crucial for preventing lag/DC)
if (flywheelTuner != lastTuner) {
PIDFCoefficients dynamicPIDF = new PIDFCoefficients(
P_BASE * flywheelTuner, 0, 0, F_BASE * flywheelTuner
); );
} else if (currentVoltage > 12.7) { robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
pidf = new PIDFCoefficients(3, 0, 0, 14); robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
} else if (currentVoltage > 11.2) { lastTuner = flywheelTuner;
pidf = new PIDFCoefficients(3, 0, 0, 14.5 );
} else {
pidf = new PIDFCoefficients(3, 0, 0, 14 );
} }
// 4. DRIVETRAIN
drive.update();
Pose2d myPose = drive.getPoseEstimate(); Pose2d myPose = drive.getPoseEstimate();
double turrPos = robot.turret.getCurrentPosition(); double speedControl = gamepad1.right_bumper ? 0.5 : 1.0;
result = limelight.getLatestResult(); drive.setWeightedDrivePower(new Pose2d(
double vision = result.getTx() * visionMult; -gamepad1.left_stick_y * speedControl,
if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){ -gamepad1.left_stick_x * speedControl,
// drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos))); -gamepad1.right_stick_x * speedControl
} ));
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) { // 5. INTAKE / TRANSFER
SPEEDCONTROL= 0.5;
}
if (!shooting) { if (!shooting) {
if (gamepad1.right_trigger > 0.3) { if (gamepad1.right_trigger > 0.3) {
robot.intake.set(0.8); robot.intake.set(1.0);
robot.activeTransfer.setPosition(0.7); robot.activeTransfer.setPosition(1.0);
} else if (gamepad1.left_trigger > 0.5) { } else if (gamepad1.left_trigger > 0.5) {
robot.intake.set(-0.8); robot.intake.set(-1.0);
robot.activeTransfer.setPosition(0.7); robot.activeTransfer.setPosition(1.0);
} else { } else {
robot.intake.set(0); robot.intake.set(0);
robot.activeTransfer.setPosition(0.7); robot.activeTransfer.setPosition(1.0);
} }
} }
driveDirection = new Pose2d( // 6. SHOOTER & HOOD LOGIC
-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[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
double targetVelocity = targets[0]; double targetVelocity = targets[0];
double targetHood = targets[1]; double baseHood = targets[1];
// targetVelocity = targetVelocity + 40 ;
if (shooting) { double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
robot.launchHood.setPosition(targetHood); double velocityError = targetVelocity - currentVel;
}
if(gamepad2.dpadUpWasPressed()) { double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
adjust = true; double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
} else if (gamepad2.dpadDownWasPressed()){
adjust = false;
}
if (adjust) { if (gamepad2.dpad_up) adjust = true;
else if (gamepad2.dpad_down) adjust = false;
if (adjust || shooting) {
robot.bottomFly.setVelocity(targetVelocity); robot.bottomFly.setVelocity(targetVelocity);
robot.topFly.setVelocity(targetVelocity); robot.topFly.setVelocity(targetVelocity);
robot.launchHood.setPosition(correctedHood);
} else { } else {
robot.bottomFly.setVelocity(0); robot.bottomFly.setVelocity(0);
robot.topFly.setVelocity(0); robot.topFly.setVelocity(0);
} }
// 7. SHOOTING STATE MACHINE
// 1. TRIGGER: Start the sequence if (gamepad2.dpadRightWasPressed()) {
if (gamepad2.xWasPressed()) {
shooting = true; shooting = true;
shootStatus = ShootState.START_SHOOT; shootStatus = ShootState.START_SHOOT;
shootTimer.reset(); shootTimer.reset();
@@ -156,46 +180,46 @@ public class DriverControlsV1 extends LinearOpMode {
switch (shootStatus) { switch (shootStatus) {
case START_SHOOT: case START_SHOOT:
// Initial actions robot.activeTransfer.setPosition(0.81);
robot.activeTransfer.setPosition(0.95); robot.intake.set(1.0);
robot.intake.set(0.55); targetTicks = 0;
robot.launchHood.setPosition(targetHood); if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
// Wait 250ms for the shot to physically fire before moving the hood
if (shootTimer.milliseconds() > 100) {
shootStatus = ShootState.LOWER_HOOD;
} }
break; 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: case RESET:
if (shootTimer.milliseconds() > 3000) { if (shootTimer.milliseconds() > 2500) {
shootStatus = ShootState.IDLE; shootStatus = ShootState.IDLE;
shooting = false; shooting = false;
} }
break; break;
case IDLE:
// Do nothing
break;
} }
if (gamepad2.yWasPressed()) { 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;
}
robot.turret.setPower(- targetTicks);
if (gamepad2.dpadLeftWasPressed()) {
shooting = false; shooting = false;
robot.activeTransfer.setPosition(.7); shootStatus = ShootState.IDLE;
robot.activeTransfer.setPosition(1.0);
} }
drive.setWeightedDrivePower(driveDirection); // 8. TELEMETRY (Keep it clean to save bandwidth)
drive.update(); telemetry.addData("Status", "Running");
telemetry.addData("Vel Error", (int)velocityError);
telemetry.addData("Vision", vision); telemetry.addData("Hood", "%.2f", correctedHood);
telemetry.addData("Voltage Sensor", currentVoltage);
telemetry.update(); telemetry.update();
} }
} }
} }

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

@@ -1,7 +1,5 @@
package org.firstinspires.ftc.teamcode; 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.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResult;
@@ -11,17 +9,45 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
@Config @Config
@TeleOp (name = "TeleOp RED") @TeleOp(name = "TeleOp RED - Final Optimized")
public class DriverControlsV1Red extends LinearOpMode { public class DriverControlsV1Red extends LinearOpMode {
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
// --- DASHBOARD TUNING ---
public static double flywheelTuner = 1.0;
private final double P_BASE = 1.09;
private final double F_BASE = 14.12;
public static double HOOD_MIN = 0.3;
public static double HOOD_MAX = 1.0;
public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
// -------------------------
// 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 }
//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; Limelight3A limelight;
LLResult result = null; LLResult result;
public static double visionMult = 1.5;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -29,123 +55,124 @@ public class DriverControlsV1Red extends LinearOpMode {
robot.initialize(hardwareMap); robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight"); limelight = hardwareMap.get(Limelight3A.class, "limelight");
double SPEEDCONTROL = 1;
Boolean adjust = false;
Boolean shooting = false;
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d(0,0,0)); if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
Pose2d driveDirection;
ShooterSubsystem shooterMap = new ShooterSubsystem();
ShooterSubsystemRed shooterMap = new ShooterSubsystemRed();
ShootState shootStatus = ShootState.IDLE; ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime(); ElapsedTime shootTimer = new ElapsedTime();
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14); boolean adjust = false;
boolean shooting = false;
drive.setPoseEstimate(new Pose2d(0, 0, 0)); // Initial Hardware Setup
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED); // robot.turret.setTargetPosition(8);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// robot.turret.setPower(0.2);
pid = new PersonalPID(p, i, d, f);
limelight.pipelineSwitch(2);
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);
waitForStart(); 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()) { 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(); result = limelight.getLatestResult();
double vision = result.getTx() * visionMult; double vision = result.getTx();
// 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) { // 1. INPUTS & TURRET
SPEEDCONTROL= 0.5; // if (gamepad2.yWasPressed()) {
// robot.turret.setTargetPosition(8);
// } else if (gamepad2.xWasPressed()) {
// robot.turret.setTargetPosition(88);
// } else if (gamepad2.bWasPressed()) {
// robot.turret.setTargetPosition(-88);
// }
double target = ticksperdegree * vision;
double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
pid.setPIDF(p, i, d, f);
//double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
// robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
telemetry.addData("PID power", - targetTicks);
telemetry.addData("Angle", result.getTx());
// 2. PTO OPTIMIZATION (Only write if value is new)
double targetPTO = 0.2;
if (targetPTO != lastPTOPosition) {
robot.leftPTO.setPosition(targetPTO);
robot.rightPTO.setPosition(targetPTO);
lastPTOPosition = targetPTO;
} }
// 3. FLYWHEEL PIDF OPTIMIZATION (Crucial for preventing lag/DC)
if (flywheelTuner != lastTuner) {
PIDFCoefficients dynamicPIDF = new PIDFCoefficients(
P_BASE * flywheelTuner, 0, 0, F_BASE * flywheelTuner
);
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
lastTuner = flywheelTuner;
}
// 4. DRIVETRAIN
drive.update();
Pose2d myPose = drive.getPoseEstimate();
double speedControl = gamepad1.right_bumper ? 0.5 : 1.0;
drive.setWeightedDrivePower(new Pose2d(
-gamepad1.left_stick_y * speedControl,
-gamepad1.left_stick_x * speedControl,
-gamepad1.right_stick_x * speedControl
));
// 5. INTAKE / TRANSFER
if (!shooting) { if (!shooting) {
if (gamepad1.right_trigger > 0.3) { if (gamepad1.right_trigger > 0.3) {
robot.intake.set(0.8); robot.intake.set(1.0);
robot.activeTransfer.setPosition(0.7); robot.activeTransfer.setPosition(1.0);
} else if (gamepad1.left_trigger > 0.5) { } else if (gamepad1.left_trigger > 0.5) {
robot.intake.set(-0.8); robot.intake.set(-1.0);
robot.activeTransfer.setPosition(0.7); robot.activeTransfer.setPosition(1.0);
} else { } else {
robot.intake.set(0); robot.intake.set(0);
robot.activeTransfer.setPosition(0.7); robot.activeTransfer.setPosition(1.0);
} }
} }
driveDirection = new Pose2d( // 6. SHOOTER & HOOD LOGIC
-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[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
double targetVelocity = targets[0]; double targetVelocity = targets[0];
double targetHood = targets[1]; double baseHood = targets[1];
// targetVelocity = targetVelocity + 40 ;
if (shooting) { double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
robot.launchHood.setPosition(targetHood); double velocityError = targetVelocity - currentVel;
}
if(gamepad2.dpadUpWasPressed()) { double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
adjust = true; double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
} else if (gamepad2.dpadDownWasPressed()){
adjust = false;
}
if (adjust) { if (gamepad2.dpad_up) adjust = true;
else if (gamepad2.dpad_down) adjust = false;
if (adjust || shooting) {
robot.bottomFly.setVelocity(targetVelocity); robot.bottomFly.setVelocity(targetVelocity);
robot.topFly.setVelocity(targetVelocity); robot.topFly.setVelocity(targetVelocity);
robot.launchHood.setPosition(correctedHood);
} else { } else {
robot.bottomFly.setVelocity(0); robot.bottomFly.setVelocity(0);
robot.topFly.setVelocity(0); robot.topFly.setVelocity(0);
} }
// 7. SHOOTING STATE MACHINE
// 1. TRIGGER: Start the sequence if (gamepad2.dpadRightWasPressed()) {
if (gamepad2.xWasPressed()) {
shooting = true; shooting = true;
shootStatus = ShootState.START_SHOOT; shootStatus = ShootState.START_SHOOT;
shootTimer.reset(); shootTimer.reset();
@@ -153,46 +180,46 @@ public class DriverControlsV1Red extends LinearOpMode {
switch (shootStatus) { switch (shootStatus) {
case START_SHOOT: case START_SHOOT:
// Initial actions robot.activeTransfer.setPosition(0.81);
robot.activeTransfer.setPosition(0.95); robot.intake.set(1.0);
robot.intake.set(0.55); targetTicks = 0;
robot.launchHood.setPosition(targetHood); if (shootTimer.milliseconds() > 2000) {
shootStatus = ShootState.RESET;
// Wait 250ms for the shot to physically fire before moving the hood
if (shootTimer.milliseconds() > 100) {
shootStatus = ShootState.LOWER_HOOD;
} }
break; 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: case RESET:
if (shootTimer.milliseconds() > 3000) { if (shootTimer.milliseconds() > 2500) {
shootStatus = ShootState.IDLE; shootStatus = ShootState.IDLE;
shooting = false; shooting = false;
} }
break; break;
case IDLE:
// Do nothing
break;
} }
if (gamepad2.yWasPressed()) { 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;
}
robot.turret.setPower(- targetTicks);
if (gamepad2.dpadLeftWasPressed()) {
shooting = false; shooting = false;
robot.activeTransfer.setPosition(.7); shootStatus = ShootState.IDLE;
robot.activeTransfer.setPosition(1.0);
} }
drive.setWeightedDrivePower(driveDirection); // 8. TELEMETRY (Keep it clean to save bandwidth)
drive.update(); telemetry.addData("Status", "Running");
telemetry.addData("Vel Error", (int)velocityError);
telemetry.addData("Vision", vision); telemetry.addData("Hood", "%.2f", correctedHood);
telemetry.addData("Voltage Sensor", currentVoltage);
telemetry.update(); telemetry.update();
} }
} }
} }

166
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV2.java Normal file
View File

@@ -0,0 +1,166 @@
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;
import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
@Config
@TeleOp (name = "TeleOp V2")
public class DriverControlsV2 extends LinearOpMode {
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
Limelight3A limelight;
LLResult result = null;
@Override
public void runOpMode() throws InterruptedException {
Hware robot = new Hware();
robot.initialize(hardwareMap);
double SPEEDCONTROL = 1;
Boolean adjust = false;
Boolean shooting = false;
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(PoseStorage.currentPose);
drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
Pose2d driveDirection;
ShooterSubsystem shooterMap = new ShooterSubsystem();
ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime();
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
waitForStart();
while (opModeIsActive()) {
robot.leftPTO.setPosition(0.2);
robot.rightPTO.setPosition(0.2);
Pose2d myPose = drive.getPoseEstimate();
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);
}
if (gamepad2.right_trigger > 0.5) {
robot.turret.setPower(.05);
} else if (gamepad2.left_trigger > 0.5) {
robot.turret.setPower(.05);
} else {
robot.turret.setPower(0);
}
drive.setWeightedDrivePower(driveDirection);
drive.update();
telemetry.update();
}
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java
View File

@@ -16,7 +16,7 @@ public class FlywheelTuner extends LinearOpMode {
robot.initialize(hardwareMap); robot.initialize(hardwareMap);
// ticks/sec targets (NOT rpm) // ticks/sec targets (NOT rpm)
double highVelocity = 1500; double highVelocity = 1700;
double lowVelocity = -900; double lowVelocity = -900;
double curTargetVelocity = highVelocity; double curTargetVelocity = highVelocity;
@@ -52,7 +52,8 @@ public class FlywheelTuner extends LinearOpMode {
// keep P/F non-negative (usually) // keep P/F non-negative (usually)
if (P < 0) P = 0; if (P < 0) P = 0;
if (F < 0) F = 0; if (F < 0) F = 0;
// P - 1.09
// F - 14.12
// ---- apply PIDF LIVE ---- // ---- apply PIDF LIVE ----
PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F); PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F);
robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);

674
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java
View File

@@ -1,337 +1,337 @@
package org.firstinspires.ftc.teamcode.OUTDATED; //package org.firstinspires.ftc.teamcode.OUTDATED;
//
import com.acmerobotics.dashboard.config.Config; //import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.drivebase.MecanumDrive; //import com.arcrobotics.ftclib.drivebase.MecanumDrive;
import com.arcrobotics.ftclib.gamepad.GamepadEx; //import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys; //import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.OpMode; //import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; //import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor; //import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx; //import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime; //import com.qualcomm.robotcore.util.ElapsedTime;
//
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; //import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; //import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; //import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Position; //import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; //import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.vision.VisionPortal; //import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; //import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; //import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; //import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
//
import java.util.List; //import java.util.List;
@Config //@Config
@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode") //@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode")
public class AutoLock extends OpMode { //public class AutoLock extends OpMode {
//
// Hardware // // Hardware
private DcMotorEx turretMotor; // private DcMotorEx turretMotor;
//
// Vision // // Vision
private VisionPortal visionPortal; // private VisionPortal visionPortal;
private AprilTagProcessor aprilTag; // private AprilTagProcessor aprilTag;
//
// PID gains // // PID gains
public double kP = 0.0012; // public double kP = 0.0012;
public static double kI = 0.001; // public static double kI = 0.001;
public static double kD = 0.002; // public static double kD = 0.002;
//
// Target center (degrees). 0 means center of camera. // // Target center (degrees). 0 means center of camera.
private double targetX = 0.0; // private double targetX = 0.0;
//
private double integral = 0.0; // private double integral = 0.0;
private double lastError = 0.0; // private double lastError = 0.0;
//
GamepadEx driverControl = null; // GamepadEx driverControl = null;
GamepadEx armControl = null; // GamepadEx armControl = null;
//
private final ElapsedTime loopTimer = new ElapsedTime(); // private final ElapsedTime loopTimer = new ElapsedTime();
private final ElapsedTime targetLostTimer = new ElapsedTime(); // private final ElapsedTime targetLostTimer = new ElapsedTime();
//
// Tuning // // Tuning
private static final double POSITION_TOLERANCE_DEG = 1.5; // private static final double POSITION_TOLERANCE_DEG = 1.5;
private static final double MIN_POWER = 0.05; // private static final double MIN_POWER = 0.05;
private static final double MAX_POWER = 0.4; // private static final double MAX_POWER = 0.4;
private static final double TARGET_LOST_TIMEOUT_SEC = 0.5; // private static final double TARGET_LOST_TIMEOUT_SEC = 0.5;
//
// Flip if turret moves the wrong way // // Flip if turret moves the wrong way
private static final boolean INVERT_MOTOR = true; // private static final boolean INVERT_MOTOR = true;
//
// OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any. // // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any.
// Example: new int[]{1,2,3} // // Example: new int[]{1,2,3}
private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any // private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any
//
private boolean targetWasVisible = false; // private boolean targetWasVisible = false;
private Position cameraPosition = new Position(DistanceUnit.INCH, // private Position cameraPosition = new Position(DistanceUnit.INCH,
0, 0, 0, 0); // 0, 0, 0, 0);
private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, // private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
0, -90, 0, 0); // 0, -90, 0, 0);
//
// Drive Variables // // Drive Variables
double x, y, xy; // double x, y, xy;
double SPEEDCONTROL; // double SPEEDCONTROL;
boolean driveMode = true; // boolean driveMode = true;
MecanumDrive drive = null; // MecanumDrive drive = null;
Hware robot = null; // Hware robot = null;
//
@Override // @Override
public void init() { // public void init() {
try { // try {
//
// Hardware Map Setup // // Hardware Map Setup
robot = new Hware(); // robot = new Hware();
robot.initialize(hardwareMap); // robot.initialize(hardwareMap);
//
driverControl = new GamepadEx(gamepad1); // driverControl = new GamepadEx(gamepad1);
armControl = new GamepadEx(gamepad2); // armControl = new GamepadEx(gamepad2);
//
// DriveBase Setup // // DriveBase Setup
drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight); //// drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight);
//
//
turretMotor = hardwareMap.get(DcMotorEx.class, "turret"); // turretMotor = hardwareMap.get(DcMotorEx.class, "turret");
turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
//
// AprilTag processor // // AprilTag processor
aprilTag = new AprilTagProcessor.Builder() // aprilTag = new AprilTagProcessor.Builder()
.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) // .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
.setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) // .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary())
.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) // .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
// If you have a Logitech C920/C270 etc, you can usually leave defaults. // // If you have a Logitech C920/C270 etc, you can usually leave defaults.
// If you have calibrated intrinsics, you can set them here (advanced). // // If you have calibrated intrinsics, you can set them here (advanced).
.build(); // .build();
//
// Vision portal with webcam // // Vision portal with webcam
visionPortal = new VisionPortal.Builder() // visionPortal = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) // .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
.addProcessor(aprilTag) // .addProcessor(aprilTag)
.build(); // .build();
//
//
loopTimer.reset(); // loopTimer.reset();
targetLostTimer.reset(); // targetLostTimer.reset();
//
telemetry.addData("Status", "Initialized (Webcam + AprilTag)"); // telemetry.addData("Status", "Initialized (Webcam + AprilTag)");
telemetry.addData("Motor Inverted", INVERT_MOTOR); // telemetry.addData("Motor Inverted", INVERT_MOTOR);
} catch (Exception e) { // } catch (Exception e) {
telemetry.addData("Init Error", e.getMessage()); // telemetry.addData("Init Error", e.getMessage());
} // }
} // }
//
@Override // @Override
public void loop() { // public void loop() {
try { // try {
// Get current detections // // Get current detections
List<AprilTagDetection> detections = aprilTag.getDetections(); // List<AprilTagDetection> detections = aprilTag.getDetections();
//
AprilTagDetection best = pickBestDetection(detections); // AprilTagDetection best = pickBestDetection(detections);
//
telemetry.addData("Detections", detections.size()); // telemetry.addData("Detections", detections.size());
for (AprilTagDetection d : detections) { // for (AprilTagDetection d : detections) {
telemetry.addData("ID", d.id); // telemetry.addData("ID", d.id);
telemetry.addData("Metadata null?", d.metadata == null); // telemetry.addData("Metadata null?", d.metadata == null);
telemetry.addData("Pose null?", d.ftcPose == null); // telemetry.addData("Pose null?", d.ftcPose == null);
//
} // }
//
/** DRIVER CONTROLS **/ // /** DRIVER CONTROLS **/
//
if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;} // if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;}
//
// Chassis // // Chassis
if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; } // if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
x = driverControl.getLeftX(); // x = driverControl.getLeftX();
y = driverControl.getLeftY(); // y = driverControl.getLeftY();
xy = driverControl.getRightX(); // xy = driverControl.getRightX();
//
drive.driveRobotCentric( // drive.driveRobotCentric(
-x * SPEEDCONTROL, // -x * SPEEDCONTROL,
-y * SPEEDCONTROL, // -y * SPEEDCONTROL,
-xy * SPEEDCONTROL // -xy * SPEEDCONTROL
); // );
//
// Intake // // Intake
if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); } // if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); }
//
/** Arm Controls **/ // /** Arm Controls **/
// if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); } //// if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); }
// if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);} //// if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);}
// if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);} //// if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);}
//
//
if (best != null) { // if (best != null) {
targetWasVisible = true; // targetWasVisible = true;
targetLostTimer.reset(); // targetLostTimer.reset();
//
// ----- "tx" equivalent ----- // // ----- "tx" equivalent -----
// AprilTag gives pose relative to camera. We want horizontal angle offset. // // AprilTag gives pose relative to camera. We want horizontal angle offset.
// Use bearing/yaw style value if available from metadata or pose. // // Use bearing/yaw style value if available from metadata or pose.
// // //
// Most reliable: compute from pose X/Z (left/right vs forward): // // Most reliable: compute from pose X/Z (left/right vs forward):
// angle = atan2(x, z) // // angle = atan2(x, z)
// // //
// In FTC pose: // // In FTC pose:
// - pose.x: left/right (inches or meters depending on config; typically inches) // // - pose.x: left/right (inches or meters depending on config; typically inches)
// - pose.z: forward distance // // - pose.z: forward distance
// // //
// If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here. // // If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here.
double x = best.ftcPose.x; // left(+) / right(-) depending on convention // double x = best.ftcPose.x; // left(+) / right(-) depending on convention
double z = best.ftcPose.z; // forward distance // double z = best.ftcPose.z; // forward distance
double txDeg = Math.toDegrees(Math.atan2(x, z)); // double txDeg = Math.toDegrees(Math.atan2(x, z));
//
// Timing // // Timing
double dt = loopTimer.seconds(); // double dt = loopTimer.seconds();
loopTimer.reset(); // loopTimer.reset();
//
if (dt < 0.001) dt = 0.001; // if (dt < 0.001) dt = 0.001;
if (dt > 1.0) dt = 1.0; // if (dt > 1.0) dt = 1.0;
//
// Error (positive tx means tag is to one side) // // Error (positive tx means tag is to one side)
double error = txDeg - targetX; // double error = txDeg - targetX;
//
// PID // // PID
integral += error * dt; // integral += error * dt;
integral = Math.max(-50, Math.min(50, integral)); // anti-windup // integral = Math.max(-50, Math.min(50, integral)); // anti-windup
//
double derivative = (error - lastError) / dt; // double derivative = (error - lastError) / dt;
//
double pidOutput = (kP * error) + (kI * integral) + (kD * derivative); // double pidOutput = (kP * error) + (kI * integral) + (kD * derivative);
//
// Deadband // // Deadband
if (Math.abs(error) < POSITION_TOLERANCE_DEG) { // if (Math.abs(error) < POSITION_TOLERANCE_DEG) {
pidOutput = 0; // pidOutput = 0;
integral = 0; // integral = 0;
} // }
//
// Min power to overcome friction // // Min power to overcome friction
if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) { // if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) {
pidOutput = MIN_POWER * Math.signum(pidOutput); // pidOutput = MIN_POWER * Math.signum(pidOutput);
} // }
//
// Clamp // // Clamp
double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER); // double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER);
//
if (INVERT_MOTOR) motorPower = -motorPower; // if (INVERT_MOTOR) motorPower = -motorPower;
//
if (!Double.isFinite(motorPower)) { // if (!Double.isFinite(motorPower)) {
motorPower = 0; // motorPower = 0;
resetPID(); // resetPID();
} // }
//
turretMotor.setPower(motorPower); // turretMotor.setPower(motorPower);
lastError = error; // lastError = error;
//
telemetry.addData("Status", "LOCKED ON"); // telemetry.addData("Status", "LOCKED ON");
telemetry.addData("Tag ID", best.id); // telemetry.addData("Tag ID", best.id);
telemetry.addData("tx (deg)", "%.2f", txDeg); // telemetry.addData("tx (deg)", "%.2f", txDeg);
telemetry.addData("Error", "%.2f", error); // telemetry.addData("Error", "%.2f", error);
telemetry.addData("PID Output", "%.3f", pidOutput); // telemetry.addData("PID Output", "%.3f", pidOutput);
telemetry.addData("Motor Power", "%.3f", motorPower); // telemetry.addData("Motor Power", "%.3f", motorPower);
//
// Extra pose telemetry (helpful for debugging) // // Extra pose telemetry (helpful for debugging)
telemetry.addData("Pose x", "%.2f", best.ftcPose.x); // telemetry.addData("Pose x", "%.2f", best.ftcPose.x);
telemetry.addData("Pose z", "%.2f", best.ftcPose.z); // telemetry.addData("Pose z", "%.2f", best.ftcPose.z);
telemetry.addData("Range: ", best.ftcPose.range); // telemetry.addData("Range: ", best.ftcPose.range);
telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing); // telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing);
telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw); // telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw);
//
if(best.ftcPose.range < 40){ // if(best.ftcPose.range < 40){
kP = 0.008; // kP = 0.008;
} else if (best.ftcPose.range < 70) { // } else if (best.ftcPose.range < 70) {
kP = 0.005; // kP = 0.005;
} else if (best.ftcPose.range < 90){ // } else if (best.ftcPose.range < 90){
kP = 0.004; // kP = 0.004;
} else if (best.ftcPose.range < 110) { // } else if (best.ftcPose.range < 110) {
kP = 0.003; // kP = 0.003;
} else if (best.ftcPose.range < 130) { // } else if (best.ftcPose.range < 130) {
kP = 0.002; // kP = 0.002;
} else if (best.ftcPose.range < 150) { // } else if (best.ftcPose.range < 150) {
kP = 0.001; // kP = 0.001;
} else { // } else {
kP = 0.0005; // kP = 0.0005;
} // }
telemetry.addData("kP: ", kP); // telemetry.addData("kP: ", kP);
//
} else { // } else {
handleNoTarget(); // handleNoTarget();
} // }
//
} catch (Exception e) { // } catch (Exception e) {
telemetry.addData("Error", e.getMessage()); // telemetry.addData("Error", e.getMessage());
telemetry.addData("Error Type", e.getClass().getSimpleName()); // telemetry.addData("Error Type", e.getClass().getSimpleName());
stopTurret(); // stopTurret();
} // }
//
telemetry.update(); // telemetry.update();
} // }
//
/** // /**
* Pick "best" detection. Simple approach: // * Pick "best" detection. Simple approach:
* - If DESIRED_TAG_IDS set: only accept those // * - If DESIRED_TAG_IDS set: only accept those
* - Choose closest (smallest range) among candidates // * - Choose closest (smallest range) among candidates
*/ // */
private AprilTagDetection pickBestDetection(List<AprilTagDetection> detections) { // private AprilTagDetection pickBestDetection(List<AprilTagDetection> detections) {
if (detections == null || detections.isEmpty()) return null; // if (detections == null || detections.isEmpty()) return null;
//
AprilTagDetection best = null; // AprilTagDetection best = null;
double bestRange = Double.POSITIVE_INFINITY; // double bestRange = Double.POSITIVE_INFINITY;
//
for (AprilTagDetection d : detections) { // for (AprilTagDetection d : detections) {
if (d == null) continue; // if (d == null) continue;
//
if (!isDesiredId(d.id)) continue; // if (!isDesiredId(d.id)) continue;
//
// Use range as "quality" metric // // Use range as "quality" metric
double r = d.ftcPose.range; // double r = d.ftcPose.range;
if (r < bestRange) { // if (r < bestRange) {
bestRange = r; // bestRange = r;
best = d; // best = d;
} // }
} // }
return best; // return best;
} // }
//
private boolean isDesiredId(int id) { // private boolean isDesiredId(int id) {
if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true; // if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true;
for (int x : DESIRED_TAG_IDS) { // for (int x : DESIRED_TAG_IDS) {
if (x == id) return true; // if (x == id) return true;
} // }
return false; // return false;
} // }
//
private void handleNoTarget() { // private void handleNoTarget() {
if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) { // if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) {
telemetry.addData("Status", "TRACKING LOST - Coasting"); // telemetry.addData("Status", "TRACKING LOST - Coasting");
telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds()); // telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds());
// You could also keep last motor power for a brief time if you store it. // // You could also keep last motor power for a brief time if you store it.
} else { // } else {
stopTurret(); // stopTurret();
telemetry.addData("Status", "SEARCHING"); // telemetry.addData("Status", "SEARCHING");
telemetry.addData("Target Visible", "No"); // telemetry.addData("Target Visible", "No");
} // }
} // }
//
private void stopTurret() { // private void stopTurret() {
if (turretMotor != null) turretMotor.setPower(0); // if (turretMotor != null) turretMotor.setPower(0);
resetPID(); // resetPID();
targetWasVisible = false; // targetWasVisible = false;
} // }
//
private void resetPID() { // private void resetPID() {
integral = 0; // integral = 0;
lastError = 0; // lastError = 0;
} // }
//
private static double clamp(double v, double lo, double hi) { // private static double clamp(double v, double lo, double hi) {
return Math.max(lo, Math.min(hi, v)); // return Math.max(lo, Math.min(hi, v));
} // }
//
@Override // @Override
public void stop() { // public void stop() {
try { // try {
if (turretMotor != null) turretMotor.setPower(0); // if (turretMotor != null) turretMotor.setPower(0);
if (visionPortal != null) visionPortal.close(); // if (visionPortal != null) visionPortal.close();
} catch (Exception ignored) {} // } catch (Exception ignored) {}
} // }
} //}

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java
View File

@@ -15,8 +15,8 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
public class Hware { public class Hware {
HardwareMap hardwareMap; HardwareMap hardwareMap;
public Motor frontRight, frontLeft, backRight, backLeft, intake; public Motor intake;
public DcMotorEx turret, bottomFly, topFly; public DcMotorEx frontRight, frontLeft, backRight, backLeft, turret, bottomFly, topFly;
public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
// public Limelight3A limelight; // public Limelight3A limelight;
public VoltageSensor batteryVoltageSensor; public VoltageSensor batteryVoltageSensor;
@@ -34,10 +34,15 @@ public class Hware {
// Chassis Motors // Chassis Motors
frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435); frontRight = hardwareMap.get(DcMotorEx.class, "frontRight");
frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435); frontLeft = hardwareMap.get(DcMotorEx.class, "frontLeft");
backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435); backRight = hardwareMap.get(DcMotorEx.class, "backRight");
backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435); backLeft = hardwareMap.get(DcMotorEx.class, "backLeft");
//
// frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
// frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
// backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435);
// backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435);
// Intake // Intake
intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620); intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620);
@@ -63,10 +68,10 @@ public class Hware {
// webcam = hardwareMap.get(WebcamName.class, "cam"); // webcam = hardwareMap.get(WebcamName.class, "cam");
// Zero Power Behaviors // Zero Power Behaviors
frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
batteryVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub"); // Or "Expansion Hub" batteryVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub"); // Or "Expansion Hub"
@@ -74,7 +79,9 @@ public class Hware {
// To get the actual number: // To get the actual number:
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14 // P - 1.09
// F - 14.12
PIDFCoefficients pidf = new PIDFCoefficients(1.09, 0, 0, 14.12
); );
bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
@@ -82,6 +89,15 @@ public class Hware {
bottomFly.setDirection(DcMotorSimple.Direction.REVERSE); bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Turret Positions
// Center = 8
// 45 deg = -88
// -45 deg = 88
// 90 deg = -240
// -90 deg = 240
//Vision //Vision
// limelight = hardwareMap.get(Limelight3A.class, "limelight"); // limelight = hardwareMap.get(Limelight3A.class, "limelight");
} }

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java
View File

@@ -11,8 +11,7 @@ import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@Config @Config
public class PTOTest extends LinearOpMode { public class PTOTest extends LinearOpMode {
public static double leftPTO = 0; public static double activeTransfer = 0;
public static double rightPTO = 0;
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -21,8 +20,7 @@ public class PTOTest extends LinearOpMode {
waitForStart(); waitForStart();
while (opModeIsActive()) { while (opModeIsActive()) {
robot.leftPTO.setPosition(leftPTO); robot.activeTransfer.setPosition(activeTransfer);
robot.rightPTO.setPosition(rightPTO);
} }
} }
} }

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java
View File

@@ -16,23 +16,24 @@ public class ShooterSubsystem {
public ShooterSubsystem() { public ShooterSubsystem() {
// Top Row (Y = 60) // Top Row (Y = 60)
shotData.add(new ShotPoint(60, 36, 1600, 0.35)); // Field (-36, 60) shotData.add(new ShotPoint(60, 36, 1500, 0.32)); // Field (-36, 60)
shotData.add(new ShotPoint(60, 12, 1700, 0.63)); // Field (-12, 60) shotData.add(new ShotPoint(60, 12, 1700, 0.57)); // Field (-12, 60)
shotData.add(new ShotPoint(60, -12, 1800, 0.63)); // Field (12, 60) shotData.add(new ShotPoint(60, -12, 1800, 0.63)); // Field (12, 60)
shotData.add(new ShotPoint(60, -36, 2000, 0.68)); // Field (36, 60) shotData.add(new ShotPoint(60, -36, 2000, 0.68)); // Field (36, 60)
// Field Middle Row (Field Y = 36) // Field Middle Row (Field Y = 36)
shotData.add(new ShotPoint(36, 36, 1600, 0.45)); // Field (-36, 36) shotData.add(new ShotPoint(36, 36, 1570, 0.42)); // Field (-36, 36)
shotData.add(new ShotPoint(36, 12, 1700, 0.55)); // Field (-12, 36) shotData.add(new ShotPoint(36, 12, 1680, 0.55)); // Field (-12, 36)
shotData.add(new ShotPoint(36, -12, 1800, 0.65)); // Field (12, 36) shotData.add(new ShotPoint(36, -12, 1800, 0.65)); // Field (12, 36)
shotData.add(new ShotPoint(36, -36, 1900, 0.65)); // Field (36, 36) shotData.add(new ShotPoint(36, -36, 1900, 0.65)); // Field (36, 36)
// Field Bottom "V" Points (Field Y = 12) // Field Bottom "V" Points (Field Y = 12)
shotData.add(new ShotPoint(12, 12, 1750, 0.65)); // Field (-12, 12) shotData.add(new ShotPoint(12, 12, 1750, 0.62)); // Field (-12, 12)
shotData.add(new ShotPoint(12, -12, 1900, 0.65)); // Field (12, 12) shotData.add(new ShotPoint(12, -12, 1900, 0.65)); // Field (12, 12)
shotData.add(new ShotPoint(0, 0, 1900, 0.65)); // Field (12, 12) shotData.add(new ShotPoint(0, 0, 1900, 0.65)); // Field (12, 12)
shotData.add(new ShotPoint(-60, 12, 2300, 0.7)); shotData.add(new ShotPoint(-60, 12, 2300, 0.72));
shotData.add(new ShotPoint(-60, -12, 2300, 0.7));
} }

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystemRed.java Normal file
View File

@@ -0,0 +1,71 @@
package org.firstinspires.ftc.teamcode;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class ShooterSubsystemRed {
public static class ShotPoint {
double x, y, velocity, hood;
public ShotPoint(double x, double y, double v, double h) {
this.x = x; this.y = y; this.velocity = v; this.hood = h;
}
}
private final List<ShotPoint> shotData = new ArrayList<>();
public ShooterSubsystemRed() {
// Top Row (X = 60) - Y Reflected
shotData.add(new ShotPoint(60, -36, 1500, 0.32));
shotData.add(new ShotPoint(60, -12, 1700, 0.57));
shotData.add(new ShotPoint(60, 12, 1800, 0.63));
shotData.add(new ShotPoint(60, 36, 2000, 0.68));
// Field Middle Row (X = 36) - Y Reflected
shotData.add(new ShotPoint(36, -36, 1570, 0.42));
shotData.add(new ShotPoint(36, -12, 1680, 0.55));
shotData.add(new ShotPoint(36, 12, 1800, 0.65));
shotData.add(new ShotPoint(36, 36, 1900, 0.65));
// Field Bottom "V" Points (X = 12) - Y Reflected
shotData.add(new ShotPoint(12, -12, 1750, 0.62));
shotData.add(new ShotPoint(12, 12, 1900, 0.65));
shotData.add(new ShotPoint(0, 0, 1900, 0.65));
// Far Points - Y Reflected
shotData.add(new ShotPoint(-60, -12, 2300, 0.72));
shotData.add(new ShotPoint(-60, 12, 2300, 0.7));
}
public double[] getInterpolatedShot(double robotX, double robotY) {
class Node {
ShotPoint p; double dist;
Node(ShotPoint p, double d) { this.p = p; this.dist = d; }
}
List<Node> nodes = new ArrayList<>();
for (ShotPoint p : shotData) {
nodes.add(new Node(p, Math.hypot(p.x - robotX, p.y - robotY)));
}
Collections.sort(nodes, (a, b) -> Double.compare(a.dist, b.dist));
if (nodes.get(0).dist < 1.0) {
return new double[]{nodes.get(0).p.velocity, nodes.get(0).p.hood};
}
double totalWeight = 0;
double weightedVel = 0;
double weightedHood = 0;
for (int i = 0; i < 3; i++) {
Node n = nodes.get(i);
double weight = 1.0 / (n.dist * n.dist);
weightedVel += n.p.velocity * weight;
weightedHood += n.p.hood * weight;
totalWeight += weight;
}
return new double[]{weightedVel / totalWeight, weightedHood / totalWeight};
}
}

66
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java
View File

@@ -29,16 +29,18 @@ public class TeleOpTurretLock extends LinearOpMode {
// Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system. // Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system.
// You said your start is (0,0). So measure these from that same origin. // You said your start is (0,0). So measure these from that same origin.
public static double GOAL_X = 75; public static double GOAL_X = 75;
public static double GOAL_Y = 75; public static double GOAL_Y = -75;
public static double p = 0.00438, i = 0, d = 0.0001, f = 0.0053; public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
public static double target = 0; public static double target = 0;
PersonalPID pid;
public static double ticksperdegree = 9.738888 * 0.29969; public static double ticksperdegree = 9.738888 * 0.29969;
public int minTicks = (int) (ticksperdegree * (-90));
public int maxTicks = (int) (ticksperdegree * (90));
PersonalPID pid;
Limelight3A limelight; Limelight3A limelight;
@@ -70,7 +72,7 @@ public class TeleOpTurretLock extends LinearOpMode {
// Start pose (you said 0,0) // Start pose (you said 0,0)
drive.setPoseEstimate(new Pose2d(0, 0, 0)); drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE); // TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED);
// limelight // limelight
@@ -95,6 +97,7 @@ public class TeleOpTurretLock extends LinearOpMode {
limelight.start(); limelight.start();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
pid = new PersonalPID(p, i, d, f);
while (opModeIsActive()) { while (opModeIsActive()) {
result = limelight.getLatestResult(); result = limelight.getLatestResult();
@@ -116,22 +119,36 @@ public class TeleOpTurretLock extends LinearOpMode {
Pose2d pose = drive.getPoseEstimate(); Pose2d pose = drive.getPoseEstimate();
/* ---------------- UPDATE TARGETS (Dashboard) ---------------- */ // /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
turretLock.goalX = GOAL_X; // turretLock.goalX = GOAL_X;
turretLock.goalY = GOAL_Y; // turretLock.goalY = GOAL_Y;
/* ---------------- MANUAL TRIM ---------------- */ /* ---------------- MANUAL TRIM ---------------- */
// Left trigger = +trim, Right trigger = -trim (same style as your old code) // Left trigger = +trim, Right trigger = -trim (same style as your old code)
double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger); // double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg); // if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim // if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim
// telemetry.update();\ // telemetry.update();\
// //
double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition()); double target = ticksperdegree * vision;
robot.turret.setTargetPosition((int) targetTicks); double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); pid.setPIDF(p, i, d, f);
robot.turret.setPower(1);
// pid.setPIDF(p, i, d, f); //double targetTicks = turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
// robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
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;
}
robot.turret.setPower(- targetTicks);
// int turretPos = robot.turret.getCurrentPosition(); // int turretPos = robot.turret.getCurrentPosition();
// double power = pid.calculate(turretPos, targetTicks); // double power = pid.calculate(turretPos, targetTicks);
// robot.turret.setPower(power); // robot.turret.setPower(power);
@@ -140,17 +157,18 @@ public class TeleOpTurretLock extends LinearOpMode {
// telemetry.addData("target", targetTicks); // telemetry.addData("target", targetTicks);
// telemetry.addData("currentPos", turretPos); // telemetry.addData("currentPos", turretPos);
telemetry.addData("VisionAngle", turretLock.getBearing()); // telemetry.addData("VisionAngle", turretLock.getBearing());
telemetry.addData("Angle", turretLock.getFinalDeg()); // telemetry.addData("Angle", turretLock.getFinalDeg());
telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543)); // telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
telemetry.addData("MODE", turretLock.getTYPE()); // telemetry.addData("MODE", turretLock.getTYPE());
telemetry.addData("Tag ID", turretLock.getObeliskID()); // telemetry.addData("Tag ID", turretLock.getObeliskID());
telemetry.addData("Does Lime have Result", result.isValid()); telemetry.addData("Does Lime have Result", result.isValid());
telemetry.addData("pid", targetTicks);
telemetry.addData("Botpose", result.getBotpose()); // telemetry.addData("Botpose", result.getBotpose());
telemetry.addData("Yaw", result.getTx()); telemetry.addData("Yaw", result.getTx());
telemetry.addData("Status", turretLock.getStatus()); // telemetry.addData("Status", turretLock.getStatus());
telemetry.addData("LimelightInfo", limelight.getStatus()); telemetry.addData("LimelightInfo", limelight.getStatus());

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java
View File

@@ -88,7 +88,7 @@ public class TurretLock {
this.webcam = cam; this.webcam = cam;
switch (side) { switch (side) {
case RED: case RED:
goalX = -75; goalX = 75;
goalY = -75; goalY = -75;
break; break;
case BLUE: case BLUE:

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
View File

@@ -45,7 +45,7 @@ public class DriveConstants {
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/ */
public static double WHEEL_RADIUS = 2.047; // in public static double WHEEL_RADIUS = 2.047; // in
public static double GEAR_RATIO = (24 / 22); // output (wheel) speed / input (motor) speed public static double GEAR_RATIO = (24.0 / 22.0); // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 12.3; // in public static double TRACK_WIDTH = 12.3; // in
/* /*