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-12 19:20:40 -06:00
parent f701ff884f
commit f31b99de10
9 changed files with 615 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java Normal file
View File

@@ -0,0 +1,52 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@TeleOp(name="AutoShot TeleOp")
public class AutoShotTeleOp extends LinearOpMode {
SampleMecanumDrive drive;
Hware robot;
ShooterSubsystem shooterMap;
@Override
public void runOpMode() {
drive = new SampleMecanumDrive(hardwareMap);
robot = new Hware();
robot.initialize(hardwareMap);
shooterMap = new ShooterSubsystem();
waitForStart();
while (opModeIsActive()) {
// 1. Update Roadrunner Localization
drive.update();
Pose2d myPose = drive.getPoseEstimate();
// 2. Get Targets based on current X, Y
double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
double targetVelocity = targets[0];
double targetHood = targets[1];
robot.bottomFly.setVelocity(targetVelocity);
robot.topFly.setVelocity(targetVelocity);
robot.launchHood.setPosition(targetHood);
robot.activeTransfer.setPosition(.95);
robot.intake.set(.65);
// Optional: Turret tracking (Angle to Goal)
// double angleToGoal = Math.atan2(GOAL_Y - myPose.getY(), GOAL_X - myPose.getX());
// robot.turret.setRotation(angleToGoal - myPose.getHeading());
telemetry.addData("Target Velocity", targetVelocity);
telemetry.addData("Target Hood", targetHood);
telemetry.update();
}
}
}

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

@@ -1,215 +1,245 @@
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.DriveConstants; 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.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;
//
//@Config @Config
//@Autonomous(name = "Blue Depot Cycle") @Autonomous(name = "Blue Depot Cycle")
//public class blueDepot extends LinearOpMode { public class blueDepot extends LinearOpMode {
//
// Hware robot = new Hware(); Hware robot = new Hware();
// public static double TICKS_PER_DEGREE = 9.738888; public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
//
// public void turretPower(double power) { public void turretPower(double power) {
// robot.turret.setPower(power); robot.turret.setPower(power);
// } }
//
// public void turretPos(int position) { public void turretPos(int position) {
// turretPower(0); turretPower(0);
// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// turretPower(1); turretPower(1);
// } }
//
// public void spintakeStart(){ public void spintakeStart(){
// robot.intake.set(1); robot.intake.set(1);
// } }
//
// public void spintakeEnd(){ public void spintakeEnd(){
// robot.intake.set(0); robot.intake.set(0);
// } }
//
// public void shoot(){ public void shoot(){
// robot.topFly.set(0.85); robot.topFly.setVelocity(1750);
// robot.bottomFly.set(0.85); robot.bottomFly.setVelocity(1750);
// robot.launchHood.setPosition(0.4); robot.launchHood.setPosition(0.6);
// } }
//
// public void reset(){ public void reset(){
// robot.intake.set(0); robot.intake.set(0);
//// robot.topFly.set(0); robot.topFly.setVelocity(1750);
//// robot.bottomFly.set(0); robot.bottomFly.setVelocity(1750);
// robot.launchHood.setPosition(0.35); // robot.topFly.set(0);
// robot.activeTransfer.setPosition(0.8); // robot.bottomFly.set(0);
// } robot.launchHood.setPosition(0.65);
// robot.activeTransfer.setPosition(0.8);
// }
// public void runOpMode() throws InterruptedException {
//
// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); public void runOpMode() throws InterruptedException {
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// robot.initialize(hardwareMap); //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); robot.initialize(hardwareMap);
//
// drive.setPoseEstimate(startPose); Pose2d startPose = new Pose2d(54, 52, Math.toRadians(135));
//
// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) drive.setPoseEstimate(startPose);
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// reset(); TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
// turretPos(5); .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
reset();
turretPos(5);
})
.strafeLeft(35)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6);
robot.activeTransfer.setPosition(1);
})
// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
// robot.launchHood.setPosition(0.5);
// }) // })
// .strafeLeft(35) .waitSeconds(2)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
//6ball
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.forward(35)
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6);
robot.activeTransfer.setPosition(1);
})
// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
// robot.launchHood.setPosition(0.5);
// })
.waitSeconds(2)
//9ball
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-13,65, Math.toRadians(55)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.back(5, 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))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6);
robot.activeTransfer.setPosition(1);
})
.waitSeconds(2)
//12ball
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-18 ,65, Math.toRadians(55)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeStart();
})
.back(5, 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))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
spintakeEnd();
})
.lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
.UNSTABLE_addTemporalMarkerOffset(.1, () -> {
reset();
})
.UNSTABLE_addTemporalMarkerOffset(.5, () -> {
robot.intake.set(0.6);
robot.activeTransfer.setPosition(1);
})
.waitSeconds(2)
// .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
.UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
turretPos(84);
})
.waitSeconds(1)
.build();
// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
// .strafeLeft(25)
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { // .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
// shoot(); // shoot();
// }) // })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { // .waitSeconds(1)
// 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(.1, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset(); // reset();
// }) // })
// .lineToLinearHeading(new Pose2d(12,50, Math.toRadians(-45))) // .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart(); // spintakeStart();
// }) // })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), // .forward(30, 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(0,35, Math.toRadians(0))) // .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot(); // shoot();
// }) // })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { // .waitSeconds(1)
// 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(.1, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset(); // reset();
// }) // })
// .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45))) // .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .forward(10)
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeStart(); // spintakeStart();
// }) // })
// .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), // .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)) // SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// spintakeEnd(); // spintakeEnd();
// }) // })
// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0))) // .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// shoot(); // shoot();
// }) // })
// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { // .waitSeconds(1)
// robot.intake.set(0.9); // .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
// reset();
// }) // })
// .UNSTABLE_addTemporalMarkerOffset(2, () -> { // .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
// 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(); // .build();
//
//// TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose) waitForStart();
//// .strafeLeft(25) if (opModeIsActive() && !isStopRequested()) {
//// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { // while (opModeIsActive()) {
//// shoot(); drive.followTrajectorySequence(threeBall);
//// }) PoseStorage.currentPose = drive.getPoseEstimate();
//// .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();
//// }
// } // }
// } }
//} }
}

