diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java index 48a3ba8..038c6b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12BallPedroPathing.java @@ -337,6 +337,8 @@ public class Auto12BallPedroPathing extends LinearOpMode { initializePoses(); follower.setPose(startPose); buildPaths(); +// turret.switchPipeline(Turret.PipelineMode.OBELISK); + robot.limelight.start(); sleep(2000); // turret.setTurret(turrDefault); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java index 599865d..946037c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java @@ -326,6 +326,8 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode { initializePoses(); follower.setPose(startPose); buildPaths(); +// turret.switchPipeline(Turret.PipelineMode.OBELISK); + robot.limelight.start(); sleep(2000); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 9f4a10b..87ba8a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -16,6 +16,8 @@ import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utilsv2.*; +import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed; + @TeleOp @Config public class TeleopV4 extends LinearOpMode { @@ -60,6 +62,10 @@ public class TeleopV4 extends LinearOpMode { spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + turret.switchPipeline(Turret.PipelineMode.TRACKING); + robot.limelight.start(); + + limelightUsed = false; waitForStart(); @@ -112,6 +118,12 @@ public class TeleopV4 extends LinearOpMode { ); } + if (gamepad2.leftBumperWasPressed()){ + limelightUsed = false; + } else if (gamepad2.rightBumperWasPressed()){ + limelightUsed = true; + } + if (gamepad1.dpad_down){ parkTilter.park(); } else if (gamepad1.dpad_up) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index b5e381f..39474b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.tests; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; +import static org.firstinspires.ftc.teamcode.utilsv2.Turret.limelightUsed; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -63,6 +64,11 @@ public class NewShooterTest extends LinearOpMode { spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + turret.switchPipeline(Turret.PipelineMode.TRACKING); + robot.limelight.start(); + + limelightUsed = true; + waitForStart(); if (isStopRequested()) return; @@ -83,6 +89,12 @@ public class NewShooterTest extends LinearOpMode { shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); + if (gamepad2.leftBumperWasPressed()){ + limelightUsed = false; + } else if (gamepad2.rightBumperWasPressed()){ + limelightUsed = true; + } + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); if (gamepad1.leftBumperWasPressed() && diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index cde9526..3d39960 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -43,13 +43,13 @@ public class Shooter { if (this.red) { goalX = 144; - turretGoalX = 136; + turretGoalX = 140; } else { goalX = 0; turretGoalX = 8; } goalY = 144; - turretGoalY = 136; + turretGoalY = 132; } private double flywheelVelocity = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index 824d815..06562e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -11,7 +11,6 @@ import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.teamcode.constants.Color; -import org.firstinspires.ftc.teamcode.teleop.TeleopV3; import java.util.List; @@ -25,7 +24,6 @@ public class Turret { private final double turretMax = 0.95; public static boolean limelightUsed = true; public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007; - Limelight3A webcam; LLResult result; PIDController bearingPID; boolean bearingAligned = false; @@ -49,6 +47,7 @@ public class Turret { public Turret(Robot rob) { this.robot = rob; + bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); } private double wrapAngle(double angle) { @@ -58,7 +57,8 @@ public class Turret { } private void limelightRead() { // only for tracking purposes, not general reads - result = webcam.getLatestResult(); + switchPipeline(PipelineMode.TRACKING); + result = robot.limelight.getLatestResult(); tx = 1000; if (result != null) { if (result.isValid()) { @@ -67,6 +67,29 @@ public class Turret { } } + public enum PipelineMode{ + OBELISK, + TRACKING + } + + private int prevPipeline = 0; + public void switchPipeline(PipelineMode pipelineMode){ + int pipeline = 0; + if (pipelineMode == PipelineMode.OBELISK){ + pipeline = 1; + } else if (pipelineMode == PipelineMode.TRACKING){ + if (Color.redAlliance){ + pipeline = 4; + } else { + pipeline = 2; + } + } + if (pipeline == 0){prevPipeline = 0;} + if (pipeline != prevPipeline){ + robot.limelight.pipelineSwitch(pipeline); + } + } + public void trackObelisk(double dx, double dy, double h) { double heading = wrapAngle(h); @@ -95,7 +118,7 @@ public class Turret { } private int detectObelisk() { - robot.limelight.pipelineSwitch(1); + switchPipeline(PipelineMode.OBELISK); result = robot.limelight.getLatestResult(); if (result != null && result.isValid()) { List fiducials = result.getFiducialResults();