From 26c7c779f97ba5c89e6a898e676766f7daf9423a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 3 Feb 2026 21:56:29 -0600 Subject: [PATCH] added calibration to auto init --- .../Auto_LT_Close_12Ball_Indexed.java | 92 ++-- .../autonomous/Auto_LT_Far_3Ball.java | 475 +++++++++++++++--- .../ftc/teamcode/teleop/TeleopV3.java | 9 - 3 files changed, 453 insertions(+), 123 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java index 766378e..7626e53 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java @@ -103,22 +103,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double spindexerSpeedIncrease = 0.03; public static double finalSpindexerSpeedIncrease = 0.025; + // These values are ADDED to turrDefault + public static double redObeliskTurrPos1 = 0.12; + public static double redObeliskTurrPos2 = 0.13; + public static double redObeliskTurrPos3 = 0.14; + public static double blueObeliskTurrPos1 = -0.12; + public static double blueObeliskTurrPos2 = -0.13; + public static double blueObeliskTurrPos3 = -0.14; + public static double redTurretShootPos = 0.12; + public static double blueTurretShootPos = -0.14; - 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 + 0.12; - public static double blueTurretShootPos = turrDefault - 0.14; double turretShootPos = 0.0; public static double finalShootAllTime = 3.0; @@ -231,6 +231,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { passengerSlotGreen++; } + if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { rearSlotGreen++; } @@ -250,8 +251,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { mostGreenSlot = 1; } - - decideGreenSlot = false; if (motif_id == 21) { @@ -293,10 +292,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { return true; } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { - - TELE.addData("MostGreenSlot", mostGreenSlot); - - +// TELE.addData("MostGreenSlot", mostGreenSlot); +// TELE.update(); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); robot.spin1.setPosition(firstSpindexShootPos); @@ -314,11 +311,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { 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 @@ -363,7 +357,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { } else { robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; spindexer.resetSpindexer(); spindexer.processIntake(); @@ -434,7 +427,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { } else { robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; spindexer.resetSpindexer(); spindexer.processIntake(); @@ -470,7 +462,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { TELE.update(); return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); - } }; } @@ -783,26 +774,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { } if (gamepad2.dpadLeftWasPressed()) { - turrDefault -=0.01; + turrDefault -= 0.01; } if (gamepad2.dpadRightWasPressed()) { - turrDefault +=0.01; + 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.12; - blueTurretShootPos = turrDefault - 0.14; - - - if (redAlliance) { robot.light.setPosition(0.28); @@ -837,10 +815,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - obeliskTurrPos1 = redObeliskTurrPos1; - obeliskTurrPos2 = redObeliskTurrPos2; - obeliskTurrPos3 = redObeliskTurrPos3; - turretShootPos = redTurretShootPos; + obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; + obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; + obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; + turretShootPos = turrDefault + redTurretShootPos; } else { robot.light.setPosition(0.6); @@ -877,10 +855,10 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - obeliskTurrPos1 = blueObeliskTurrPos1; - obeliskTurrPos2 = blueObeliskTurrPos2; - obeliskTurrPos3 = blueObeliskTurrPos3; - turretShootPos = blueTurretShootPos; + obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; + obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; + obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; + turretShootPos = turrDefault + blueTurretShootPos; } @@ -913,8 +891,6 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); - - TELE.update(); } @@ -975,9 +951,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { motif = turret.getObeliskID(); - if (motif == 0) motif = 22; - + int prevMotif = motif; Actions.runBlocking( new ParallelAction( @@ -1033,6 +1008,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { ) ); + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + Actions.runBlocking( new ParallelAction( manageFlywheelAuto( @@ -1084,6 +1064,11 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { ) ); + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + Actions.runBlocking( new ParallelAction( manageFlywheelAuto( @@ -1112,14 +1097,15 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { ); - drive.updatePoseEstimate(); + while (opModeIsActive()) { - teleStart = drive.localizer.getPose(); + drive.updatePoseEstimate(); - TELE.addLine("finished"); - TELE.update(); + teleStart = drive.localizer.getPose(); - sleep(2000); + TELE.addLine("finished"); + TELE.update(); + } } 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 index 12dab12..3cac5a7 100644 --- 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 @@ -4,6 +4,11 @@ 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; @@ -29,24 +34,17 @@ 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_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 double redTurretShootPos = 0.05; + public static double blueTurretShootPos = -0.05; 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; @@ -75,15 +73,134 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { private int mostGreenSlot = 0; private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; + public static double firstShootTime = 0.3; + + 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); +// TELE.update(); + robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); + + robot.spin1.setPosition(firstSpindexShootPos); + robot.spin2.setPosition(1 - firstSpindexShootPos); + + return true; + } else { + return false; + } + + } + }; + } 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 @@ -128,7 +245,6 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { } else { robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; spindexer.resetSpindexer(); spindexer.processIntake(); @@ -141,6 +257,75 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { }; } + 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); + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + } + }; + } + public Action intake(double intakeTime) { return new Action() { double stamp = 0.0; @@ -165,6 +350,67 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { 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; + } } }; @@ -222,6 +468,145 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { }; } + 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 { @@ -248,7 +633,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { turret.manualSetTurret(turrDefault); - drive = new MecanumDrive(hardwareMap, teleEnd); + drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); robot.spin1.setPosition(autoSpinStartPos); robot.spin2.setPosition(1 - autoSpinStartPos); @@ -259,6 +644,13 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { while (opModeInInit()) { + // Recalibration in initialization + drive.updatePoseEstimate(); + if (gamepad2.square) { + teleEnd = drive.localizer.getPose(); // use this position as starting position + gamepad2.rumble(1000); + } + robot.hood.setPosition(shoot0Hood); turret.manualSetTurret(turretShootPos); @@ -274,16 +666,6 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { 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); @@ -299,8 +681,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { yShoot = rShootY; hShoot = rShootH; - turretShootPos = redTurretShootPos; - + turretShootPos = turrDefault + redTurretShootPos; } else { robot.light.setPosition(0.6); @@ -316,13 +697,13 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { yShoot = bShootY; hShoot = bShootH; - turretShootPos = blueTurretShootPos; - + turretShootPos = turrDefault + blueTurretShootPos; } TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); - + TELE.addData("Start Position", teleEnd); + TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); } @@ -330,6 +711,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { if (isStopRequested()) return; + // Currently only shoots; keep this start and modify times and then add extra paths if (opModeIsActive()) { robot.transfer.setPower(1); @@ -357,36 +739,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { 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); - + // Actual way to end autonomous in to find final position while (opModeIsActive()) { drive.updatePoseEstimate(); 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 d406445..34e42c9 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 @@ -284,20 +284,11 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.left_stick_button) { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; - shootAll = false; spindexer.resetSpindexer(); } } - if (gamepad2.square) { - drive.updatePoseEstimate(); - - teleEnd = drive.localizer.getPose(); - gamepad2.rumble(1000); - - } - //EXTRA STUFFINESS: drive.updatePoseEstimate();