diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 88c74f7..92dc1e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -32,7 +32,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double spindexerSpeedIncrease = 0.005; + public static double spindexerSpeedIncrease = 0.008; // These values are ADDED to turrDefault public static double redObeliskTurrPos1 = 0.12; @@ -167,8 +167,8 @@ public class Auto_LT_Close extends LinearOpMode { } if (gamepad2.squareWasPressed()){ + turret.pipelineSwitch(1); robot.limelight.start(); - robot.limelight.pipelineSwitch(1); gamepad2.rumble(500); } @@ -336,6 +336,7 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); flywheel.manageFlywheel(0); + robot.transfer.setPower(0); TELE.addLine("finished"); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 93242a4..26ca9a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -72,7 +72,7 @@ public class Auto_LT_Far extends LinearOpMode { double xStackPickupB, yStackPickupB, hStackPickupB; public static int pickupStackSpeed = 12; int prevMotif = 0; - public static double spindexerSpeedIncrease = 0.005; + public static double spindexerSpeedIncrease = 0.008; public static double shootAllTime = 2; // ---- POSITION TOLERANCES ---- public static double posXTolerance = 5.0; @@ -131,12 +131,6 @@ public class Auto_LT_Far extends LinearOpMode { while (opModeInInit()) { - if (gamepad2.squareWasPressed()){ - robot.limelight.start(); - robot.limelight.pipelineSwitch(1); - gamepad2.rumble(500); - } - if (gamepad2.leftBumperWasPressed()){ gatePickup = !gatePickup; } @@ -187,6 +181,12 @@ public class Auto_LT_Far extends LinearOpMode { obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; turretShootPos = turrDefault + redTurretShootPos; + + if (gamepad2.squareWasPressed()){ + turret.pipelineSwitch(4); + robot.limelight.start(); + gamepad2.rumble(500); + } } else { robot.light.setPosition(0.6); @@ -214,6 +214,12 @@ public class Auto_LT_Far extends LinearOpMode { obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; turretShootPos = turrDefault + blueTurretShootPos; + + if (gamepad2.squareWasPressed()){ + turret.pipelineSwitch(2); + robot.limelight.start(); + gamepad2.rumble(500); + } } leave3Ball = drive.actionBuilder(autoStart) @@ -274,6 +280,7 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); flywheel.manageFlywheel(0); + robot.transfer.setPower(0); TELE.addLine("finished"); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index b13bd89..e950604 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -193,77 +193,6 @@ public class AutoActions{ }; } - public Action shootAll(int vel, double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - double stamp = 0.0; - double velo = vel; - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - spindexer.setIntakePower(-0.1); - - if (ticker == 1) { - stamp = System.currentTimeMillis(); - } - ticker++; - - double robX = drive.localizer.getPose().position.x; - double robY = drive.localizer.getPose().position.y; - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robX - goalX; // delta x from robot to goal - double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robX, robY, robotHeading, 0.0, turretInterpolate); - - turret.trackGoal(deltaPose); - - if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < 0.85) || shooterTicker == 0) { - - if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) { - servos.setSpinPos(spinStartPos); - } else { - servos.setTransferPos(transferServo_in); - shooterTicker++; - double prevSpinPos = servos.getSpinCmdPos(); - servos.setSpinPos(prevSpinPos + spindexSpeed); - } - - return true; - - } else { - servos.setTransferPos(transferServo_out); - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - private boolean doneShooting = false; public Action shootAllAuto(double shootTime, double spindexSpeed) { return new Action() { @@ -385,7 +314,7 @@ public class AutoActions{ if (ticker == 0) { stamp = System.currentTimeMillis(); - robot.limelight.pipelineSwitch(1); + turret.pipelineSwitch(1); } ticker++; @@ -403,9 +332,9 @@ public class AutoActions{ if (shouldFinish){ if (redAlliance){ - robot.limelight.pipelineSwitch(4); + turret.pipelineSwitch(4); } else { - robot.limelight.pipelineSwitch(2); + turret.pipelineSwitch(2); } detectingObelisk = false; return false; @@ -417,56 +346,6 @@ public class AutoActions{ }; } - public Action manageFlywheel( - double vel, - double hoodPos, - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(vel); - servos.setHoodPos(hoodPos); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - - teleStart = currentPose; - - return !shouldFinish; - - } - }; - } - public Action manageShooterAuto( double time, double posX, @@ -511,7 +390,7 @@ public class AutoActions{ double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal Pose2d deltaPose; - if (posX != 0.501){ + if (posX != 0.501) { deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); } else { deltaPose = new Pose2d(robotX, robotY, robotHeading); @@ -522,7 +401,7 @@ public class AutoActions{ targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - if (!detectingObelisk){ + if (!detectingObelisk) { turret.trackGoal(deltaPose); } @@ -536,7 +415,7 @@ public class AutoActions{ boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance; boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance; boolean shouldFinish; - if (whileIntaking){ + if (whileIntaking) { shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull(); } else { shouldFinish = timeDone || (xDone && yDone) || doneShooting; @@ -547,7 +426,7 @@ public class AutoActions{ TELE.addData("Steady?", flywheel.getSteady()); TELE.update(); - if (shouldFinish){ + if (shouldFinish) { doneShooting = false; return false; } else { @@ -557,72 +436,6 @@ public class AutoActions{ } }; } - - public Action manageFlywheelAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - boolean timeFallback = (time != 0.501); - boolean posXFallback = (posX != 0.501); - boolean posYFallback = (posY != 0.501); - - return new Action() { - - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); - - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - - ticker++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - servos.setHoodPos(targetingSettings.hoodAngle); - - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; - boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; - boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - - boolean shouldFinish = timeDone || xDone || yDone; - - teleStart = currentPose; - - return !shouldFinish; - - } - }; - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 932fd11..cdc8d6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -116,10 +116,10 @@ public class TeleopV3 extends LinearOpMode { while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { - robot.limelight.pipelineSwitch(4); + turret.pipelineSwitch(4); light.setManualLightColor(Color.LightRed); } else { - robot.limelight.pipelineSwitch(2); + turret.pipelineSwitch(2); light.setManualLightColor(Color.LightBlue); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index e9740f2..fa44604 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -14,6 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.constants.StateEnums; import java.util.List; @@ -48,6 +49,7 @@ public class Turret { double limelightPosX = 0.0; double limelightPosY = 0.0; LLResult result; + boolean bearingAligned = false; private boolean lockOffset = false; private int obeliskID = 0; @@ -56,6 +58,7 @@ public class Turret { private double lightColor = Color.LightRed; private int currentTrackCount = 0; private double permanentOffset = 0.0; + private int prevPipeline = -1; private PIDController bearingPID; private double prevTurretPos = 0.0; @@ -91,6 +94,12 @@ public class Turret { } prevTurrPos = pos; } + public void pipelineSwitch(int pipeline){ + if (prevPipeline != pipeline){ + robot.limelight.pipelineSwitch(pipeline); + } + prevPipeline = pipeline; + } public boolean turretEqual(double pos) { return Math.abs(pos - this.getTurrPos()) < turretTolerance;