diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java new file mode 100644 index 0000000..f93cf87 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java @@ -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(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java index 079604a..0709b81 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java @@ -1,215 +1,245 @@ package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton; -// -//import com.acmerobotics.dashboard.config.Config; -//import com.acmerobotics.roadrunner.geometry.Pose2d; -//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -//import com.qualcomm.robotcore.hardware.DcMotor; -// -//import org.firstinspires.ftc.teamcode.OUTDATED.Hware; -//import org.firstinspires.ftc.teamcode.drive.DriveConstants; -//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; -//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -// -//@Config -//@Autonomous(name = "Blue Depot Cycle") -//public class blueDepot extends LinearOpMode { -// -// Hware robot = new Hware(); -// public static double TICKS_PER_DEGREE = 9.738888; -// -// public void turretPower(double power) { -// robot.turret.setPower(power); -// } -// -// public void turretPos(int position) { -// turretPower(0); -// robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); -// robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); -// robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); -// turretPower(1); -// } -// -// public void spintakeStart(){ -// robot.intake.set(1); -// } -// -// public void spintakeEnd(){ -// robot.intake.set(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); -// } -// -// -// public void runOpMode() throws InterruptedException { -// -// //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); -// SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); -// robot.initialize(hardwareMap); -// -// Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0)); -// -// drive.setPoseEstimate(startPose); -// -// TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) -// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { -// reset(); -// turretPos(5); + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; + +@Config +@Autonomous(name = "Blue Depot Cycle") +public class blueDepot extends LinearOpMode { + + Hware robot = new Hware(); + public static double TICKS_PER_DEGREE = 9.738888 * 0.29969; + + public void turretPower(double power) { + robot.turret.setPower(power); + } + + public void turretPos(int position) { + turretPower(0); + robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); + robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); + turretPower(1); + } + + public void spintakeStart(){ + robot.intake.set(1); + } + + public void spintakeEnd(){ + robot.intake.set(0); + } + + public void shoot(){ + robot.topFly.setVelocity(1750); + robot.bottomFly.setVelocity(1750); + robot.launchHood.setPosition(0.6); + } + + public void reset(){ + robot.intake.set(0); + robot.topFly.setVelocity(1750); + robot.bottomFly.setVelocity(1750); +// robot.topFly.set(0); +// 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"); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + robot.initialize(hardwareMap); + + Pose2d startPose = new Pose2d(54, 52, Math.toRadians(135)); + + drive.setPoseEstimate(startPose); + + TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) + .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, () -> { // 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) +// .waitSeconds(1) // .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // reset(); // }) -// .lineToLinearHeading(new Pose2d(12,50, Math.toRadians(-45))) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) // .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // 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)) // .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // spintakeEnd(); // }) -// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0))) -// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // 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) +// .waitSeconds(1) // .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // reset(); // }) -// .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45))) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .forward(10) // .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // 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)) // .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // spintakeEnd(); // }) -// .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0))) -// .UNSTABLE_addTemporalMarkerOffset(.3, () -> { +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { // shoot(); // }) -// .UNSTABLE_addTemporalMarkerOffset(1.5, () -> { -// robot.intake.set(0.9); +// .waitSeconds(1) +// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { +// reset(); // }) -// .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))) +// .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0))) // .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(); -//// } -// } -// } -//} \ No newline at end of file + + waitForStart(); + if (opModeIsActive() && !isStopRequested()) { +// while (opModeIsActive()) { + drive.followTrajectorySequence(threeBall); + PoseStorage.currentPose = drive.getPoseEstimate(); +// } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java new file mode 100644 index 0000000..a02ed9c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java @@ -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(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java index f12856e..af46afc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java @@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -17,7 +18,10 @@ public class Hware { public Motor frontRight, frontLeft, backRight, backLeft, intake; public DcMotorEx turret, bottomFly, topFly; public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; - public Limelight3A limelight; + // public Limelight3A limelight; + public VoltageSensor batteryVoltageSensor; + + WebcamName webcam; public void initialize(HardwareMap hwMap) { hardwareMap = hwMap; @@ -63,14 +67,20 @@ public class Hware { backRight.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 ); + bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); bottomFly.setDirection(DcMotorSimple.Direction.REVERSE); //Vision - limelight = hardwareMap.get(Limelight3A.class, "limelight"); + // limelight = hardwareMap.get(Limelight3A.class, "limelight"); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java new file mode 100644 index 0000000..83153e0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java @@ -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); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java new file mode 100644 index 0000000..5cf879b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java @@ -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 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 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}; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java index 0364d20..6de05c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java @@ -1,15 +1,19 @@ package org.firstinspires.ftc.teamcode; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; 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 org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.OUTDATED.Hware; 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.apriltag.AprilTagDetection; 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_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; @@ -63,7 +70,8 @@ public class TeleOpTurretLock extends LinearOpMode { // Start pose (you said 0,0) drive.setPoseEstimate(new Pose2d(0, 0, 0)); - TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry, p, i, d, f); + TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry); + // limelight @@ -77,15 +85,20 @@ public class TeleOpTurretLock extends LinearOpMode { boolean autoLockEnabled = true; boolean xPrev = false; + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + pid = new PersonalPID(p, i, d, f); + waitForStart(); // turretLock.cameraInit(); limelight.pipelineSwitch(1); limelight.start(); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); while (opModeIsActive()) { - - result = robot.limelight.getLatestResult(); + result = limelight.getLatestResult(); + double vision = result.getTx(); /* ---------------- DRIVE ---------------- */ double speed = DRIVE_SPEED; @@ -114,7 +127,18 @@ public class TeleOpTurretLock extends LinearOpMode { if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim // 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("Angle", turretLock.getFinalDeg()); @@ -128,7 +152,7 @@ public class TeleOpTurretLock extends LinearOpMode { telemetry.addData("Status", turretLock.getStatus()); - telemetry.addData("LimelightInfo", robot.limelight.getStatus()); + telemetry.addData("LimelightInfo", limelight.getStatus()); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java index cf93b5e..1c02f4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java @@ -28,7 +28,7 @@ public class TurretLock { /* ------------------- TUNABLES ------------------- */ // 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 goalY = 75; @@ -36,7 +36,7 @@ public class TurretLock { // Turret conversion // 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. public int minTicks = (int) (ticksPerDegree * (-90)); @@ -78,14 +78,10 @@ public class TurretLock { // 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.webcam = cam; this.tele = tele; - this.p = p; - this.i = i; - this.d = d; - this.f = f; } public double getTurrPos() { @@ -172,7 +168,7 @@ public class TurretLock { /** 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) double dx = goalX - robotPose.getX(); double dy = goalY - robotPose.getY(); @@ -192,23 +188,25 @@ public class TurretLock { if (invertTurret) turretCmdDeg = -turretCmdDeg; // 2) VISION: find tag error and update vision trim + finalDeg = manualTrimDeg + turretCmdDeg; limelightRead(result); - filteredVisionTrimDeg = result.getTx(); + filteredVisionTrimDeg = -visionCorrect; switch (TYPE){ case LIME: - finalDeg = turretCmdDeg + manualTrimDeg - filteredVisionTrimDeg; - if (!result.isValid()) { + finalDeg = manualTrimDeg + filteredVisionTrimDeg + turretCmdDeg; + if (result != null && (finalDeg > 20 || finalDeg < -20)) { TYPE = MODE.ODOMETRY; } break; case ODOMETRY: finalDeg = turretCmdDeg + manualTrimDeg; - if (finalDeg < 12 || finalDeg > -12) { + if ((finalDeg < 20 || finalDeg > -20) && result.isValid()) { TYPE = MODE.LIME; } break; } +// finalDeg = filteredVisionTrimDeg; // Optional: keep within your allowed lock range (like ±90) finalDeg = clamp(finalDeg, -90, 90); @@ -216,12 +214,13 @@ public class TurretLock { int targetTicks = (int) Math.round(finalDeg * ticksPerDegree); targetTicks = clamp(targetTicks, minTicks, maxTicks); + return targetTicks; // double pow = pid.calculate(targetTicks); // turret.setPower(pow); - turret.setTargetPosition(targetTicks); - turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); - turret.setPower(1.0); +// turret.setTargetPosition(targetTicks); +// turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); +// turret.setPower(1.0); } public MODE getTYPE() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java index 9ed3f0d..50ccac0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java @@ -65,8 +65,8 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 60; - public static double MAX_ACCEL = 60; + public static double MAX_VEL = 72; + public static double MAX_ACCEL = 72; public static double MAX_ANG_VEL = Math.toRadians(90); public static double MAX_ANG_ACCEL = Math.toRadians(90);