diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java index f93cf87..389dc35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoShotTeleOp.java @@ -3,6 +3,7 @@ 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 com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; @@ -15,6 +16,7 @@ public class AutoShotTeleOp extends LinearOpMode { @Override public void runOpMode() { + // Initialization drive = new SampleMecanumDrive(hardwareMap); robot = new Hware(); robot.initialize(hardwareMap); @@ -27,25 +29,48 @@ public class AutoShotTeleOp extends LinearOpMode { drive.update(); 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 targetVelocity = targets[0]; - double targetHood = targets[1]; + double baseHoodPos = targets[1]; - robot.bottomFly.setVelocity(targetVelocity); - robot.topFly.setVelocity(targetVelocity); - robot.launchHood.setPosition(targetHood); + // 3. Calculate Velocity Error and Hood Correction + // Get current speed (averaging both flywheels for accuracy) + 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.intake.set(.65); + } + if (gamepad1.dpadDownWasPressed()) { + robot.activeTransfer.setPosition(.7); + } - // 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 for real-time tuning + telemetry.addData("Robot X", myPose.getX()); + telemetry.addData("Robot Y", myPose.getY()); 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(); } } 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 382ecc7..0d6dee7 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 @@ -41,18 +41,18 @@ public class blueDepot extends LinearOpMode { } public void shoot(){ - robot.topFly.setVelocity(1750); - robot.bottomFly.setVelocity(1750); + robot.topFly.setVelocity(1610); + robot.bottomFly.setVelocity(1610); robot.launchHood.setPosition(0.6); } public void reset(){ robot.intake.set(0); - robot.topFly.setVelocity(1750); - robot.bottomFly.setVelocity(1750); +// robot.topFly.setVelocity(1500); +// robot.bottomFly.setVelocity(1500); // robot.topFly.set(0); // robot.bottomFly.set(0); - robot.launchHood.setPosition(0.65); +// robot.launchHood.setPosition(0.6); robot.activeTransfer.setPosition(0.8); } @@ -68,14 +68,14 @@ public class blueDepot extends LinearOpMode { drive.setPoseEstimate(startPose); TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) - .UNSTABLE_addTemporalMarkerOffset(.3, () -> { + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); turretPos(5); }) - .strafeLeft(35) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - reset(); + .UNSTABLE_addTemporalMarkerOffset(.3, () -> { + shoot(); }) + .strafeLeft(35) .UNSTABLE_addTemporalMarkerOffset(.5, () -> { robot.intake.set(0.6); robot.activeTransfer.setPosition(1); @@ -90,18 +90,19 @@ public class blueDepot extends LinearOpMode { }) //6ball - .lineToLinearHeading(new Pose2d(-13,27, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(11,28, Math.toRadians(90))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeStart(); }) - .forward(35) + .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeEnd(); }) - .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - reset(); + shoot(); }) + .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) .UNSTABLE_addTemporalMarkerOffset(.5, () -> { robot.intake.set(0.6); robot.activeTransfer.setPosition(1); @@ -116,19 +117,15 @@ public class blueDepot extends LinearOpMode { .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) - .lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) - .lineToLinearHeading(new Pose2d(-16,65, Math.toRadians(55))) + .lineToLinearHeading(new Pose2d(-14,27, Math.toRadians(90))) .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), + .forward(35, 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(); @@ -143,22 +140,18 @@ public class blueDepot extends LinearOpMode { .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) - .lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) - .lineToLinearHeading(new Pose2d(-18 ,65, Math.toRadians(55))) + .lineToLinearHeading(new Pose2d(-33,27, Math.toRadians(90))) .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), + .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeEnd(); }) - .lineToLinearHeading(new Pose2d(-16,27, Math.toRadians(90))) .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { - reset(); + shoot(); }) .UNSTABLE_addTemporalMarkerOffset(.5, () -> { robot.intake.set(0.6); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java index 97ff20a..346d5c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java @@ -1,80 +1,106 @@ 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.SampleMecanumDrive; -//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; -//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -// -//@Config -//@Autonomous(name = "Blue Depot Far NO USE") -//public class blueDepotFar 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(1); -// robot.bottomFly.set(1); -// robot.launchHood.setPosition(0.58); -// } -// -// 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) -// .waitSeconds(3) -// .forward(5) -// .build(); -// -// waitForStart(); -// if (opModeIsActive() && !isStopRequested()) { -//// while (opModeIsActive()) { -// drive.followTrajectorySequence(threeBall); -// PoseStorage.currentPose = drive.getPoseEstimate(); -//// } -// } -// } -//} \ No newline at end of file + +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.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; + +@Config +@Autonomous(name = "Blue / Red Far") +public class blueDepotFar 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.setVelocity(2200); + robot.bottomFly.setVelocity(2200); + robot.launchHood.setPosition(0.66); + } + + public void reset(){ + robot.intake.set(0); + robot.topFly.setVelocity(0); + robot.bottomFly.setVelocity(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(-64, -20, Math.toRadians(25)); + + drive.setPoseEstimate(startPose); + + TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) + .forward(3) + .addDisplacementMarker(()-> { + shoot(); + }) + .build(); + + TrajectorySequence letGo = drive.trajectorySequenceBuilder(threeBall.end()) + .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(); + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java index b80bb27..04b293a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java @@ -1,215 +1,110 @@ -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 = "Red Depot Cycle") -//public class redDepot 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(){ +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.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; + +@Config +@Autonomous(name = "Clean Red Depot") +public class redDepot extends LinearOpMode { + + Hware robot = new Hware(); + public static double TICKS_PER_DEGREE = 9.738888 * 0.29969; + + // --- Action Helpers --- + public void setTurret(int degrees) { + robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE)); + robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.turret.setPower(1.0); + } + + public void startShooter(double velocity) { + robot.topFly.setVelocity(velocity); + robot.bottomFly.setVelocity(velocity); + robot.launchHood.setPosition(0.54); + } + + public void stopEverything() { // 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.topFly.setVelocity(0); +// robot.bottomFly.setVelocity(0); // 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(-10); -// }) -// .strafeRight(35) -// .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(.1, () -> { -// reset(); -// }) -// .lineToLinearHeading(new Pose2d(12,-50, Math.toRadians(45))) -// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { -// spintakeStart(); -// }) -// .forward(35, 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, () -> { -// 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(.1, () -> { -// reset(); -// }) -// .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45))) -// .UNSTABLE_addTemporalMarkerOffset(.1, () -> { -// spintakeStart(); -// }) -// .forward(35, 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, () -> { -// 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(); -//// } -// } -// } -//} \ No newline at end of file + } + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + robot.initialize(hardwareMap); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205)); + drive.setPoseEstimate(startPose); + + // --- TRAJECTORY 1: Initial Score --- + TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose) + .addDisplacementMarker(() -> { + setTurret(-5); + startShooter(1660); + robot.intake.set(.8); + }) + .strafeRight(35) + .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0)) + .waitSeconds(1.5) + .addDisplacementMarker(this::stopEverything) + .build(); + + // --- TRAJECTORY 2: Intake 6-Ball --- + TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end()) + .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265))) + .addDisplacementMarker(() -> robot.intake.set(1.0)) + .forward(35) + .addDisplacementMarker(() -> robot.intake.set(0)) + .build(); + + // --- TRAJECTORY 3: Score 6-Ball --- + TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end()) + .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205))) + .addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through + .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0)) + .waitSeconds(1.5) + .addDisplacementMarker(this::stopEverything) + .build(); + + // --- TRAJECTORY 4: Intake 9-Ball --- + TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end()) + .lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265))) + .addDisplacementMarker(() -> robot.intake.set(1.0)) + .forward(35) + .addDisplacementMarker(() -> robot.intake.set(0)) + .build(); + + // --- TRAJECTORY 5: Score 9-Ball & Park --- + TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end()) + .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205))) + .addDisplacementMarker(() -> { + startShooter(1660); + robot.activeTransfer.setPosition(1.0); + }) + .waitSeconds(1.5) + .addDisplacementMarker(this::stopEverything) + .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90))) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + // Execute sequentially + drive.followTrajectorySequence(traj1); + drive.followTrajectorySequence(traj2); + drive.followTrajectorySequence(traj3); + drive.followTrajectorySequence(traj4); + drive.followTrajectorySequence(traj5); + } +} \ 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 index 7c17aa5..6797527 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java @@ -1,10 +1,7 @@ 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.fasterxml.jackson.databind.node.POJONode; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; 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.PIDFCoefficients; 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.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +import org.firstinspires.ftc.teamcode.util.PersonalPID; @Config -@TeleOp (name = "TeleOp BLUE") +@TeleOp(name = "TeleOp BLUE - Final Optimized") 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; - LLResult result = null; - public static double visionMult = 1.5; + LLResult result; + @Override public void runOpMode() throws InterruptedException { @@ -32,123 +55,124 @@ public class DriverControlsV1 extends LinearOpMode { 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(PoseStorage.currentPose); - drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose); - - Pose2d driveDirection; + if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose); 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, TurretLock.SIDE.BLUE); - - waitForStart(); + boolean adjust = false; + boolean shooting = false; + // 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.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); + robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + pid = new PersonalPID(p, i, d, f); + + waitForStart(); while (opModeIsActive()) { - robot.leftPTO.setPosition(0.2); - robot.rightPTO.setPosition(0.2); + result = limelight.getLatestResult(); + double vision = result.getTx(); - double currentVoltage = robot.batteryVoltageSensor.getVoltage(); - if (currentVoltage > 13.5) { - pidf = new PIDFCoefficients(3, 0, 0, 13.8 + // 1. INPUTS & TURRET +// 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 ); - } 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 ); - + 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 turrPos = robot.turret.getCurrentPosition(); + double speedControl = gamepad1.right_bumper ? 0.5 : 1.0; - result = limelight.getLatestResult(); - double vision = result.getTx() * visionMult; - 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); + drive.setWeightedDrivePower(new Pose2d( + -gamepad1.left_stick_y * speedControl, + -gamepad1.left_stick_x * speedControl, + -gamepad1.right_stick_x * speedControl + )); - if (gamepad1.right_bumper) { - SPEEDCONTROL= 0.5; - } + // 5. INTAKE / TRANSFER if (!shooting) { if (gamepad1.right_trigger > 0.3) { - robot.intake.set(0.8); - robot.activeTransfer.setPosition(0.7); + robot.intake.set(1.0); + robot.activeTransfer.setPosition(1.0); } else if (gamepad1.left_trigger > 0.5) { - robot.intake.set(-0.8); - robot.activeTransfer.setPosition(0.7); + robot.intake.set(-1.0); + robot.activeTransfer.setPosition(1.0); } else { robot.intake.set(0); - robot.activeTransfer.setPosition(0.7); + robot.activeTransfer.setPosition(1.0); } } - 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 + // 6. SHOOTER & HOOD LOGIC double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY()); double targetVelocity = targets[0]; - double targetHood = targets[1]; + double baseHood = targets[1]; +// targetVelocity = targetVelocity + 40 ; - if (shooting) { - robot.launchHood.setPosition(targetHood); - } + double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0; + double velocityError = targetVelocity - currentVel; - if(gamepad2.dpadUpWasPressed()) { - adjust = true; - } else if (gamepad2.dpadDownWasPressed()){ - adjust = false; - } + double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY; + double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX); - if (adjust) { + if (gamepad2.dpad_up) adjust = true; + else if (gamepad2.dpad_down) adjust = false; + + if (adjust || shooting) { robot.bottomFly.setVelocity(targetVelocity); robot.topFly.setVelocity(targetVelocity); + robot.launchHood.setPosition(correctedHood); } else { robot.bottomFly.setVelocity(0); robot.topFly.setVelocity(0); } - - // 1. TRIGGER: Start the sequence - if (gamepad2.xWasPressed()) { + // 7. SHOOTING STATE MACHINE + if (gamepad2.dpadRightWasPressed()) { shooting = true; shootStatus = ShootState.START_SHOOT; shootTimer.reset(); @@ -156,46 +180,46 @@ public class DriverControlsV1 extends LinearOpMode { 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; + robot.activeTransfer.setPosition(0.81); + robot.intake.set(1.0); + targetTicks = 0; + if (shootTimer.milliseconds() > 2000) { + shootStatus = ShootState.RESET; } 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) { + if (shootTimer.milliseconds() > 2500) { shootStatus = ShootState.IDLE; shooting = false; } 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; - robot.activeTransfer.setPosition(.7); + shootStatus = ShootState.IDLE; + robot.activeTransfer.setPosition(1.0); } - drive.setWeightedDrivePower(driveDirection); - drive.update(); - - telemetry.addData("Vision", vision); - telemetry.addData("Voltage Sensor", currentVoltage); + // 8. TELEMETRY (Keep it clean to save bandwidth) + telemetry.addData("Status", "Running"); + telemetry.addData("Vel Error", (int)velocityError); + telemetry.addData("Hood", "%.2f", correctedHood); telemetry.update(); } - } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java index 06e1d5b..118271c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java @@ -1,7 +1,5 @@ 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; @@ -11,17 +9,45 @@ 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 com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.teamcode.OUTDATED.Hware; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +import org.firstinspires.ftc.teamcode.util.PersonalPID; @Config -@TeleOp (name = "TeleOp RED") +@TeleOp(name = "TeleOp RED - Final Optimized") 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; - LLResult result = null; - public static double visionMult = 1.5; + LLResult result; + @Override public void runOpMode() throws InterruptedException { @@ -29,123 +55,124 @@ public class DriverControlsV1Red extends LinearOpMode { 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(); + if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose); + ShooterSubsystemRed shooterMap = new ShooterSubsystemRed(); ShootState shootStatus = ShootState.IDLE; 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.RED); + // 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(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(); - 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()) { - 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(); - double vision = result.getTx() * visionMult; -// 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); + double vision = result.getTx(); - if (gamepad1.right_bumper) { - SPEEDCONTROL= 0.5; + // 1. INPUTS & TURRET +// 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 (gamepad1.right_trigger > 0.3) { - robot.intake.set(0.8); - robot.activeTransfer.setPosition(0.7); + robot.intake.set(1.0); + robot.activeTransfer.setPosition(1.0); } else if (gamepad1.left_trigger > 0.5) { - robot.intake.set(-0.8); - robot.activeTransfer.setPosition(0.7); + robot.intake.set(-1.0); + robot.activeTransfer.setPosition(1.0); } else { robot.intake.set(0); - robot.activeTransfer.setPosition(0.7); + robot.activeTransfer.setPosition(1.0); } } - 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 + // 6. SHOOTER & HOOD LOGIC double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY()); double targetVelocity = targets[0]; - double targetHood = targets[1]; + double baseHood = targets[1]; +// targetVelocity = targetVelocity + 40 ; - if (shooting) { - robot.launchHood.setPosition(targetHood); - } + double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0; + double velocityError = targetVelocity - currentVel; - if(gamepad2.dpadUpWasPressed()) { - adjust = true; - } else if (gamepad2.dpadDownWasPressed()){ - adjust = false; - } + double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY; + double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX); - if (adjust) { + if (gamepad2.dpad_up) adjust = true; + else if (gamepad2.dpad_down) adjust = false; + + if (adjust || shooting) { robot.bottomFly.setVelocity(targetVelocity); robot.topFly.setVelocity(targetVelocity); + robot.launchHood.setPosition(correctedHood); } else { robot.bottomFly.setVelocity(0); robot.topFly.setVelocity(0); } - - // 1. TRIGGER: Start the sequence - if (gamepad2.xWasPressed()) { + // 7. SHOOTING STATE MACHINE + if (gamepad2.dpadRightWasPressed()) { shooting = true; shootStatus = ShootState.START_SHOOT; shootTimer.reset(); @@ -153,46 +180,46 @@ public class DriverControlsV1Red extends LinearOpMode { 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; + robot.activeTransfer.setPosition(0.81); + robot.intake.set(1.0); + targetTicks = 0; + if (shootTimer.milliseconds() > 2000) { + shootStatus = ShootState.RESET; } 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) { + if (shootTimer.milliseconds() > 2500) { shootStatus = ShootState.IDLE; shooting = false; } 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; - robot.activeTransfer.setPosition(.7); + shootStatus = ShootState.IDLE; + robot.activeTransfer.setPosition(1.0); } - drive.setWeightedDrivePower(driveDirection); - drive.update(); - - telemetry.addData("Vision", vision); - telemetry.addData("Voltage Sensor", currentVoltage); + // 8. TELEMETRY (Keep it clean to save bandwidth) + telemetry.addData("Status", "Running"); + telemetry.addData("Vel Error", (int)velocityError); + telemetry.addData("Hood", "%.2f", correctedHood); telemetry.update(); } - } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV2.java new file mode 100644 index 0000000..4459280 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV2.java @@ -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(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java index d1c25e1..86b16c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlywheelTuner.java @@ -16,7 +16,7 @@ public class FlywheelTuner extends LinearOpMode { robot.initialize(hardwareMap); // ticks/sec targets (NOT rpm) - double highVelocity = 1500; + double highVelocity = 1700; double lowVelocity = -900; double curTargetVelocity = highVelocity; @@ -52,7 +52,8 @@ public class FlywheelTuner extends LinearOpMode { // keep P/F non-negative (usually) if (P < 0) P = 0; if (F < 0) F = 0; - +// P - 1.09 + // F - 14.12 // ---- apply PIDF LIVE ---- PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F); robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java index 6ed88d6..ab5f5af 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java @@ -1,337 +1,337 @@ -package org.firstinspires.ftc.teamcode.OUTDATED; - -import com.acmerobotics.dashboard.config.Config; -import com.arcrobotics.ftclib.drivebase.MecanumDrive; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Position; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; - -import java.util.List; -@Config -@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode") -public class AutoLock extends OpMode { - - // Hardware - private DcMotorEx turretMotor; - - // Vision - private VisionPortal visionPortal; - private AprilTagProcessor aprilTag; - - // PID gains - public double kP = 0.0012; - public static double kI = 0.001; - public static double kD = 0.002; - - // Target center (degrees). 0 means center of camera. - private double targetX = 0.0; - - private double integral = 0.0; - private double lastError = 0.0; - - GamepadEx driverControl = null; - GamepadEx armControl = null; - - private final ElapsedTime loopTimer = new ElapsedTime(); - private final ElapsedTime targetLostTimer = new ElapsedTime(); - - // Tuning - private static final double POSITION_TOLERANCE_DEG = 1.5; - private static final double MIN_POWER = 0.05; - private static final double MAX_POWER = 0.4; - private static final double TARGET_LOST_TIMEOUT_SEC = 0.5; - - // Flip if turret moves the wrong way - private static final boolean INVERT_MOTOR = true; - - // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any. - // Example: new int[]{1,2,3} - private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any - - private boolean targetWasVisible = false; - private Position cameraPosition = new Position(DistanceUnit.INCH, - 0, 0, 0, 0); - private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, - 0, -90, 0, 0); - - // Drive Variables - double x, y, xy; - double SPEEDCONTROL; - boolean driveMode = true; - MecanumDrive drive = null; - Hware robot = null; - - @Override - public void init() { - try { - - // Hardware Map Setup - robot = new Hware(); - robot.initialize(hardwareMap); - - driverControl = new GamepadEx(gamepad1); - armControl = new GamepadEx(gamepad2); - - // DriveBase Setup - drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight); - - - turretMotor = hardwareMap.get(DcMotorEx.class, "turret"); - turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // AprilTag processor - aprilTag = new AprilTagProcessor.Builder() - .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) - .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) - .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) - // If you have a Logitech C920/C270 etc, you can usually leave defaults. - // If you have calibrated intrinsics, you can set them here (advanced). - .build(); - - // Vision portal with webcam - visionPortal = new VisionPortal.Builder() - .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) - .addProcessor(aprilTag) - .build(); - - - loopTimer.reset(); - targetLostTimer.reset(); - - telemetry.addData("Status", "Initialized (Webcam + AprilTag)"); - telemetry.addData("Motor Inverted", INVERT_MOTOR); - } catch (Exception e) { - telemetry.addData("Init Error", e.getMessage()); - } - } - - @Override - public void loop() { - try { - // Get current detections - List detections = aprilTag.getDetections(); - - AprilTagDetection best = pickBestDetection(detections); - - telemetry.addData("Detections", detections.size()); - for (AprilTagDetection d : detections) { - telemetry.addData("ID", d.id); - telemetry.addData("Metadata null?", d.metadata == null); - telemetry.addData("Pose null?", d.ftcPose == null); - - } - - /** DRIVER CONTROLS **/ - - if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;} - - // Chassis - 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(); - y = driverControl.getLeftY(); - xy = driverControl.getRightX(); - - drive.driveRobotCentric( - -x * SPEEDCONTROL, - -y * SPEEDCONTROL, - -xy * SPEEDCONTROL - ); - - // 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); } - - /** 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.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 (best != null) { - targetWasVisible = true; - targetLostTimer.reset(); - - // ----- "tx" equivalent ----- - // AprilTag gives pose relative to camera. We want horizontal angle offset. - // Use bearing/yaw style value if available from metadata or pose. - // - // Most reliable: compute from pose X/Z (left/right vs forward): - // angle = atan2(x, z) - // - // In FTC pose: - // - pose.x: left/right (inches or meters depending on config; typically inches) - // - pose.z: forward distance - // - // 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 z = best.ftcPose.z; // forward distance - double txDeg = Math.toDegrees(Math.atan2(x, z)); - - // Timing - double dt = loopTimer.seconds(); - loopTimer.reset(); - - if (dt < 0.001) dt = 0.001; - if (dt > 1.0) dt = 1.0; - - // Error (positive tx means tag is to one side) - double error = txDeg - targetX; - - // PID - integral += error * dt; - integral = Math.max(-50, Math.min(50, integral)); // anti-windup - - double derivative = (error - lastError) / dt; - - double pidOutput = (kP * error) + (kI * integral) + (kD * derivative); - - // Deadband - if (Math.abs(error) < POSITION_TOLERANCE_DEG) { - pidOutput = 0; - integral = 0; - } - - // Min power to overcome friction - if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) { - pidOutput = MIN_POWER * Math.signum(pidOutput); - } - - // Clamp - double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER); - - if (INVERT_MOTOR) motorPower = -motorPower; - - if (!Double.isFinite(motorPower)) { - motorPower = 0; - resetPID(); - } - - turretMotor.setPower(motorPower); - lastError = error; - - telemetry.addData("Status", "LOCKED ON"); - telemetry.addData("Tag ID", best.id); - telemetry.addData("tx (deg)", "%.2f", txDeg); - telemetry.addData("Error", "%.2f", error); - telemetry.addData("PID Output", "%.3f", pidOutput); - telemetry.addData("Motor Power", "%.3f", motorPower); - - // Extra pose telemetry (helpful for debugging) - telemetry.addData("Pose x", "%.2f", best.ftcPose.x); - telemetry.addData("Pose z", "%.2f", best.ftcPose.z); - telemetry.addData("Range: ", best.ftcPose.range); - telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing); - telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw); - - if(best.ftcPose.range < 40){ - kP = 0.008; - } else if (best.ftcPose.range < 70) { - kP = 0.005; - } else if (best.ftcPose.range < 90){ - kP = 0.004; - } else if (best.ftcPose.range < 110) { - kP = 0.003; - } else if (best.ftcPose.range < 130) { - kP = 0.002; - } else if (best.ftcPose.range < 150) { - kP = 0.001; - } else { - kP = 0.0005; - } - telemetry.addData("kP: ", kP); - - } else { - handleNoTarget(); - } - - } catch (Exception e) { - telemetry.addData("Error", e.getMessage()); - telemetry.addData("Error Type", e.getClass().getSimpleName()); - stopTurret(); - } - - telemetry.update(); - } - - /** - * Pick "best" detection. Simple approach: - * - If DESIRED_TAG_IDS set: only accept those - * - Choose closest (smallest range) among candidates - */ - private AprilTagDetection pickBestDetection(List detections) { - if (detections == null || detections.isEmpty()) return null; - - AprilTagDetection best = null; - double bestRange = Double.POSITIVE_INFINITY; - - for (AprilTagDetection d : detections) { - if (d == null) continue; - - if (!isDesiredId(d.id)) continue; - - // Use range as "quality" metric - double r = d.ftcPose.range; - if (r < bestRange) { - bestRange = r; - best = d; - } - } - return best; - } - - private boolean isDesiredId(int id) { - if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true; - for (int x : DESIRED_TAG_IDS) { - if (x == id) return true; - } - return false; - } - - private void handleNoTarget() { - if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) { - telemetry.addData("Status", "TRACKING LOST - Coasting"); - telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds()); - // You could also keep last motor power for a brief time if you store it. - } else { - stopTurret(); - telemetry.addData("Status", "SEARCHING"); - telemetry.addData("Target Visible", "No"); - } - } - - private void stopTurret() { - if (turretMotor != null) turretMotor.setPower(0); - resetPID(); - targetWasVisible = false; - } - - private void resetPID() { - integral = 0; - lastError = 0; - } - - private static double clamp(double v, double lo, double hi) { - return Math.max(lo, Math.min(hi, v)); - } - - @Override - public void stop() { - try { - if (turretMotor != null) turretMotor.setPower(0); - if (visionPortal != null) visionPortal.close(); - } catch (Exception ignored) {} - } -} \ No newline at end of file +//package org.firstinspires.ftc.teamcode.OUTDATED; +// +//import com.acmerobotics.dashboard.config.Config; +//import com.arcrobotics.ftclib.drivebase.MecanumDrive; +//import com.arcrobotics.ftclib.gamepad.GamepadEx; +//import com.arcrobotics.ftclib.gamepad.GamepadKeys; +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.hardware.DcMotor; +//import com.qualcomm.robotcore.hardware.DcMotorEx; +//import com.qualcomm.robotcore.util.ElapsedTime; +// +//import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +//import org.firstinspires.ftc.robotcore.external.navigation.Position; +//import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +//import org.firstinspires.ftc.vision.VisionPortal; +//import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +//import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; +//import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +// +//import java.util.List; +//@Config +//@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode") +//public class AutoLock extends OpMode { +// +// // Hardware +// private DcMotorEx turretMotor; +// +// // Vision +// private VisionPortal visionPortal; +// private AprilTagProcessor aprilTag; +// +// // PID gains +// public double kP = 0.0012; +// public static double kI = 0.001; +// public static double kD = 0.002; +// +// // Target center (degrees). 0 means center of camera. +// private double targetX = 0.0; +// +// private double integral = 0.0; +// private double lastError = 0.0; +// +// GamepadEx driverControl = null; +// GamepadEx armControl = null; +// +// private final ElapsedTime loopTimer = new ElapsedTime(); +// private final ElapsedTime targetLostTimer = new ElapsedTime(); +// +// // Tuning +// private static final double POSITION_TOLERANCE_DEG = 1.5; +// private static final double MIN_POWER = 0.05; +// private static final double MAX_POWER = 0.4; +// private static final double TARGET_LOST_TIMEOUT_SEC = 0.5; +// +// // Flip if turret moves the wrong way +// private static final boolean INVERT_MOTOR = true; +// +// // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any. +// // Example: new int[]{1,2,3} +// private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any +// +// private boolean targetWasVisible = false; +// private Position cameraPosition = new Position(DistanceUnit.INCH, +// 0, 0, 0, 0); +// private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, +// 0, -90, 0, 0); +// +// // Drive Variables +// double x, y, xy; +// double SPEEDCONTROL; +// boolean driveMode = true; +// MecanumDrive drive = null; +// Hware robot = null; +// +// @Override +// public void init() { +// try { +// +// // Hardware Map Setup +// robot = new Hware(); +// robot.initialize(hardwareMap); +// +// driverControl = new GamepadEx(gamepad1); +// armControl = new GamepadEx(gamepad2); +// +// // DriveBase Setup +//// drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight); +// +// +// turretMotor = hardwareMap.get(DcMotorEx.class, "turret"); +// turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); +// +// // AprilTag processor +// aprilTag = new AprilTagProcessor.Builder() +// .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) +// .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary()) +// .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) +// // If you have a Logitech C920/C270 etc, you can usually leave defaults. +// // If you have calibrated intrinsics, you can set them here (advanced). +// .build(); +// +// // Vision portal with webcam +// visionPortal = new VisionPortal.Builder() +// .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) +// .addProcessor(aprilTag) +// .build(); +// +// +// loopTimer.reset(); +// targetLostTimer.reset(); +// +// telemetry.addData("Status", "Initialized (Webcam + AprilTag)"); +// telemetry.addData("Motor Inverted", INVERT_MOTOR); +// } catch (Exception e) { +// telemetry.addData("Init Error", e.getMessage()); +// } +// } +// +// @Override +// public void loop() { +// try { +// // Get current detections +// List detections = aprilTag.getDetections(); +// +// AprilTagDetection best = pickBestDetection(detections); +// +// telemetry.addData("Detections", detections.size()); +// for (AprilTagDetection d : detections) { +// telemetry.addData("ID", d.id); +// telemetry.addData("Metadata null?", d.metadata == null); +// telemetry.addData("Pose null?", d.ftcPose == null); +// +// } +// +// /** DRIVER CONTROLS **/ +// +// if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;} +// +// // Chassis +// 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(); +// y = driverControl.getLeftY(); +// xy = driverControl.getRightX(); +// +// drive.driveRobotCentric( +// -x * SPEEDCONTROL, +// -y * SPEEDCONTROL, +// -xy * SPEEDCONTROL +// ); +// +// // 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); } +// +// /** 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.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 (best != null) { +// targetWasVisible = true; +// targetLostTimer.reset(); +// +// // ----- "tx" equivalent ----- +// // AprilTag gives pose relative to camera. We want horizontal angle offset. +// // Use bearing/yaw style value if available from metadata or pose. +// // +// // Most reliable: compute from pose X/Z (left/right vs forward): +// // angle = atan2(x, z) +// // +// // In FTC pose: +// // - pose.x: left/right (inches or meters depending on config; typically inches) +// // - pose.z: forward distance +// // +// // 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 z = best.ftcPose.z; // forward distance +// double txDeg = Math.toDegrees(Math.atan2(x, z)); +// +// // Timing +// double dt = loopTimer.seconds(); +// loopTimer.reset(); +// +// if (dt < 0.001) dt = 0.001; +// if (dt > 1.0) dt = 1.0; +// +// // Error (positive tx means tag is to one side) +// double error = txDeg - targetX; +// +// // PID +// integral += error * dt; +// integral = Math.max(-50, Math.min(50, integral)); // anti-windup +// +// double derivative = (error - lastError) / dt; +// +// double pidOutput = (kP * error) + (kI * integral) + (kD * derivative); +// +// // Deadband +// if (Math.abs(error) < POSITION_TOLERANCE_DEG) { +// pidOutput = 0; +// integral = 0; +// } +// +// // Min power to overcome friction +// if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) { +// pidOutput = MIN_POWER * Math.signum(pidOutput); +// } +// +// // Clamp +// double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER); +// +// if (INVERT_MOTOR) motorPower = -motorPower; +// +// if (!Double.isFinite(motorPower)) { +// motorPower = 0; +// resetPID(); +// } +// +// turretMotor.setPower(motorPower); +// lastError = error; +// +// telemetry.addData("Status", "LOCKED ON"); +// telemetry.addData("Tag ID", best.id); +// telemetry.addData("tx (deg)", "%.2f", txDeg); +// telemetry.addData("Error", "%.2f", error); +// telemetry.addData("PID Output", "%.3f", pidOutput); +// telemetry.addData("Motor Power", "%.3f", motorPower); +// +// // Extra pose telemetry (helpful for debugging) +// telemetry.addData("Pose x", "%.2f", best.ftcPose.x); +// telemetry.addData("Pose z", "%.2f", best.ftcPose.z); +// telemetry.addData("Range: ", best.ftcPose.range); +// telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing); +// telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw); +// +// if(best.ftcPose.range < 40){ +// kP = 0.008; +// } else if (best.ftcPose.range < 70) { +// kP = 0.005; +// } else if (best.ftcPose.range < 90){ +// kP = 0.004; +// } else if (best.ftcPose.range < 110) { +// kP = 0.003; +// } else if (best.ftcPose.range < 130) { +// kP = 0.002; +// } else if (best.ftcPose.range < 150) { +// kP = 0.001; +// } else { +// kP = 0.0005; +// } +// telemetry.addData("kP: ", kP); +// +// } else { +// handleNoTarget(); +// } +// +// } catch (Exception e) { +// telemetry.addData("Error", e.getMessage()); +// telemetry.addData("Error Type", e.getClass().getSimpleName()); +// stopTurret(); +// } +// +// telemetry.update(); +// } +// +// /** +// * Pick "best" detection. Simple approach: +// * - If DESIRED_TAG_IDS set: only accept those +// * - Choose closest (smallest range) among candidates +// */ +// private AprilTagDetection pickBestDetection(List detections) { +// if (detections == null || detections.isEmpty()) return null; +// +// AprilTagDetection best = null; +// double bestRange = Double.POSITIVE_INFINITY; +// +// for (AprilTagDetection d : detections) { +// if (d == null) continue; +// +// if (!isDesiredId(d.id)) continue; +// +// // Use range as "quality" metric +// double r = d.ftcPose.range; +// if (r < bestRange) { +// bestRange = r; +// best = d; +// } +// } +// return best; +// } +// +// private boolean isDesiredId(int id) { +// if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true; +// for (int x : DESIRED_TAG_IDS) { +// if (x == id) return true; +// } +// return false; +// } +// +// private void handleNoTarget() { +// if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) { +// telemetry.addData("Status", "TRACKING LOST - Coasting"); +// telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds()); +// // You could also keep last motor power for a brief time if you store it. +// } else { +// stopTurret(); +// telemetry.addData("Status", "SEARCHING"); +// telemetry.addData("Target Visible", "No"); +// } +// } +// +// private void stopTurret() { +// if (turretMotor != null) turretMotor.setPower(0); +// resetPID(); +// targetWasVisible = false; +// } +// +// private void resetPID() { +// integral = 0; +// lastError = 0; +// } +// +// private static double clamp(double v, double lo, double hi) { +// return Math.max(lo, Math.min(hi, v)); +// } +// +// @Override +// public void stop() { +// try { +// if (turretMotor != null) turretMotor.setPower(0); +// if (visionPortal != null) visionPortal.close(); +// } catch (Exception ignored) {} +// } +//} \ No newline at end of file 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 a83710d..38262b0 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 @@ -15,8 +15,8 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; public class Hware { HardwareMap hardwareMap; - public Motor frontRight, frontLeft, backRight, backLeft, intake; - public DcMotorEx turret, bottomFly, topFly; + public Motor intake; + public DcMotorEx frontRight, frontLeft, backRight, backLeft, turret, bottomFly, topFly; public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; // public Limelight3A limelight; public VoltageSensor batteryVoltageSensor; @@ -34,10 +34,15 @@ public class Hware { // Chassis Motors - 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); + frontRight = hardwareMap.get(DcMotorEx.class, "frontRight"); + frontLeft = hardwareMap.get(DcMotorEx.class, "frontLeft"); + backRight = hardwareMap.get(DcMotorEx.class, "backRight"); + 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 = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620); @@ -63,10 +68,10 @@ public class Hware { // webcam = hardwareMap.get(WebcamName.class, "cam"); // Zero Power Behaviors - frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); batteryVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub"); // Or "Expansion Hub" @@ -74,7 +79,9 @@ public class Hware { // 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); @@ -82,6 +89,15 @@ public class Hware { 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 // limelight = hardwareMap.get(Limelight3A.class, "limelight"); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java index 98e7432..fd65f45 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java @@ -11,8 +11,7 @@ import org.firstinspires.ftc.teamcode.OUTDATED.Hware; @Config public class PTOTest extends LinearOpMode { - public static double leftPTO = 0; - public static double rightPTO = 0; + public static double activeTransfer = 0; @Override public void runOpMode() throws InterruptedException { @@ -21,8 +20,7 @@ public class PTOTest extends LinearOpMode { waitForStart(); while (opModeIsActive()) { - robot.leftPTO.setPosition(leftPTO); - robot.rightPTO.setPosition(rightPTO); + robot.activeTransfer.setPosition(activeTransfer); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java index 5cf879b..d2a6e16 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystem.java @@ -16,23 +16,24 @@ public class ShooterSubsystem { 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, 36, 1500, 0.32)); // Field (-36, 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, -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, 36, 1570, 0.42)); // Field (-36, 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, -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, 1750, 0.62)); // 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)); + shotData.add(new ShotPoint(-60, 12, 2300, 0.72)); + shotData.add(new ShotPoint(-60, -12, 2300, 0.7)); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystemRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystemRed.java new file mode 100644 index 0000000..8b4f07f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShooterSubsystemRed.java @@ -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 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 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}; + } +} \ 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 137b222..5b7f6ed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java @@ -29,16 +29,18 @@ public class TeleOpTurretLock extends LinearOpMode { // 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. 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; - PersonalPID pid; - - public static double ticksperdegree = 9.738888 * 0.29969; + public int minTicks = (int) (ticksperdegree * (-90)); + public int maxTicks = (int) (ticksperdegree * (90)); + + PersonalPID pid; + Limelight3A limelight; @@ -70,7 +72,7 @@ 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, TurretLock.SIDE.BLUE); +// TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED); // limelight @@ -95,6 +97,7 @@ public class TeleOpTurretLock extends LinearOpMode { 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); while (opModeIsActive()) { result = limelight.getLatestResult(); @@ -116,22 +119,36 @@ public class TeleOpTurretLock extends LinearOpMode { Pose2d pose = drive.getPoseEstimate(); - /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */ - turretLock.goalX = GOAL_X; - turretLock.goalY = GOAL_Y; +// /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */ +// turretLock.goalX = GOAL_X; +// turretLock.goalY = GOAL_Y; /* ---------------- MANUAL TRIM ---------------- */ // 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); - if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg); - if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim +// 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(pose, result, vision, robot.turret.getCurrentPosition()); - robot.turret.setTargetPosition((int) targetTicks); - robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); - robot.turret.setPower(1); -// pid.setPIDF(p, i, d, f); + 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); + + 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(); // double power = pid.calculate(turretPos, targetTicks); // robot.turret.setPower(power); @@ -140,17 +157,18 @@ public class TeleOpTurretLock extends LinearOpMode { // telemetry.addData("target", targetTicks); // telemetry.addData("currentPos", turretPos); - telemetry.addData("VisionAngle", turretLock.getBearing()); - telemetry.addData("Angle", turretLock.getFinalDeg()); - telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543)); - telemetry.addData("MODE", turretLock.getTYPE()); - telemetry.addData("Tag ID", turretLock.getObeliskID()); +// telemetry.addData("VisionAngle", turretLock.getBearing()); +// telemetry.addData("Angle", turretLock.getFinalDeg()); +// telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543)); +// telemetry.addData("MODE", turretLock.getTYPE()); +// telemetry.addData("Tag ID", turretLock.getObeliskID()); 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("Status", turretLock.getStatus()); +// telemetry.addData("Status", turretLock.getStatus()); telemetry.addData("LimelightInfo", limelight.getStatus()); 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 d86955c..ff7c378 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java @@ -88,7 +88,7 @@ public class TurretLock { this.webcam = cam; switch (side) { case RED: - goalX = -75; + goalX = 75; goalY = -75; break; case BLUE: 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 50ccac0..c69821c 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 @@ -45,7 +45,7 @@ public class DriveConstants { * 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 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 /*