diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java new file mode 100644 index 0000000..12dab12 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java @@ -0,0 +1,403 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +public class Auto_LT_Far_3Ball extends LinearOpMode { + public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; + public static double autoSpinStartPos = 0.2; + public static double shoot0SpinSpeedIncrease = 0.015; + + public static double redObeliskTurrPos1 = turrDefault + 0.12; + public static double redObeliskTurrPos2 = turrDefault + 0.13; + public static double redObeliskTurrPos3 = turrDefault + 0.14; + + public static double blueObeliskTurrPos1 = turrDefault - 0.12; + public static double blueObeliskTurrPos2 = turrDefault - 0.13; + public static double blueObeliskTurrPos3 = turrDefault - 0.14; + + public static double shoot0XTolerance = 1.0; + public static double redTurretShootPos = turrDefault; + public static double blueTurretShootPos = turrDefault; + public static int fwdTime = 200, strafeTime = 2300; + public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1; + public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1; + public static double rShootX = 1, rShootY = 1, rShootH = 1; + public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1; + public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1; + public static double bShootX = 1, bShootY = 1, bShootH = 1; + public static int sleepTime = 1300; + public int motif = 0; + double turretShootPos = 0.0; + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + Servos servos; + Spindexer spindexer; + Flywheel flywheel; + Turret turret; + Targeting targeting; + Targeting.Settings targetingSettings; + private double firstSpindexShootPos = autoSpinStartPos; + private boolean shootForward = true; + private double xShoot, yShoot, hShoot; + private int driverSlotGreen = 0; + private int passengerSlotGreen = 0; + private int rearSlotGreen = 0; + private int mostGreenSlot = 0; + private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; + private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; + + 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) { + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + flywheel.manageFlywheel(vel); + velo = flywheel.getVelo(); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + robot.intake.setPower(-0.3); + + if (ticker == 1) { + stamp = getRuntime(); + } + ticker++; + + robot.intake.setPower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + if (getRuntime() - stamp < shootTime) { + + if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + } else { + robot.transferServo.setPosition(transferServo_in); + shooterTicker++; + double prevSpinPos = robot.spin1.getPosition(); + robot.spin1.setPosition(prevSpinPos + spindexSpeed); + robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + } + + return true; + + } else { + robot.transferServo.setPosition(transferServo_out); + //spindexPos = spindexer_intakePos1; + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + + } + }; + } + + public Action intake(double intakeTime) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.processIntake(); + robot.intake.setPower(1); + + spindexer.ballCounterLight(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); + + } + }; + } + + 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++; + + flywheel.manageFlywheel(vel); + robot.hood.setPosition(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; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + flywheel = new Flywheel(hardwareMap); + + targeting = new Targeting(); + targetingSettings = new Targeting.Settings(0.0, 0.0); + + spindexer = new Spindexer(hardwareMap); + + servos = new Servos(hardwareMap); + + robot.limelight.start(); + + robot.limelight.pipelineSwitch(1); + + turret = new Turret(robot, TELE, robot.limelight); + + turret.manualSetTurret(turrDefault); + + drive = new MecanumDrive(hardwareMap, teleEnd); + + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + + robot.transferServo.setPosition(transferServo_out); + + robot.light.setPosition(1); + + while (opModeInInit()) { + + robot.hood.setPosition(shoot0Hood); + turret.manualSetTurret(turretShootPos); + + if (gamepad2.crossWasPressed()) { + redAlliance = !redAlliance; + } + + if (gamepad2.dpadLeftWasPressed()) { + turrDefault -= 0.01; + } + + if (gamepad2.dpadRightWasPressed()) { + turrDefault += 0.01; + } + + redObeliskTurrPos1 = turrDefault + 0.12; + redObeliskTurrPos2 = turrDefault + 0.13; + redObeliskTurrPos3 = turrDefault + 0.14; + + blueObeliskTurrPos1 = turrDefault - 0.12; + blueObeliskTurrPos2 = turrDefault - 0.13; + blueObeliskTurrPos3 = turrDefault - 0.14; + + redTurretShootPos = turrDefault + 0.05; + + if (redAlliance) { + robot.light.setPosition(0.28); + + pickupGateX = rPickupGateX; + pickupGateY = rPickupGateY; + pickupGateH = rPickupGateH; + + pickupZoneX = rPickupZoneX; + pickupZoneY = rPickupZoneY; + pickupZoneH = rPickupZoneH; + + xShoot = rShootX; + yShoot = rShootY; + hShoot = rShootH; + + turretShootPos = redTurretShootPos; + + } else { + robot.light.setPosition(0.6); + + pickupGateX = bPickupGateX; + pickupGateY = bPickupGateY; + pickupGateH = bPickupGateH; + + pickupZoneX = bPickupZoneX; + pickupZoneY = bPickupZoneY; + pickupZoneH = bPickupZoneH; + + xShoot = bShootX; + yShoot = bShootY; + hShoot = bShootH; + + turretShootPos = blueTurretShootPos; + + } + + TELE.addData("Red?", redAlliance); + TELE.addData("Turret Default", turrDefault); + + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + robot.transfer.setPower(1); + + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shoot0Vel, + shoot0Hood, + 9, + 0.501, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease) + ); + + robot.frontLeft.setPower(-0.4); + robot.frontRight.setPower(-0.4); + robot.backLeft.setPower(-0.4); + robot.backRight.setPower(-0.4); + + sleep(fwdTime); + + robot.frontLeft.setPower(0); + robot.frontRight.setPower(0); + robot.backLeft.setPower(0); + robot.backRight.setPower(0); + + sleep(sleepTime); + + robot.frontLeft.setPower(-0.4); + robot.frontRight.setPower(0.4); + robot.backLeft.setPower(0.4); + robot.backRight.setPower(-0.4); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + sleep(strafeTime); + + robot.frontLeft.setPower(0); + robot.frontRight.setPower(0); + robot.backLeft.setPower(0); + robot.backRight.setPower(0); + + while (opModeIsActive()) { + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addLine("finished"); + TELE.update(); + } + + } + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Indexed.java deleted file mode 100644 index acb51c9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Indexed.java +++ /dev/null @@ -1,881 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.teamcode.utils.Spindexer; -import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; - -import java.util.Objects; - -@Config -@Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Far_Indexed extends LinearOpMode { - public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; - public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.015; - - public static double spindexerSpeedIncrease = 0.03; - public static double finalSpindexerSpeedIncrease = 0.025; - - - public static double redObeliskTurrPos1 = turrDefault + 0.12; - public static double redObeliskTurrPos2 = turrDefault + 0.13; - public static double redObeliskTurrPos3 = turrDefault + 0.14; - - public static double blueObeliskTurrPos1 = turrDefault - 0.12; - public static double blueObeliskTurrPos2 = turrDefault - 0.13; - public static double blueObeliskTurrPos3 = turrDefault - 0.14; - double obeliskTurrPos1 = 0.0; - double obeliskTurrPos2 = 0.0; - double obeliskTurrPos3 = 0.0; - public static double normalIntakeTime = 3.3; - public static double shoot1Turr = 0.57; - public static double shoot0XTolerance = 1.0; - public static double redTurretShootPos = turrDefault ; - public static double blueTurretShootPos = turrDefault ; - double turretShootPos = 0.0; - - public static double finalShootAllTime = 3.0; - public static double shootAllTime = 1.8; - public static double shoot0Time = 1.6; - public static double intake1Time = 3.3; - public static double intake2Time = 3.8; - - public static double intake3Time = 4.2; - - public static double flywheel0Time = 3.5; - public static double pickup1Speed = 23; - // ---- SECOND SHOT / PICKUP ---- - public static double shoot1Vel = 2300; - public static double shootAllVelocity = 2500; - public static double shootAllHood = 0.78 + hoodOffset; - // ---- PICKUP POSITION TOLERANCES ---- - public static double pickup1XTolerance = 2.0; - public static double pickup1YTolerance = 2.0; - // ---- OBELISK DETECTION ---- - public static double obelisk1Time = 1.5; - public static double obelisk1XTolerance = 2.0; - public static double obelisk1YTolerance = 2.0; - public static double shoot1ToleranceX = 2.0; - public static double shoot1ToleranceY = 2.0; - public static double shoot1Time = 2; - public static double shoot2Time = 2; - public static double shoot3Time = 2; - public static double colorSenseTime = 1.2; - - public static double firstShootTime = 0.3; - - public static int fwdTime = 200, strafeTime = 2300; - public int motif = 0; - - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - Servos servos; - Spindexer spindexer; - Flywheel flywheel; - Turret turret; - Targeting targeting; - Targeting.Settings targetingSettings; - private double firstSpindexShootPos = autoSpinStartPos; - private boolean shootForward = true; - private double x1, y1, h1; - - private double x2a, y2a, h2a, t2a; - - private double x2b, y2b, h2b, t2b; - private double x2c, y2c, h2c, t2c; - - private double x3a, y3a, h3a; - private double x3b, y3b, h3b; - private double x4a, y4a, h4a; - private double x4b, y4b, h4b; - - private double xShoot, yShoot, hShoot; - private double xGate, yGate, hGate; - private double xPrep, yPrep, hPrep; - - private double shoot1Tangent; - - private int driverSlotGreen = 0; - private int passengerSlotGreen = 0; - - private int rearSlotGreen = 0; - private int mostGreenSlot = 0; - - private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; - private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; - public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1; - public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1; - public static double rShootX = 1, rShootY = 1, rShootH = 1; - public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1; - public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1; - public static double bShootX = 1, bShootY = 1, bShootH = 1; - public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - double spindexerWiggle = 0.01; - - boolean decideGreenSlot = false; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - robot.transferServo.setPosition(transferServo_out); - - turret.manualSetTurret(turretShootPos); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.addData("motif", motif_id); - TELE.update(); - - if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { - - spindexerWiggle *= -1.0; - - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); - - spindexer.detectBalls(true, true); - - if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { - driverSlotGreen++; - } - - if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { - passengerSlotGreen++; - } - if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { - rearSlotGreen++; - } - - robot.intake.setPower(1); - - decideGreenSlot = true; - - return true; - } else if (decideGreenSlot) { - - if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) { - mostGreenSlot = 3; - } else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) { - mostGreenSlot = 2; - } else { - mostGreenSlot = 1; - } - - - - decideGreenSlot = false; - - if (motif_id == 21) { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; - } else { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } - } else if (motif_id == 22) { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } else { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = true; - } - - } else { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3b; - shootForward = true; - } else { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; - } - - } - - return true; - } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { - - TELE.addData("MostGreenSlot", mostGreenSlot); - - - robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); - - return true; - } else { - return false; - } - - } - }; - } - - public static int sleepTime = 1300; - - 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) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action shootAllAuto(double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = 0.0; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (getRuntime() - stamp < shootTime) { - - if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - - if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); - } - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - - spindexer.processIntake(); - robot.intake.setPower(1); - - spindexer.ballCounterLight(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); - - } - }; - } - - public Action detectObelisk( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance, - double turrPos - ) { - - 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(); - robot.limelight.pipelineSwitch(1); - } - - ticker++; - motif = turret.detectObelisk(); - - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); - - 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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - if (shouldFinish){ - if (redAlliance){ - robot.limelight.pipelineSwitch(4); - } else { - robot.limelight.pipelineSwitch(2); - } - return false; - } else { - return true; - } - - } - }; - } - - 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++; - - flywheel.manageFlywheel(vel); - robot.hood.setPosition(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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - public Action manageShooterAuto( - 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); - - turret.trackGoal(deltaPose); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - 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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - 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); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - 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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - flywheel = new Flywheel(hardwareMap); - - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0, 0.0); - - spindexer = new Spindexer(hardwareMap); - - servos = new Servos(hardwareMap); - - robot.limelight.start(); - - robot.limelight.pipelineSwitch(1); - - - turret = new Turret(robot, TELE, robot.limelight); - - turret.manualSetTurret(turrDefault); - - drive = new MecanumDrive(hardwareMap, teleEnd); - - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - - robot.transferServo.setPosition(transferServo_out); - - TrajectoryActionBuilder shoot0 = null; - TrajectoryActionBuilder pickupGate = null; - TrajectoryActionBuilder pickupLoadingZone = null; - TrajectoryActionBuilder shootFromGate = null; - TrajectoryActionBuilder shootFromZone = null; - - robot.light.setPosition(1); - - while (opModeInInit()) { - - robot.hood.setPosition(shoot0Hood); - turret.manualSetTurret(turretShootPos); - - if (gamepad2.crossWasPressed()) { - redAlliance = !redAlliance; - } - - if (gamepad2.dpadLeftWasPressed()) { - turrDefault -=0.01; - } - - if (gamepad2.dpadRightWasPressed()) { - turrDefault +=0.01; - } - - redObeliskTurrPos1 = turrDefault + 0.12; - redObeliskTurrPos2 = turrDefault + 0.13; - redObeliskTurrPos3 = turrDefault + 0.14; - - blueObeliskTurrPos1 = turrDefault - 0.12; - blueObeliskTurrPos2 = turrDefault - 0.13; - blueObeliskTurrPos3 = turrDefault - 0.14; - - redTurretShootPos = turrDefault + 0.05; - - - - if (redAlliance) { - robot.light.setPosition(0.28); - - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; - - pickupZoneX = rPickupZoneX; - pickupZoneY = rPickupZoneY; - pickupZoneH = rPickupZoneH; - - xShoot = rShootX; - yShoot = rShootY; - hShoot = rShootH; - - turretShootPos = redTurretShootPos; - - } else { - robot.light.setPosition(0.6); - - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; - - pickupZoneX = bPickupZoneX; - pickupZoneY = bPickupZoneY; - pickupZoneH = bPickupZoneH; - - xShoot = bShootX; - yShoot = bShootY; - hShoot = bShootH; - - turretShootPos = blueTurretShootPos; - - } - - - - - TELE.addData("Red?", redAlliance); - TELE.addData("Turret Default", turrDefault); - - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.transfer.setPower(1); - - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shoot0Vel, - shoot0Hood, - 9, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease) - ); - - robot.frontLeft.setPower(-0.4); - robot.frontRight.setPower(-0.4); - robot.backLeft.setPower(-0.4); - robot.backRight.setPower(-0.4); - - sleep (fwdTime); - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - sleep (sleepTime); - - robot.frontLeft.setPower(-0.4); - robot.frontRight.setPower(0.4); - robot.backLeft.setPower(0.4); - robot.backRight.setPower(-0.4); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - sleep (strafeTime); - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - while(opModeIsActive()) { - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addLine("finished"); - TELE.update(); - } - - - } - - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Red2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Red2.java deleted file mode 100644 index c58e7e9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Red2.java +++ /dev/null @@ -1,1013 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.teamcode.utils.Spindexer; -import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; - -import java.util.Objects; - -@Config -@Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Far_Red2 extends LinearOpMode { - public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; - public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.015; - - public static double spindexerSpeedIncrease = 0.03; - public static double finalSpindexerSpeedIncrease = 0.025; - - - public static double redObeliskTurrPos1 = turrDefault + 0.12; - public static double redObeliskTurrPos2 = turrDefault + 0.13; - public static double redObeliskTurrPos3 = turrDefault + 0.14; - - public static double blueObeliskTurrPos1 = turrDefault - 0.12; - public static double blueObeliskTurrPos2 = turrDefault - 0.13; - public static double blueObeliskTurrPos3 = turrDefault - 0.14; - double obeliskTurrPos1 = 0.0; - double obeliskTurrPos2 = 0.0; - double obeliskTurrPos3 = 0.0; - public static double normalIntakeTime = 3.3; - public static double shoot1Turr = 0.57; - public static double shoot0XTolerance = 1.0; - public static double redTurretShootPos = turrDefault ; - public static double blueTurretShootPos = turrDefault ; - double turretShootPos = 0.0; - - public static double finalShootAllTime = 3.0; - public static double shootAllTime = 1.8; - public static double shoot0Time = 1.6; - public static double intake1Time = 3.3; - public static double intake2Time = 3.8; - - public static double intake3Time = 4.2; - - public static double flywheel0Time = 3.5; - public static double pickup1Speed = 23; - // ---- SECOND SHOT / PICKUP ---- - public static double shoot1Vel = 2300; - public static double shootAllVelocity = 2500; - public static double shootAllHood = 0.78 + hoodOffset; - // ---- PICKUP POSITION TOLERANCES ---- - public static double pickup1XTolerance = 2.0; - public static double pickup1YTolerance = 2.0; - // ---- OBELISK DETECTION ---- - public static double obelisk1Time = 1.5; - public static double obelisk1XTolerance = 2.0; - public static double obelisk1YTolerance = 2.0; - public static double shoot1ToleranceX = 2.0; - public static double shoot1ToleranceY = 2.0; - public static double shoot1Time = 2; - public static double shoot2Time = 2; - public static double shoot3Time = 2; - public static double colorSenseTime = 1.2; - - public static double firstShootTime = 0.3; - - public static int fwdTime = 200, strafeTime = 4000, strafeRTime = 2500; - public int motif = 0; - - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - Servos servos; - Spindexer spindexer; - Flywheel flywheel; - Turret turret; - Targeting targeting; - Targeting.Settings targetingSettings; - private double firstSpindexShootPos = autoSpinStartPos; - private boolean shootForward = true; - private double x1, y1, h1; - - private double x2a, y2a, h2a, t2a; - - private double x2b, y2b, h2b, t2b; - private double x2c, y2c, h2c, t2c; - - private double x3a, y3a, h3a; - private double x3b, y3b, h3b; - private double x4a, y4a, h4a; - private double x4b, y4b, h4b; - - private double xShoot, yShoot, hShoot; - private double xGate, yGate, hGate; - private double xPrep, yPrep, hPrep; - - private double shoot1Tangent; - - private int driverSlotGreen = 0; - private int passengerSlotGreen = 0; - - private int rearSlotGreen = 0; - private int mostGreenSlot = 0; - - private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; - private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; - public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1; - public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1; - public static double rShootX = 1, rShootY = 1, rShootH = 1; - public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1; - public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1; - public static double bShootX = 1, bShootY = 1, bShootH = 1; - public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - double spindexerWiggle = 0.01; - - boolean decideGreenSlot = false; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - robot.transferServo.setPosition(transferServo_out); - - turret.manualSetTurret(turretShootPos); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.addData("motif", motif_id); - TELE.update(); - - if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { - - spindexerWiggle *= -1.0; - - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); - - spindexer.detectBalls(true, true); - - if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { - driverSlotGreen++; - } - - if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { - passengerSlotGreen++; - } - if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { - rearSlotGreen++; - } - - robot.intake.setPower(1); - - decideGreenSlot = true; - - return true; - } else if (decideGreenSlot) { - - if (driverSlotGreen >= passengerSlotGreen && driverSlotGreen >= rearSlotGreen) { - mostGreenSlot = 3; - } else if (passengerSlotGreen >= driverSlotGreen && passengerSlotGreen >= rearSlotGreen) { - mostGreenSlot = 2; - } else { - mostGreenSlot = 1; - } - - - - decideGreenSlot = false; - - if (motif_id == 21) { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; - } else { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } - } else if (motif_id == 22) { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } else { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = true; - } - - } else { - if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; - } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3b; - shootForward = true; - } else { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; - } - - } - - return true; - } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { - - TELE.addData("MostGreenSlot", mostGreenSlot); - - - robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); - - return true; - } else { - return false; - } - - } - }; - } - - public static int sleepTime = 1300; - - 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) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action shootAllAuto(double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = 0.0; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (getRuntime() - stamp < shootTime) { - - if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - - if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); - } - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = System.currentTimeMillis(); - } - ticker++; - - spindexer.processIntake(); - robot.intake.setPower(1); - - spindexer.ballCounterLight(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); - - } - }; - } - - public Action detectObelisk( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance, - double turrPos - ) { - - 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(); - robot.limelight.pipelineSwitch(1); - } - - ticker++; - motif = turret.detectObelisk(); - - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); - - 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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - if (shouldFinish){ - if (redAlliance){ - robot.limelight.pipelineSwitch(4); - } else { - robot.limelight.pipelineSwitch(2); - } - return false; - } else { - return true; - } - - } - }; - } - - 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++; - - flywheel.manageFlywheel(vel); - robot.hood.setPosition(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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - public Action manageShooterAuto( - 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); - - turret.trackGoal(deltaPose); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - 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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - public Action fwd(){ - 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++; - - if (System.currentTimeMillis() - stamp < strafeTime) { - robot.frontLeft.setPower(0.4); - robot.frontRight.setPower(0.4); - robot.backLeft.setPower(0.4); - robot.backRight.setPower(0.4); - - return true; - } else { - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - return false; - } - - } - }; - } - - public Action bwd(){ - 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++; - - if (System.currentTimeMillis() - stamp < strafeRTime) { - robot.frontLeft.setPower(-0.4); - robot.frontRight.setPower(-0.4); - robot.backLeft.setPower(-0.4); - robot.backRight.setPower(-0.4); - - return true; - } else { - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - return false; - } - - } - }; - } - - - 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); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - 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; - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - - return !shouldFinish; - - } - }; - } - - public static double angle = 90; - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - flywheel = new Flywheel(hardwareMap); - - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0, 0.0); - - spindexer = new Spindexer(hardwareMap); - - servos = new Servos(hardwareMap); - - robot.limelight.start(); - - robot.limelight.pipelineSwitch(1); - - - turret = new Turret(robot, TELE, robot.limelight); - - turret.manualSetTurret(turrDefault); - - drive = new MecanumDrive(hardwareMap, teleEnd); - - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - - robot.transferServo.setPosition(transferServo_out); - - TrajectoryActionBuilder shoot0 = null; - TrajectoryActionBuilder pickupGate = null; - TrajectoryActionBuilder pickupLoadingZone = null; - TrajectoryActionBuilder shootFromGate = null; - TrajectoryActionBuilder shootFromZone = null; - - robot.light.setPosition(1); - - while (opModeInInit()) { - - robot.hood.setPosition(shoot0Hood); - turret.manualSetTurret(turretShootPos); - - if (gamepad2.crossWasPressed()) { - redAlliance = !redAlliance; - } - - if (gamepad2.dpadLeftWasPressed()) { - turrDefault -=0.01; - } - - if (gamepad2.dpadRightWasPressed()) { - turrDefault +=0.01; - } - - redObeliskTurrPos1 = turrDefault + 0.12; - redObeliskTurrPos2 = turrDefault + 0.13; - redObeliskTurrPos3 = turrDefault + 0.14; - - blueObeliskTurrPos1 = turrDefault - 0.12; - blueObeliskTurrPos2 = turrDefault - 0.13; - blueObeliskTurrPos3 = turrDefault - 0.14; - - redTurretShootPos = turrDefault + 0.05; - - - - if (redAlliance) { - robot.light.setPosition(0.28); - - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; - - pickupZoneX = rPickupZoneX; - pickupZoneY = rPickupZoneY; - pickupZoneH = rPickupZoneH; - - xShoot = rShootX; - yShoot = rShootY; - hShoot = rShootH; - - turretShootPos = redTurretShootPos; - - } else { - robot.light.setPosition(0.6); - - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; - - pickupZoneX = bPickupZoneX; - pickupZoneY = bPickupZoneY; - pickupZoneH = bPickupZoneH; - - xShoot = bShootX; - yShoot = bShootY; - hShoot = bShootH; - - turretShootPos = blueTurretShootPos; - - } - - - - - TELE.addData("Red?", redAlliance); - TELE.addData("Turret Default", turrDefault); - - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.transfer.setPower(1); - - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shoot0Vel, - shoot0Hood, - 5.5, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - Actions.runBlocking( - shootAll((int) shoot0Vel, 3.5, shoot0SpinSpeedIncrease) - ); - - robot.frontLeft.setPower(-0.4); - robot.frontRight.setPower(-0.4); - robot.backLeft.setPower(-0.4); - robot.backRight.setPower(-0.4); - - sleep (fwdTime); - - robot.frontLeft.setPower(0); - robot.frontRight.setPower(0); - robot.backLeft.setPower(0); - robot.backRight.setPower(0); - - sleep (sleepTime); - - drive.updatePoseEstimate(); - - TrajectoryActionBuilder turn = drive.actionBuilder(drive.localizer.getPose()) - .turn(Math.toRadians(angle)); - - Actions.runBlocking( - turn.build() - ); - - sleep(sleepTime); - - drive.updatePoseEstimate(); - - - Actions.runBlocking( - new ParallelAction( - fwd(), - manageShooterAuto( - intake1Time, - 0.501, - 0.501, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - manageFlywheel( - shoot0Vel, - shoot0Hood, - intake1Time, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - sleep (sleepTime); - - drive.updatePoseEstimate(); - - TrajectoryActionBuilder comeBack = drive.actionBuilder(drive.localizer.getPose()) - .strafeToLinearHeading(new Vector2d(teleEnd.position.x, teleEnd.position.y), teleEnd.heading.toDouble()); - - Actions.runBlocking( - new ParallelAction( - bwd(), - manageShooterAuto( - intake2Time, - 0.501, - 0.501, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake2Time), - manageFlywheel( - shoot0Vel, - shoot0Hood, - intake2Time, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - Actions.runBlocking( - shootAll((int) shoot0Vel, 3.5, shoot0SpinSpeedIncrease) - ); - - - while(opModeIsActive()) { - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addLine("finished"); - TELE.update(); - } - - - } - - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index c2047ac..fbdeb9a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -6,19 +6,11 @@ import com.acmerobotics.roadrunner.Pose2d; @Config public class Poses { - public static double goalHeight = 42; //in inches - - public static double turretHeight = 12; - - public static double relativeGoalHeight = goalHeight - turretHeight; - - public static Pose2d goalPose = new Pose2d(-10, 0, 0); public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1); public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1); - public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); @@ -32,7 +24,6 @@ public class Poses { public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1); public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140); public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1); - public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140); public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140); public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java index 8107157..53fabef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java @@ -1,15 +1,10 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; @Config public class Poses_V2 { - public static double goalHeight = 42; //in inches - - public static double turretHeight = 12; - public static double rXGateA = 35.5, rYGateA = 46.5, rHGateA = 2.7; public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836; @@ -21,5 +16,4 @@ public class Poses_V2 { public static double bXGate = 28, bYGate = -65, bHGate = Math.toRadians(-179); - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index fbc4fa3..0acbe4e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -19,25 +19,17 @@ public class ServoPositions { public static double spinStartPos = 0.22; public static double spinEndPos = 0.85; - public static double shootAllAutoSpinStartPos = 0.2; public static double shootAllSpindexerSpeedIncrease = 0.014; - public static double shootAllTime = 1.8; public static double transferServo_out = 0.15; public static double transferServo_in = 0.38; - public static double turret_range = 0.9; - - public static double hoodDefault = 0.6; - public static double hoodAuto = 0.27; -public static double hoodOffset = -0.05; + public static double hoodOffset = -0.05; public static double turret_redClose = 0.42; public static double turret_blueClose = 0.38; - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index 966316e..bd096b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -4,23 +4,8 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ShooterVars { - public static double turret_GearRatio = 0.9974; - - public static double turret_Range = 355; - - public static int velTolerance = 300; - - public static int initTolerance = 1000; - - public static int maxVel = 4500; - public static double waitTransferOut = 0.3; - public static double waitTransfer = 0.4; - public static double kP = 0.001; // small proportional gain (tune this) - public static double maxStep = 0.06; // prevents sudden jumps // VELOCITY CONSTANTS public static int AUTO_CLOSE_VEL = 3175; //3300; - public static int AUTO_FAR_VEL = 4000; //TODO: test this - public static Types.Motif currentMotif = Types.Motif.NONE; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Test.java deleted file mode 100644 index 5d64a39..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Test.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -@Config -@TeleOp -public class Test extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - - } -} 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 2074ecd..ff1f0f6 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 @@ -9,6 +9,7 @@ import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; @@ -16,6 +17,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import java.util.List; @Config +@TeleOp public class Turret { public static double turretTolerance = 0.02;