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 0d6dee7..8bbc7d9 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 @@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.tea import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -11,6 +13,7 @@ import org.firstinspires.ftc.teamcode.drive.DriveConstants; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.util.PersonalPID; @Config @Autonomous(name = "Blue Depot Cycle") @@ -19,6 +22,20 @@ public class blueDepot extends LinearOpMode { Hware robot = new Hware(); public static double TICKS_PER_DEGREE = 9.738888 * 0.29969; + //turret + public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; + public static double target = 0; + + public static double ticksperdegree = 9.738888 * 0.29969; + + public int minTicks = (int) (ticksperdegree * (-90)); + public int maxTicks = (int) (ticksperdegree * (75)); + PersonalPID pid; + Limelight3A limelight; + LLResult result; + + boolean lock = true; + public void turretPower(double power) { robot.turret.setPower(power); } @@ -41,19 +58,19 @@ public class blueDepot extends LinearOpMode { } public void shoot(){ - robot.topFly.setVelocity(1610); - robot.bottomFly.setVelocity(1610); - robot.launchHood.setPosition(0.6); + robot.topFly.setVelocity(1800); + robot.bottomFly.setVelocity(1800); + robot.launchHood.setPosition(0.55); } public void reset(){ robot.intake.set(0); -// robot.topFly.setVelocity(1500); -// robot.bottomFly.setVelocity(1500); +// robot.topFly.setVelocity(1200); +// robot.bottomFly.setVelocity(1200); // robot.topFly.set(0); // robot.bottomFly.set(0); // robot.launchHood.setPosition(0.6); - robot.activeTransfer.setPosition(0.8); + robot.activeTransfer.setPosition(1); } @@ -62,107 +79,114 @@ public class blueDepot extends LinearOpMode { //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); robot.initialize(hardwareMap); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + pid = new PersonalPID(p, i, d, f); + limelight.pipelineSwitch(1); + limelight.start(); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + pid = new PersonalPID(p, i, d, f); - Pose2d startPose = new Pose2d(54, 52, Math.toRadians(135)); + Pose2d startPose = new Pose2d(54, 46, Math.toRadians(52)); drive.setPoseEstimate(startPose); TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); - turretPos(5); +// turretPos(5); }) .UNSTABLE_addTemporalMarkerOffset(.3, () -> { shoot(); }) - .strafeLeft(35) - .UNSTABLE_addTemporalMarkerOffset(.5, () -> { - robot.intake.set(0.6); - robot.activeTransfer.setPosition(1); + .back(47) + .waitSeconds(0.5) + .UNSTABLE_addTemporalMarkerOffset(0.3, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.85); }) // .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { // robot.launchHood.setPosition(0.5); // }) - .waitSeconds(2) + .waitSeconds(1.5) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) //6ball - .lineToLinearHeading(new Pose2d(11,28, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(8,28, Math.toRadians(90))) .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(); - }) - .UNSTABLE_addTemporalMarkerOffset(.1, () -> { shoot(); }) - .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) + .lineToLinearHeading(new Pose2d(24,14, Math.toRadians(52))) .UNSTABLE_addTemporalMarkerOffset(.5, () -> { - robot.intake.set(0.6); - robot.activeTransfer.setPosition(1); + robot.intake.set(1); + robot.activeTransfer.setPosition(0.85); }) // .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { // robot.launchHood.setPosition(0.5); // }) - .waitSeconds(2) + .waitSeconds(1.5) //9ball .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) - .lineToLinearHeading(new Pose2d(-14,27, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(-19,27, Math.toRadians(90))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeStart(); }) - .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeEnd(); }) - .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135))) + .lineToLinearHeading(new Pose2d(-12,54, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(26,14, Math.toRadians(52))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) .UNSTABLE_addTemporalMarkerOffset(.5, () -> { - robot.intake.set(0.6); - robot.activeTransfer.setPosition(1); + robot.intake.set(1); + robot.activeTransfer.setPosition(0.85); }) - .waitSeconds(2) + .waitSeconds(1.5) //12ball .UNSTABLE_addTemporalMarkerOffset(.1, () -> { reset(); }) - .lineToLinearHeading(new Pose2d(-33,27, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(-40,22, Math.toRadians(90))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { spintakeStart(); }) - .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + .forward(33, 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))) + .lineToLinearHeading(new Pose2d(35,14, Math.toRadians(52))) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { shoot(); }) .UNSTABLE_addTemporalMarkerOffset(.5, () -> { - robot.intake.set(0.6); - robot.activeTransfer.setPosition(1); + robot.intake.set(0.8); + robot.activeTransfer.setPosition(0.85); }) - .waitSeconds(2) + .waitSeconds(1.5) // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45))) - .UNSTABLE_addTemporalMarkerOffset(0.5, () -> { - turretPos(84); - }) +// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> { +// turretPos(84); +// }) .waitSeconds(1) .build(); @@ -229,9 +253,32 @@ public class blueDepot extends LinearOpMode { waitForStart(); if (opModeIsActive() && !isStopRequested()) { -// while (opModeIsActive()) { - drive.followTrajectorySequence(threeBall); +// while (opModeIsActive()) { + drive.followTrajectorySequenceAsync(threeBall); PoseStorage.currentPose = drive.getPoseEstimate(); + + while(opModeIsActive()){ + robot.leftPTO.setPosition(0.2); + robot.rightPTO.setPosition(0.14); + drive.update(); + result = limelight.getLatestResult(); + double vision = result.getTx() - 3; + + double target = ticksperdegree * vision; + double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target); + pid.setPIDF(p, i, d, f); + + if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){ + targetTicks = targetTicks; + } else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){ + targetTicks = targetTicks; + + } else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){ + targetTicks = 0; + } + PoseStorage.currentPose = drive.getPoseEstimate(); + robot.turret.setPower(- targetTicks); + } // } } } 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 deleted file mode 100644 index 346d5c7..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java +++ /dev/null @@ -1,106 +0,0 @@ -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; -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/blueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueFar.java new file mode 100644 index 0000000..c6ff771 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueFar.java @@ -0,0 +1,166 @@ +package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.util.PersonalPID; + +@Config +@Autonomous(name = "Blue Far Cycle") +public class blueFar extends LinearOpMode { + + Hware robot = new Hware(); + public static double TICKS_PER_DEGREE = 9.738888 * 0.29969; + + //turret + public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007; + public static double target = 0; + + public static double ticksperdegree = 9.738888 * 0.29969; + + public int minTicks = (int) (ticksperdegree * (-90)); + public int maxTicks = (int) (ticksperdegree * (75)); + PersonalPID pid; + Limelight3A limelight; + LLResult result; + + boolean lock = true; + + public void turretPower(double power) { + robot.turret.setPower(power); + } + + public void turretPos(int position) { + turretPower(0); + robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE); + robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION); + turretPower(1); + } + + public void spintakeStart(){ + robot.intake.set(1); + } + + public void spintakeEnd(){ + robot.intake.set(0); + } + + public void shoot(){ + robot.topFly.setVelocity(2250); + robot.bottomFly.setVelocity(2250); + robot.launchHood.setPosition(0.65); + } + + public void reset(){ + robot.intake.set(0); + robot.activeTransfer.setPosition(1); + } + + + public void runOpMode() throws InterruptedException { + + //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2"); + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + robot.initialize(hardwareMap); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + pid = new PersonalPID(p, i, d, f); + limelight.pipelineSwitch(1); + limelight.start(); + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + pid = new PersonalPID(p, i, d, f); + + Pose2d startPose = new Pose2d(-63, 16, Math.toRadians(0)); + + drive.setPoseEstimate(startPose); + + TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) + + //first shot + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .UNSTABLE_addTemporalMarkerOffset(.3, () -> { + shoot(); + }) + .forward(5) + .turn(Math.toRadians(15)) + .waitSeconds(4) + .UNSTABLE_addTemporalMarkerOffset(0.3, () -> { + robot.intake.set(0.6); + robot.activeTransfer.setPosition(0.6); + }) + .waitSeconds(1.5) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + + //intaking more + .lineToLinearHeading(new Pose2d(-64,53, Math.toRadians(90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(5, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .back(5, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + shoot(); + }) + + //moving to shoot + .lineToLinearHeading(new Pose2d(-58, 16, Math.toRadians(0))) + //shoot position + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(0.52); + robot.activeTransfer.setPosition(0.5); + }) + .waitSeconds(1.5) + .strafeLeft(20) + .waitSeconds(1) + .build(); + + waitForStart(); + if (opModeIsActive() && !isStopRequested()) { +// while (opModeIsActive()) { + drive.followTrajectorySequenceAsync(threeBall); + PoseStorage.currentPose = drive.getPoseEstimate(); + + while(opModeIsActive()){ + robot.leftPTO.setPosition(0.15); + robot.rightPTO.setPosition(0.15); + drive.update(); + result = limelight.getLatestResult(); + double vision = result.getTx(); + + double target = ticksperdegree * vision; + double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target); + pid.setPIDF(p, i, d, f); + + if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){ + targetTicks = targetTicks; + } else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){ + targetTicks = targetTicks; + + } else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){ + targetTicks = 0; + } + PoseStorage.currentPose = drive.getPoseEstimate(); + robot.turret.setPower(- targetTicks); + } +// } + } + } +} \ 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 6797527..368c3f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java @@ -27,7 +27,7 @@ public class DriverControlsV1 extends LinearOpMode { public static double HOOD_MIN = 0.3; public static double HOOD_MAX = 1.0; - public static double HOOD_CORRECTION_SENSITIVITY = 0.04; + public static double HOOD_CORRECTION_SENSITIVITY = 0.01; // ------------------------- // State tracking to prevent redundant hardware writes (The Fix) @@ -81,7 +81,7 @@ public class DriverControlsV1 extends LinearOpMode { while (opModeIsActive()) { result = limelight.getLatestResult(); - double vision = result.getTx(); + double vision = result.getTx()-3; // 1. INPUTS & TURRET // if (gamepad2.yWasPressed()) { @@ -104,7 +104,7 @@ public class DriverControlsV1 extends LinearOpMode { // 2. PTO OPTIMIZATION (Only write if value is new) - double targetPTO = 0.2; + double targetPTO = 0.15; if (targetPTO != lastPTOPosition) { robot.leftPTO.setPosition(targetPTO); robot.rightPTO.setPosition(targetPTO); @@ -156,7 +156,7 @@ public class DriverControlsV1 extends LinearOpMode { double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0; double velocityError = targetVelocity - currentVel; - double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY; + double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY; double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX); if (gamepad2.dpad_up) adjust = true; @@ -167,8 +167,8 @@ public class DriverControlsV1 extends LinearOpMode { robot.topFly.setVelocity(targetVelocity); robot.launchHood.setPosition(correctedHood); } else { - robot.bottomFly.setVelocity(0); - robot.topFly.setVelocity(0); + robot.bottomFly.setVelocity(1700); + robot.topFly.setVelocity(1700); } // 7. SHOOTING STATE MACHINE @@ -178,10 +178,18 @@ public class DriverControlsV1 extends LinearOpMode { shootTimer.reset(); } + if (gamepad2.yWasPressed()) { + flywheelTuner+=.01; + } + + if (gamepad2.aWasPressed()) { + flywheelTuner-=.01; + } + switch (shootStatus) { case START_SHOOT: - robot.activeTransfer.setPosition(0.81); - robot.intake.set(1.0); + robot.activeTransfer.setPosition(0.85); + robot.intake.set(1); targetTicks = 0; if (shootTimer.milliseconds() > 2000) { shootStatus = ShootState.RESET; @@ -219,6 +227,7 @@ public class DriverControlsV1 extends LinearOpMode { telemetry.addData("Status", "Running"); telemetry.addData("Vel Error", (int)velocityError); telemetry.addData("Hood", "%.2f", correctedHood); + telemetry.addData("TunerMult", flywheelTuner); telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/blueDepotFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/blueDepotFar.java new file mode 100644 index 0000000..dcec12f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/blueDepotFar.java @@ -0,0 +1,204 @@ +package org.firstinspires.ftc.teamcode.OUTDATED;//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.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 / 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 threeBall1 = drive.trajectorySequenceBuilder(startPose) + .forward(3) + .addDisplacementMarker(()-> { + shoot(); + }) + .build(); + + TrajectorySequence letGo = drive.trajectorySequenceBuilder(threeBall1.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(); + + TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); +// turretPos(5); + }) + .UNSTABLE_addTemporalMarkerOffset(.3, () -> { + shoot(); + }) + .back(42) + .waitSeconds(0.3) + .UNSTABLE_addTemporalMarkerOffset(0.3, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.6); + }) + +// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +// robot.launchHood.setPosition(0.5); +// }) + .waitSeconds(1.5) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + + //6ball + .lineToLinearHeading(new Pose2d(8,28, Math.toRadians(90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(20, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + shoot(); + }) + .lineToLinearHeading(new Pose2d(24,14, Math.toRadians(52))) + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.5); + }) + +// .UNSTABLE_addTemporalMarkerOffset(2.2, () -> { +// robot.launchHood.setPosition(0.5); +// }) + .waitSeconds(1.5) + + //9ball + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .lineToLinearHeading(new Pose2d(-19,27, Math.toRadians(90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(18, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + }) + .lineToLinearHeading(new Pose2d(-12,54, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(24,14, Math.toRadians(52))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(1); + robot.activeTransfer.setPosition(0.5); + }) + .waitSeconds(1.5) + + //12ball + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + reset(); + }) + .lineToLinearHeading(new Pose2d(-40,27, Math.toRadians(90))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeStart(); + }) + .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + spintakeEnd(); + }) + .lineToLinearHeading(new Pose2d(35,14, Math.toRadians(52))) + .UNSTABLE_addTemporalMarkerOffset(.1, () -> { + shoot(); + }) + .UNSTABLE_addTemporalMarkerOffset(.5, () -> { + robot.intake.set(0.8); + robot.activeTransfer.setPosition(0.6); + }) + .waitSeconds(1.5) + // .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45))) + +// .UNSTABLE_addTemporalMarkerOffset(0.5, () -> { +// turretPos(84); +// }) + .waitSeconds(1) + .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/PTOTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java index fd65f45..2f8d91b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java @@ -11,7 +11,8 @@ import org.firstinspires.ftc.teamcode.OUTDATED.Hware; @Config public class PTOTest extends LinearOpMode { - public static double activeTransfer = 0; + public static double launchHood = 0.3; + public static double activeTransfer = 0.3; @Override public void runOpMode() throws InterruptedException { @@ -20,7 +21,9 @@ public class PTOTest extends LinearOpMode { waitForStart(); while (opModeIsActive()) { + robot.launchHood.setPosition(launchHood); robot.activeTransfer.setPosition(activeTransfer); + } } }