189
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java Normal file
View File

@@ -0,0 +1,189 @@
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.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
@Config
@TeleOp
public class DriverControlsV1 extends LinearOpMode {
enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
Limelight3A limelight;
LLResult result = null;
public static double visionMult = 1.5;
@Override
public void runOpMode() throws InterruptedException {
Hware robot = new Hware();
robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
double SPEEDCONTROL = 1;
Boolean adjust = false;
Boolean shooting = false;
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setPoseEstimate(new Pose2d(0,0,0));
Pose2d driveDirection;
ShooterSubsystem shooterMap = new ShooterSubsystem();
ShootState shootStatus = ShootState.IDLE;
ElapsedTime shootTimer = new ElapsedTime();
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry);
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()) {
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();
result = limelight.getLatestResult();
double vision = result.getTx() * visionMult;
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);
robot.turret.setTargetPosition((int) targetTicks);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.turret.setPower(1);
if (gamepad1.right_bumper) {
SPEEDCONTROL= 0.5;
}
if (!shooting) {
if (gamepad1.right_trigger > 0.3) {
robot.intake.set(1);
robot.activeTransfer.setPosition(0.7);
} else if (gamepad1.left_trigger > 0.5) {
robot.intake.set(-1);
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.45, targetHood - 0.2);
robot.launchHood.setPosition(targetHood);
shootStatus = ShootState.RESET;
break;
case RESET:
if (shootTimer.milliseconds() > 3000) {
shootStatus = ShootState.IDLE;
shooting = false;
}
break;
}
if (gamepad2.yWasPressed()) {
shooting = false;
robot.activeTransfer.setPosition(.7);
}
drive.setWeightedDrivePower(driveDirection);
drive.update();
telemetry.addData("Vision", vision);
telemetry.addData("Voltage Sensor", currentVoltage);
telemetry.update();
}
}
}

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

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -17,7 +18,10 @@ public class Hware {
public Motor frontRight, frontLeft, backRight, backLeft, intake; public Motor frontRight, frontLeft, backRight, backLeft, intake;
public DcMotorEx turret, bottomFly, topFly; public DcMotorEx turret, bottomFly, topFly;
public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
public Limelight3A limelight; // public Limelight3A limelight;
public VoltageSensor batteryVoltageSensor;
WebcamName webcam; WebcamName webcam;
public void initialize(HardwareMap hwMap) { public void initialize(HardwareMap hwMap) {
hardwareMap = hwMap; hardwareMap = hwMap;
@@ -63,14 +67,20 @@ public class Hware {
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
batteryVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub"); // Or "Expansion Hub"
// To get the actual number:
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14 PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14
); );
bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
bottomFly.setDirection(DcMotorSimple.Direction.REVERSE); bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
//Vision //Vision
limelight = hardwareMap.get(Limelight3A.class, "limelight"); // limelight = hardwareMap.get(Limelight3A.class, "limelight");
} }
} }

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java Normal file
View File

@@ -0,0 +1,27 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.apache.commons.math3.geometry.euclidean.twod.Line;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@TeleOp
@Config
public class PTOTest extends LinearOpMode {
public static double leftPTO = 0;
public static double rightPTO = 0;
@Override
public void runOpMode() throws InterruptedException {
Hware robot = new Hware();
robot.initialize(hardwareMap);
waitForStart();
while (opModeIsActive()) {
robot.leftPTO.setPosition(leftPTO);
robot.rightPTO.setPosition(rightPTO);
}
}
}

75
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java Normal file
View File

@@ -0,0 +1,75 @@
package org.firstinspires.ftc.teamcode;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class ShooterSubsystem {
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 ShooterSubsystem() {
// Top Row (Y = 60)
shotData.add(new ShotPoint(60, 36, 1600, 0.35)); // Field (-36, 60)
shotData.add(new ShotPoint(60, 12, 1700, 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)
// Field Middle Row (Field Y = 36)
shotData.add(new ShotPoint(36, 36, 1600, 0.45)); // Field (-36, 36)
shotData.add(new ShotPoint(36, 12, 1700, 0.55)); // 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)
// 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, 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));
}
public double[] getInterpolatedShot(double robotX, double robotY) {
// Calculate distances and store in a temporary list
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)));
}
// Sort by distance
Collections.sort(nodes, (a, b) -> Double.compare(a.dist, b.dist));
// If we are basically on top of the point, return it exactly
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;
// Take the 3 closest points for local averaging
for (int i = 0; i < 3; i++) {
Node n = nodes.get(i);
// Inverse Square Weighting: weight = 1 / d^2
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};
}
}

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

@@ -1,15 +1,19 @@
package org.firstinspires.ftc.teamcode; package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
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;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
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.util.PersonalPID;
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.AprilTagProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@@ -27,7 +31,10 @@ public class TeleOpTurretLock extends LinearOpMode {
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 = 1, i = 0, d = 0, f = 1; public static double p = 0.00438, i = 0, d = 0.0001, f = 0.0053;
public static double target = 0;
PersonalPID pid;
public static double ticksperdegree = 9.738888 * 0.29969; public static double ticksperdegree = 9.738888 * 0.29969;
@@ -63,7 +70,8 @@ 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, telemetry, p, i, d, f); TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry);
// limelight // limelight
@@ -77,15 +85,20 @@ public class TeleOpTurretLock extends LinearOpMode {
boolean autoLockEnabled = true; boolean autoLockEnabled = true;
boolean xPrev = false; boolean xPrev = false;
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
pid = new PersonalPID(p, i, d, f);
waitForStart(); waitForStart();
// turretLock.cameraInit(); // turretLock.cameraInit();
limelight.pipelineSwitch(1); limelight.pipelineSwitch(1);
limelight.start(); limelight.start();
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
while (opModeIsActive()) { while (opModeIsActive()) {
result = limelight.getLatestResult();
result = robot.limelight.getLatestResult(); double vision = result.getTx();
/* ---------------- DRIVE ---------------- */ /* ---------------- DRIVE ---------------- */
double speed = DRIVE_SPEED; double speed = DRIVE_SPEED;
@@ -114,7 +127,18 @@ public class TeleOpTurretLock extends LinearOpMode {
if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim
// telemetry.update();\ // telemetry.update();\
// //
turretLock.update(pose, result); double targetTicks = turretLock.update(pose, result, vision);
robot.turret.setTargetPosition((int) targetTicks);
robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.turret.setPower(1);
// pid.setPIDF(p, i, d, f);
// int turretPos = robot.turret.getCurrentPosition();
// double power = pid.calculate(turretPos, targetTicks);
// robot.turret.setPower(power);
//
// telemetry.addData("pid", power);
// telemetry.addData("target", targetTicks);
// telemetry.addData("currentPos", turretPos);
telemetry.addData("VisionAngle", turretLock.getBearing()); telemetry.addData("VisionAngle", turretLock.getBearing());
telemetry.addData("Angle", turretLock.getFinalDeg()); telemetry.addData("Angle", turretLock.getFinalDeg());
@@ -128,7 +152,7 @@ public class TeleOpTurretLock extends LinearOpMode {
telemetry.addData("Status", turretLock.getStatus()); telemetry.addData("Status", turretLock.getStatus());
telemetry.addData("LimelightInfo", robot.limelight.getStatus()); telemetry.addData("LimelightInfo", limelight.getStatus());
telemetry.update(); telemetry.update();
} }

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

@@ -28,7 +28,7 @@ public class TurretLock {
/* ------------------- TUNABLES ------------------- */ /* ------------------- TUNABLES ------------------- */
// Field target (in RoadRunner field units; usually inches) // Field target (in RoadRunner field units; usually inches)
// Set this to the AprilTag/backdrop location you want to aim at. // Set this to the AprilTag/backdrop location 4 you want to aim at.
public double goalX = 75; public double goalX = 75;
public double goalY = 75; public double goalY = 75;
@@ -36,7 +36,7 @@ public class TurretLock {
// Turret conversion // Turret conversion
// If you already trust your TICKS_PER_DEGREE, keep it. // If you already trust your TICKS_PER_DEGREE, keep it.
public double ticksPerDegree = 2.543; //9.738888 * 0.29969; public double ticksPerDegree = 9.738888 * 0.29969; //2.543
// Mechanical limits (ticks). Keep your existing numbers. // Mechanical limits (ticks). Keep your existing numbers.
public int minTicks = (int) (ticksPerDegree * (-90)); public int minTicks = (int) (ticksPerDegree * (-90));
@@ -78,14 +78,10 @@ public class TurretLock {
// private final ElapsedTime tagTimer = new ElapsedTime(); // private final ElapsedTime tagTimer = new ElapsedTime();
public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele, double p, double i, double d, double f) { public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele) {
this.turret = turretMotor; this.turret = turretMotor;
this.webcam = cam; this.webcam = cam;
this.tele = tele; this.tele = tele;
this.p = p;
this.i = i;
this.d = d;
this.f = f;
} }
public double getTurrPos() { public double getTurrPos() {
@@ -172,7 +168,7 @@ public class TurretLock {
/** Main update: call every loop. */ /** Main update: call every loop. */
public void update(Pose2d robotPose, LLResult result) { public double update(Pose2d robotPose, LLResult result, double visionCorrect) {
// 1) ODOM: compute turret angle needed to face goal (in degrees) // 1) ODOM: compute turret angle needed to face goal (in degrees)
double dx = goalX - robotPose.getX(); double dx = goalX - robotPose.getX();
double dy = goalY - robotPose.getY(); double dy = goalY - robotPose.getY();
@@ -192,23 +188,25 @@ public class TurretLock {
if (invertTurret) turretCmdDeg = -turretCmdDeg; if (invertTurret) turretCmdDeg = -turretCmdDeg;
// 2) VISION: find tag error and update vision trim // 2) VISION: find tag error and update vision trim
finalDeg = manualTrimDeg + turretCmdDeg;
limelightRead(result); limelightRead(result);
filteredVisionTrimDeg = result.getTx(); filteredVisionTrimDeg = -visionCorrect;
switch (TYPE){ switch (TYPE){
case LIME: case LIME:
finalDeg = turretCmdDeg + manualTrimDeg - filteredVisionTrimDeg; finalDeg = manualTrimDeg + filteredVisionTrimDeg + turretCmdDeg;
if (!result.isValid()) { if (result != null && (finalDeg > 20 || finalDeg < -20)) {
TYPE = MODE.ODOMETRY; TYPE = MODE.ODOMETRY;
} }
break; break;
case ODOMETRY: case ODOMETRY:
finalDeg = turretCmdDeg + manualTrimDeg; finalDeg = turretCmdDeg + manualTrimDeg;
if (finalDeg < 12 || finalDeg > -12) { if ((finalDeg < 20 || finalDeg > -20) && result.isValid()) {
TYPE = MODE.LIME; TYPE = MODE.LIME;
} }
break; break;
} }
// finalDeg = filteredVisionTrimDeg;
// Optional: keep within your allowed lock range (like ±90) // Optional: keep within your allowed lock range (like ±90)
finalDeg = clamp(finalDeg, -90, 90); finalDeg = clamp(finalDeg, -90, 90);
@@ -216,12 +214,13 @@ public class TurretLock {
int targetTicks = (int) Math.round(finalDeg * ticksPerDegree); int targetTicks = (int) Math.round(finalDeg * ticksPerDegree);
targetTicks = clamp(targetTicks, minTicks, maxTicks); targetTicks = clamp(targetTicks, minTicks, maxTicks);
return targetTicks;
// double pow = pid.calculate(targetTicks); // double pow = pid.calculate(targetTicks);
// turret.setPower(pow); // turret.setPower(pow);
turret.setTargetPosition(targetTicks); // turret.setTargetPosition(targetTicks);
turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); // turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
turret.setPower(1.0); // turret.setPower(1.0);
} }
public MODE getTYPE() { public MODE getTYPE() {

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

@@ -65,8 +65,8 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are * small and gradually increase them later after everything is working. All distance units are
* inches. * inches.
*/ */
public static double MAX_VEL = 60; public static double MAX_VEL = 72;
public static double MAX_ACCEL = 60; public static double MAX_ACCEL = 72;
public static double MAX_ANG_VEL = Math.toRadians(90); public static double MAX_ANG_VEL = Math.toRadians(90);
public static double MAX_ANG_ACCEL = Math.toRadians(90); public static double MAX_ANG_ACCEL = Math.toRadians(90);