From 26c7c779f97ba5c89e6a898e676766f7daf9423a Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 3 Feb 2026 21:56:29 -0600 Subject: [PATCH 01/14] 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(); From e4dd2147d6e9f8b5cc2039dbbe4749210bdcac47 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 6 Feb 2026 21:39:23 -0600 Subject: [PATCH 02/14] bunch of minor changes plus major change in running auton with customizable settings --- ...12Ball_Indexed.java => Auto_LT_Close.java} | 556 ++++++++++-------- .../autonomous/Auto_LT_Close_12Ball.java | 111 ++-- .../autonomous/Auto_LT_Close_GateOpen.java | 112 ++-- ...uto_LT_Far_3Ball.java => Auto_LT_Far.java} | 93 +-- .../disabled/ProtoAutoClose_V3.java | 86 +-- .../ftc/teamcode/constants/Back_Poses.java | 10 + .../ftc/teamcode/constants/Front_Poses.java | 52 ++ .../ftc/teamcode/constants/Poses.java | 49 -- .../ftc/teamcode/teleop/TeleopV3.java | 31 +- .../ftc/teamcode/utils/Robot.java | 7 +- .../ftc/teamcode/utils/Targeting.java | 1 + 11 files changed, 607 insertions(+), 501 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_LT_Close_12Ball_Indexed.java => Auto_LT_Close.java} (64%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_LT_Far_3Ball.java => Auto_LT_Far.java} (90%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java 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.java similarity index 64% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 7626e53..bb5a7b0 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.java @@ -1,62 +1,69 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; 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; @@ -64,6 +71,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ 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.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -82,7 +90,6 @@ 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.constants.Poses_V2; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -95,7 +102,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { +public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; @@ -110,7 +117,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { 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 redTurretShootPos = 0.1; public static double blueTurretShootPos = -0.14; double obeliskTurrPos1 = 0.0; @@ -163,21 +170,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { Targeting.Settings targetingSettings; private double firstSpindexShootPos = autoSpinStartPos; private boolean shootForward = true; - private double x1, y1, h1; + double x1, y1, h1; - private double x2a, y2a, h2a, t2a; + double x2a, y2a, h2a, t2a; - private double x2b, y2b, h2b, t2b; - private double x2c, y2c, h2c, t2c; + double x2b, y2b, h2b, t2b; + 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; + double x3a, y3a, h3a; + double x3b, y3b, h3b; + double x4a, y4a, h4a; + double x4b, y4b, h4b; - private double xShoot, yShoot, hShoot; - private double xGate, yGate, hGate; - private double xPrep, yPrep, hPrep; + double xShoot, yShoot, hShoot; + double xGate, yGate, hGate; + double xPrep, yPrep, hPrep; + double xLeave, yLeave, hLeave; private double shoot1Tangent; @@ -186,6 +194,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { private int rearSlotGreen = 0; private int mostGreenSlot = 0; + int ballCycles = 3; + int prevMotif = 0; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -340,7 +350,25 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { teleStart = drive.localizer.getPose(); - if (getRuntime() - stamp < shootTime) { + 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 ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { robot.spin1.setPosition(autoSpinStartPos); @@ -403,6 +431,24 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { teleStart = drive.localizer.getPose(); + 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 (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { @@ -457,11 +503,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { 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); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; } @@ -781,6 +824,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { turrDefault += 0.01; } + if (gamepad2.rightBumperWasPressed()){ + ballCycles++; + } + if (gamepad2.leftBumperWasPressed()){ + ballCycles--; + } + if (redAlliance) { robot.light.setPosition(0.28); @@ -814,6 +864,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { xShoot = rShootX; yShoot = rShootY; hShoot = rShootH; + xLeave = rLeaveX; + yLeave = rLeaveY; + hLeave = rLeaveH; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -854,6 +907,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { xShoot = bShootX; yShoot = bShootY; hShoot = bShootH; + xLeave = bLeaveX; + yLeave = bLeaveY; + hLeave = bLeaveH; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -863,33 +919,45 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { } shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1); + .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) - .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), new TranslationalVelConstraint(pickup1Speed)); - shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + if (ballCycles < 2){ + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + } - pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(x3a, y3a), h3a) - .strafeToLinearHeading(new Vector2d(x3b, y3b), h3b, + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) + .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), new TranslationalVelConstraint(pickup1Speed)); - shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + if (ballCycles < 3){ + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave)); + } - pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) - .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a)) + .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b), new TranslationalVelConstraint(pickup1Speed)); - shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); + TELE.addData("Ball Cycles", ballCycles); TELE.update(); } @@ -924,178 +992,184 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) ); - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - intake1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - detectObelisk( - intake1Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos1 - ) + if (ballCycles > 0){ + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shootAllVelocity, + shootAllHood, + intake1Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time), + detectObelisk( + intake1Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos1 + ) - ) - ); + ) + ); - motif = turret.getObeliskID(); + motif = turret.getObeliskID(); - if (motif == 0) motif = 22; - int prevMotif = motif; + if (motif == 0) motif = 22; + prevMotif = motif; - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shootAllVelocity, - shootAllHood, - shoot1Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot1.build(), - prepareShootAll(colorSenseTime, shoot1Time, motif) - ) - ); + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shootAllVelocity, + shootAllHood, + shoot1Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot1.build(), + prepareShootAll(colorSenseTime, shoot1Time, motif) + ) + ); - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) - ); + ); + } - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - manageShooterAuto( - intake2Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake2Time), - detectObelisk( - intake2Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos2 - ) + if (ballCycles > 1){ + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + manageShooterAuto( + intake2Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake2Time), + detectObelisk( + intake2Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos2 + ) - ) - ); + ) + ); - motif = turret.getObeliskID(); + motif = turret.getObeliskID(); - if (motif == 0) motif = prevMotif; - prevMotif = motif; + if (motif == 0) motif = prevMotif; + prevMotif = motif; - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot2Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot2.build(), - prepareShootAll(colorSenseTime, shoot2Time, motif) - ) - ); + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot2Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot2.build(), + prepareShootAll(colorSenseTime, shoot2Time, motif) + ) + ); - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) - ); + ); + } - Actions.runBlocking( - new ParallelAction( - pickup3.build(), - manageShooterAuto( - intake3Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake3Time), - detectObelisk( - intake3Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos3 - ) + if (ballCycles > 2){ + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + intake3Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake3Time), + detectObelisk( + intake3Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos3 + ) - ) - ); + ) + ); - motif = turret.getObeliskID(); + motif = turret.getObeliskID(); - if (motif == 0) motif = prevMotif; - prevMotif = motif; + if (motif == 0) motif = prevMotif; + prevMotif = motif; - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot3.build(), - prepareShootAll(colorSenseTime, shoot3Time, motif) - ) - ); + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(colorSenseTime, shoot3Time, motif) + ) + ); - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - finalShootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) - ) + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + finalShootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) + ) - ); + ); + } while (opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index 56376fe..ad11f86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -1,61 +1,61 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; @@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.Poses_V2; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java index 268855c..86cd3b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java @@ -1,61 +1,61 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; 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; @@ -78,6 +78,7 @@ 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.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Poses_V2; @@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.Objects; +@Disabled @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close_GateOpen extends LinearOpMode { 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.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 3cac5a7..fa0a85d 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.java @@ -1,9 +1,16 @@ package org.firstinspires.ftc.teamcode.autonomous; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY; 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.Front_Poses.teleEnd; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; 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; @@ -11,6 +18,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ 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.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -22,6 +30,8 @@ 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; @@ -38,7 +48,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Far_3Ball extends LinearOpMode { +public class Auto_LT_Far extends LinearOpMode { public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; @@ -46,12 +56,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { 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; - 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; + double xLeave, yLeave, hLeave; public static int sleepTime = 1300; public int motif = 0; double turretShootPos = 0.0; @@ -228,7 +233,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { teleStart = drive.localizer.getPose(); - if (getRuntime() - stamp < shootTime) { + 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 ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { robot.spin1.setPosition(autoSpinStartPos); @@ -345,11 +368,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { 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); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; } @@ -642,6 +662,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { robot.light.setPosition(1); + TrajectoryActionBuilder leave = null; + while (opModeInInit()) { // Recalibration in initialization @@ -669,37 +691,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { 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; + xLeave = rLeaveX; + yLeave = rLeaveY; + hLeave = rLeaveH; turretShootPos = turrDefault + 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; + xLeave = bLeaveX; + yLeave = bLeaveY; + hLeave = bLeaveH; turretShootPos = turrDefault + blueTurretShootPos; } + leave = drive.actionBuilder(teleEnd) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Start Position", teleEnd); @@ -721,7 +731,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { manageFlywheel( shoot0Vel, shoot0Hood, - 9, + 5, 0.501, 0.501, shoot0XTolerance, @@ -736,9 +746,16 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { teleStart = drive.localizer.getPose(); Actions.runBlocking( - shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease) + shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease) ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + assert leave != null; + Actions.runBlocking(leave.build()); + // Actual way to end autonomous in to find final position while (opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java index d2b5214..755b71f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java @@ -1,49 +1,49 @@ package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java new file mode 100644 index 0000000..7927364 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class Back_Poses { + public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50; + public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java new file mode 100644 index 0000000..43e18df --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; + +@Config +public class Front_Poses { + + + public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; + public static double rx2a = 41, ry2a = 18, rh2a = 140; + public static double rx2b = 19, ry2b = 40, rh2b = 140; + + + public static double rx3a = 55, ry3a = 39, rh3a = 140; + + public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; + + public static double rx3b = 36, ry3b = 58, rh3b = 140.1; + + public static double rx4a = 80, ry4a = 48, rh4a = 140; + public static double rx4b = 45, ry4b = 83, rh4b = 140.1; + + public static double bx1 = 20, by1 = 0.5, bh1 = 0.1; + public static double bx2a = 41, by2a = -18, bh2a = -140; + public static double bx2b = 19, by2b = -40, bh2b = -140.1; + + public static double bx3a = 55, by3a = -39, bh3a = -140; + public static double bx3b = 41, by3b = -59, bh3b = -140.1; + public static double bx3aG = 55, by3aG = -43, bh3aG = -140; + + public static double bx4a = 75, by4a = -53, bh4a = -140; + public static double bx4b = 47, by4b = -85, bh4b = -140.1; + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; + + public static double rShootX = 40, rShootY = 10, rShootH = 50; + public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; + + public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; + + public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; + + public static double bShootX = 40, bShootY = 0, bShootH = -50; + public static double bxPrep = 45, byPrep = -10, bhPrep = -50; + + + public static Pose2d teleStart = new Pose2d(0, 0, 0); + + public static Pose2d teleEnd = new Pose2d(0, 0, 0); + + +} 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 deleted file mode 100644 index fbdeb9a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; - -@Config -public class Poses { - - - 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 rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); - - public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140); - - public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1); - - public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140); - public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1); - - 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 bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140); - public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1); - public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140); - - - public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140); - public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1); - public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - - public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50); - public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50); - - public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50); - public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50); - - - public static Pose2d teleStart = new Pose2d(0, 0, 0); - - public static Pose2d teleEnd = new Pose2d(0, 0, 0); - - -} 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 34e42c9..e32e81f 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 @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.teleop; 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.Front_Poses.teleStart; 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.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import com.acmerobotics.dashboard.FtcDashboard; @@ -70,8 +70,6 @@ public class TeleopV3 extends LinearOpMode { boolean overrideTurr = false; int intakeTicker = 0; - - boolean turretInterpolate = false; private boolean shootAll = false; @Override @@ -141,7 +139,7 @@ public class TeleopV3 extends LinearOpMode { robot.transfer.setPower(1); - double offset; + //TURRET TRACKING double robX = drive.localizer.getPose().position.x; double robY = drive.localizer.getPose().position.y; @@ -318,18 +316,19 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("autoSpintake", autoSpintake); // - TELE.addData("shootall commanded", shootAll); +// TELE.addData("shootall commanded", shootAll); // Targeting Debug - TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); - TELE.addData("robotInchesX", targeting.robotInchesX); - TELE.addData( "robotInchesY", targeting.robotInchesY); - TELE.addData("Targeting Interpolate", turretInterpolate); - TELE.addData("Targeting GridX", targeting.robotGridX); - TELE.addData("Targeting GridY", targeting.robotGridY); - TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); - TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); - TELE.addData("timeSinceStamp", getRuntime() - shootStamp); +// TELE.addData("robotX", robotX); +// TELE.addData("robotY", robotY); +// TELE.addData("robotInchesX", targeting.robotInchesX); +// TELE.addData( "robotInchesY", targeting.robotInchesY); +// TELE.addData("Targeting Interpolate", turretInterpolate); +// TELE.addData("Targeting GridX", targeting.robotGridX); +// TELE.addData("Targeting GridY", targeting.robotGridY); +// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); +// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); +// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); + TELE.addData("Voltage", robot.voltage.getVoltage()); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 7b07256..f5716a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; @@ -56,11 +57,11 @@ public class Robot { public RevColorSensorV3 color3; public Limelight3A limelight; public Servo light; + public VoltageSensor voltage; public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map - //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); frontRight = hardwareMap.get(DcMotorEx.class, "fr"); backLeft = hardwareMap.get(DcMotorEx.class, "bl"); @@ -78,7 +79,7 @@ public class Robot { shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); - //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -96,7 +97,6 @@ public class Robot { turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port - //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer spin1 = hardwareMap.get(Servo.class, "spin2"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); @@ -128,5 +128,6 @@ public class Robot { } light = hardwareMap.get(Servo.class, "light"); + voltage = hardwareMap.voltageSensor.iterator().next(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 6b32de0..0ca44b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -77,6 +77,7 @@ public class Targeting { double cancelOffsetY = 0.0; // was 7.0 double unitConversionFactor = 0.95; int tileSize = 24; //inches + public static boolean turretInterpolate = false; public Targeting() { } From 0d483f2a63527b2401f872516c0aafd3687e7971 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 7 Feb 2026 15:48:04 -0600 Subject: [PATCH 03/14] Progress update: some changes to auto and voltage added to flywheel (tested briefly, need more tests to verify) --- .../teamcode/autonomous/Auto_LT_Close.java | 40 ++++++++++++++---- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 2 + .../ftc/teamcode/constants/Front_Poses.java | 41 ++++++++----------- .../ftc/teamcode/teleop/TeleopV3.java | 12 +++--- .../ftc/teamcode/tests/ShooterTest.java | 9 ++-- .../ftc/teamcode/utils/Robot.java | 2 +- 6 files changed, 64 insertions(+), 42 deletions(-) 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 bb5a7b0..92673eb 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 @@ -72,6 +72,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ 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.Targeting.turretInterpolate; +import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -137,7 +138,7 @@ public class Auto_LT_Close extends LinearOpMode { public static double intake3Time = 4.2; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 23; + public static double pickup1Speed = 15; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shootAllVelocity = 2500; @@ -214,12 +215,28 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + 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); + TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Hood", robot.hood.getPosition()); TELE.addData("motif", motif_id); @@ -504,6 +521,9 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; @@ -780,11 +800,6 @@ public class Auto_LT_Close extends LinearOpMode { servos = new Servos(hardwareMap); - robot.limelight.start(); - - robot.limelight.pipelineSwitch(1); - - turret = new Turret(robot, TELE, robot.limelight); turret.manualSetTurret(turrDefault); @@ -796,6 +811,8 @@ public class Auto_LT_Close extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); + limelightUsed = false; + TrajectoryActionBuilder shoot0 = null; TrajectoryActionBuilder pickup1 = null; TrajectoryActionBuilder shoot1 = null; @@ -831,6 +848,11 @@ public class Auto_LT_Close extends LinearOpMode { ballCycles--; } + if (gamepad2.squareWasPressed()){ + robot.limelight.start(); + robot.limelight.pipelineSwitch(1); + } + if (redAlliance) { robot.light.setPosition(0.28); @@ -1177,6 +1199,8 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); + flywheel.manageFlywheel(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 fa0a85d..8e9709a 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 @@ -763,6 +763,8 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); + flywheel.manageFlywheel(0); + TELE.addLine("finished"); TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index 43e18df..bb7d29b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -8,45 +8,40 @@ public class Front_Poses { public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; - public static double rx2a = 41, ry2a = 18, rh2a = 140; - public static double rx2b = 19, ry2b = 40, rh2b = 140; - - - public static double rx3a = 55, ry3a = 39, rh3a = 140; - - public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; - - public static double rx3b = 36, ry3b = 58, rh3b = 140.1; - - public static double rx4a = 80, ry4a = 48, rh4a = 140; - public static double rx4b = 45, ry4b = 83, rh4b = 140.1; - public static double bx1 = 20, by1 = 0.5, bh1 = 0.1; + + public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double bx2a = 41, by2a = -18, bh2a = -140; + + public static double rx2b = 23, ry2b = 36, rh2b = 140.1; public static double bx2b = 19, by2b = -40, bh2b = -140.1; + public static double rx3a = 55, ry3a = 39, rh3a = 140; public static double bx3a = 55, by3a = -39, bh3a = -140; - public static double bx3b = 41, by3b = -59, bh3b = -140.1; + + public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; public static double bx3aG = 55, by3aG = -43, bh3aG = -140; + public static double rx3b = 36, ry3b = 58, rh3b = 140.1; + public static double bx3b = 41, by3b = -59, bh3b = -140.1; + + public static double rx4a = 75, ry4a = 53, rh4a = 140; public static double bx4a = 75, by4a = -53, bh4a = -140; + + public static double rx4b = 50, ry4b = 78, rh4b = 140.1; public static double bx4b = 47, by4b = -85, bh4b = -140.1; + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; public static double rShootX = 40, rShootY = 10, rShootH = 50; - public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; - - public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; - - public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; - public static double bShootX = 40, bShootY = 0, bShootH = -50; + + public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50; + public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; + public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); - public static Pose2d teleEnd = new Pose2d(0, 0, 0); - - } 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 e32e81f..c09cafd 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 @@ -97,7 +97,10 @@ public class TeleopV3 extends LinearOpMode { tController.setTolerance(0.001); Turret turret = new Turret(robot, TELE, robot.limelight); + limelightUsed = true; + robot.light.setPosition(1); + while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { @@ -107,9 +110,6 @@ public class TeleopV3 extends LinearOpMode { } } - limelightUsed= true; - - waitForStart(); if (isStopRequested()) return; @@ -186,6 +186,8 @@ public class TeleopV3 extends LinearOpMode { } //SHOOTER: + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); //HOOD: @@ -225,8 +227,6 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); } - - if (enableSpindexerManager) { //if (!shootAll) { spindexer.processIntake(); @@ -328,7 +328,7 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); - TELE.addData("Voltage", robot.voltage.getVoltage()); + TELE.addData("Voltage", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub) TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index d45ffc0..63e85bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -26,12 +26,11 @@ public class ShooterTest extends LinearOpMode { public static double parameter = 0.0; // --- CONSTANTS YOU TUNE --- - //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED public static double Velocity = 0.0; public static double P = 255.0; public static double I = 0.0; public static double D = 0.0; - public static double F = 7.5; + public static double F = 90; public static double transferPower = 1.0; public static double hoodPos = 0.501; public static double turretPos = 0.501; @@ -73,11 +72,13 @@ public class ShooterTest extends LinearOpMode { while (opModeIsActive()) { + double voltage = robot.voltage.getVoltage(); + if (mode == 0) { rightShooter.setPower(parameter); leftShooter.setPower(parameter); } else if (mode == 1) { - flywheel.setPIDF(P, I, D, F); + flywheel.setPIDF(P, I, D, F / voltage); flywheel.manageFlywheel((int) Velocity); } @@ -109,7 +110,6 @@ public class ShooterTest extends LinearOpMode { //intake = false; //reject = false; - // TODO: Change starting position based on desired order to shoot green ball //spindexPos = spindexer_intakePos1; if (getRuntime() - shootStamp < 3.5) { @@ -150,6 +150,7 @@ public class ShooterTest extends LinearOpMode { TELE.addData("Power", robot.shooter1.getPower()); TELE.addData("Steady?", flywheel.getSteady()); TELE.addData("Position", robot.shooter1.getCurrentPosition()); + TELE.addData("Voltage", voltage); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index f5716a4..4ce0461 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -34,7 +34,7 @@ public class Robot { public double shooterPIDF_P = 255.0; public double shooterPIDF_I = 0.0; public double shooterPIDF_D = 0.0; - public double shooterPIDF_F = 7.5; + public double shooterPIDF_F = 90; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; From de9ce388c427fd7649d39cc20a27a392c84a001c Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 7 Feb 2026 16:00:10 -0600 Subject: [PATCH 04/14] made it so each cycle was its own void --- .../teamcode/autonomous/Auto_LT_Close.java | 416 +++++++++--------- 1 file changed, 216 insertions(+), 200 deletions(-) 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 92673eb..43321d4 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 @@ -782,6 +782,15 @@ public class Auto_LT_Close extends LinearOpMode { }; } + // initialize path variables here + TrajectoryActionBuilder shoot0 = null; + TrajectoryActionBuilder pickup1 = null; + TrajectoryActionBuilder shoot1 = null; + TrajectoryActionBuilder pickup2 = null; + TrajectoryActionBuilder shoot2 = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; + @Override public void runOpMode() throws InterruptedException { @@ -813,15 +822,6 @@ public class Auto_LT_Close extends LinearOpMode { limelightUsed = false; - TrajectoryActionBuilder shoot0 = null; - TrajectoryActionBuilder pickup1 = null; - TrajectoryActionBuilder shoot1 = null; - TrajectoryActionBuilder pickup2 = null; - TrajectoryActionBuilder shoot2 = null; - TrajectoryActionBuilder pickup3 = null; - TrajectoryActionBuilder shoot3 = null; - - robot.light.setPosition(1); while (opModeInInit()) { @@ -992,205 +992,18 @@ public class Auto_LT_Close extends LinearOpMode { robot.transfer.setPower(1); - assert shoot0 != null; - - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - manageFlywheel( - shoot0Vel, - shoot0Hood, - flywheel0Time, - x1, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - Actions.runBlocking( - shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) - ); + startCycle(); if (ballCycles > 0){ - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - intake1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - detectObelisk( - intake1Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos1 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = 22; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shootAllVelocity, - shootAllHood, - shoot1Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot1.build(), - prepareShootAll(colorSenseTime, shoot1Time, motif) - ) - ); - - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) - - ); + cycleStackClose(); } if (ballCycles > 1){ - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - manageShooterAuto( - intake2Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake2Time), - detectObelisk( - intake2Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos2 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot2Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot2.build(), - prepareShootAll(colorSenseTime, shoot2Time, motif) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) - - ); + cycleStackMiddle(); } if (ballCycles > 2){ - Actions.runBlocking( - new ParallelAction( - pickup3.build(), - manageShooterAuto( - intake3Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake3Time), - detectObelisk( - intake3Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos3 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot3.build(), - prepareShootAll(colorSenseTime, shoot3Time, motif) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - finalShootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) - ) - - ); + cycleStackFar(); } while (opModeIsActive()) { @@ -1208,4 +1021,207 @@ public class Auto_LT_Close extends LinearOpMode { } } + + void startCycle() { + assert shoot0 != null; + + Actions.runBlocking( + new ParallelAction( + shoot0.build(), + manageFlywheel( + shoot0Vel, + shoot0Hood, + flywheel0Time, + x1, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + ); + } + + void cycleStackClose(){ + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shootAllVelocity, + shootAllHood, + intake1Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time), + detectObelisk( + intake1Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos1 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = 22; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shootAllVelocity, + shootAllHood, + shoot1Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot1.build(), + prepareShootAll(colorSenseTime, shoot1Time, motif) + ) + ); + + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + } + + void cycleStackMiddle(){ + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + manageShooterAuto( + intake2Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake2Time), + detectObelisk( + intake2Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos2 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot2Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot2.build(), + prepareShootAll(colorSenseTime, shoot2Time, motif) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + } + + void cycleStackFar(){ + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + intake3Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake3Time), + detectObelisk( + intake3Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos3 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(colorSenseTime, shoot3Time, motif) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + finalShootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) + ) + + ); + } } \ No newline at end of file From 9f5dcb4343dc20bb2cfc997562d375f45ca12495 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Sat, 7 Feb 2026 16:06:03 -0600 Subject: [PATCH 05/14] Reduce calls to the spindexer, transfer and color sensors. Add new MeasuringLoopTimes class to measure min, max and avg loop times. --- .../ftc/teamcode/teleop/TeleopV3.java | 24 +++++-- .../teamcode/utils/MeasuringLoopTimes.java | 66 +++++++++++++++++++ .../ftc/teamcode/utils/Servos.java | 34 +++++++++- .../ftc/teamcode/utils/Spindexer.java | 53 +++++++-------- .../ftc/teamcode/utils/Turret.java | 1 + 5 files changed, 142 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java 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..7057e24 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 @@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; @@ -74,6 +75,8 @@ public class TeleopV3 extends LinearOpMode { boolean turretInterpolate = false; private boolean shootAll = false; + public MeasuringLoopTimes measuringLoopTimes; + @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); @@ -94,6 +97,9 @@ public class TeleopV3 extends LinearOpMode { drivetrain = new Drivetrain(robot, drive); + measuringLoopTimes = new MeasuringLoopTimes(); + measuringLoopTimes.init(); + PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); @@ -115,7 +121,7 @@ public class TeleopV3 extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); while (opModeIsActive()) { @@ -135,7 +141,7 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.right_bumper) { shootAll = false; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); } @@ -263,14 +269,14 @@ public class TeleopV3 extends LinearOpMode { spindexer.prepareShootAllContinous(); //TELE.addLine("preparing to shoot"); // } else if (shooterTicker == 2) { -// //robot.transferServo.setPosition(transferServo_in); +// //servo.setTransferPos(transferServo_in); // spindexer.shootAll(); // TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { - robot.transferServo.setPosition(transferServo_in); + servo.setTransferPos(transferServo_in); //TELE.addLine("shoot"); } else { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); @@ -282,7 +288,7 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad1.left_stick_button) { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; @@ -304,6 +310,7 @@ public class TeleopV3 extends LinearOpMode { for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + measuringLoopTimes.loop(); // // TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); @@ -340,6 +347,11 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); TELE.addData("timeSinceStamp", getRuntime() - shootStamp); + TELE.addData("Avg Loop Time", measuringLoopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", measuringLoopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", measuringLoopTimes.getMaxLoopTimeOneMin()); + + TELE.update(); ticker++; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java new file mode 100644 index 0000000..d7d0d1e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.qualcomm.robotcore.util.ElapsedTime; + +public class MeasuringLoopTimes { + private ElapsedTime elapsedtime; + private double minLoopTime = 999999999999.0; + + private double maxLoopTime = 0.0; + private double mainLoopTime = 0.0; + + private double MeasurementStart = 0.0; + private double currentTime = 0.0; + + private double avgLoopTime = 0.0; + private int avgLoopTimeTicker = 0; + private double avgLoopTimeSum = 0; + + private double getTimeSeconds () + { + return (double) System.currentTimeMillis()/1000.0; + } + + public void init() { + elapsedtime = new ElapsedTime(); + elapsedtime.reset(); + + MeasurementStart = getTimeSeconds(); + } + + + public double getAvgLoopTime() { + return avgLoopTime; + } + + public double getMaxLoopTimeOneMin() { + return maxLoopTime; + } + + public double getMinLoopTimeOneMin() { + return minLoopTime; + } + + public void loop() { + currentTime = getTimeSeconds(); + if ((MeasurementStart + 15.0) < currentTime) + { + minLoopTime = 9999999.0; + maxLoopTime = 0.0; + MeasurementStart = currentTime; + + avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker; + avgLoopTimeSum = 0.0; + avgLoopTimeTicker = 0; + } + + mainLoopTime = elapsedtime.milliseconds(); + elapsedtime.reset(); + + avgLoopTimeSum += mainLoopTime; + avgLoopTimeTicker++; + minLoopTime = Math.min(minLoopTime,mainLoopTime); + maxLoopTime = Math.max(maxLoopTime,mainLoopTime); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 324a36f..99944e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -18,6 +18,14 @@ public class Servos { PIDFController spinPID; PIDFController turretPID; + private double prevSpinPos = 0.0; + private boolean firstSpinPos = true; + + private double prevTransferPos = 0.0; + private boolean firstTransferPos = true; + + private double spinPos = 0.0; + public Servos(HardwareMap hardwareMap) { robot = new Robot(hardwareMap); spinPID = new PIDFController(spinP, spinI, spinD, spinF); @@ -32,8 +40,32 @@ public class Servos { return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); } - public double setSpinPos(double pos) { + public double getSpinCmdPos() { + return prevSpinPos; + } + public boolean servoPosEqual(double pos1, double pos2) { + return (Math.abs(pos1 - pos2) < 0.005); + } + + public double setTransferPos(double pos) { + if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) { + robot.transferServo.setPosition(pos); + firstTransferPos = false; + } + + prevTransferPos = pos; + return pos; + } + + public double setSpinPos(double pos) { + if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) { + robot.spin1.setPosition(pos); + robot.spin2.setPosition(1-pos); + firstSpinPos = false; + } + + prevSpinPos = pos; return pos; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index ea5aa6e..d4e0d66 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; @@ -184,11 +186,9 @@ public class Spindexer { if (detectRearColor) { // Detect which color - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; + NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue); // FIXIT - Add filtering to improve accuracy. if (gP >= 0.38) { @@ -205,11 +205,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; + NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); if (gP >= 0.4) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -234,11 +232,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; + NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); if (gP >= 0.42) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -269,8 +265,7 @@ public class Spindexer { } // Has code to unjam spindexer private void moveSpindexerToPos(double pos) { - robot.spin1.setPosition(pos); - robot.spin2.setPosition(1-pos); + servos.setSpinPos(pos); // double currentPos = servos.getSpinPos(); // if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ @@ -281,7 +276,7 @@ public class Spindexer { // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } // } -// prevPos = currentPos; +// prevPos = pos; } public void stopSpindexer() { @@ -325,7 +320,7 @@ public class Spindexer { case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; - moveSpindexerToPos(intakePositions[0]); + servos.setSpinPos(intakePositions[0]); currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; break; case UNKNOWN_MOVE: @@ -336,7 +331,7 @@ public class Spindexer { unknownColorDetect = 0; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; case UNKNOWN_DETECT: @@ -355,7 +350,7 @@ public class Spindexer { } else { // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); } break; case FINDNEXT: @@ -387,7 +382,7 @@ public class Spindexer { //commandedIntakePosition = bestFitMotif(); currentIntakeState = Spindexer.IntakeState.FULL; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); break; case MOVING: @@ -403,7 +398,7 @@ public class Spindexer { //detectBalls(false, false); } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; @@ -416,7 +411,7 @@ public class Spindexer { } // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); break; case SHOOT_ALL_PREP: @@ -425,7 +420,7 @@ public class Spindexer { commandedIntakePosition = 0; if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; @@ -437,7 +432,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -458,7 +453,7 @@ public class Spindexer { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -467,7 +462,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); } break; @@ -492,14 +487,14 @@ public class Spindexer { } // Keep moving the spindexer spindexerOuttakeWiggle *= -1.0; - moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); + servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); break; case SHOOT_PREP_CONTINOUS: if (servos.spinEqual(spinStartPos)){ currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { - moveSpindexerToPos(spinStartPos); + servos.setSpinPos(spinStartPos); } break; @@ -510,11 +505,11 @@ public class Spindexer { if (servos.getSpinPos() > spinEndPos){ currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; + double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - moveSpindexerToPos(spinPos); + servos.setSpinPos(spinPos); } break; 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 9f01c43..61e05b7 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 @@ -11,6 +11,7 @@ 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 com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; From 50060d3812019a705e0e3839ef8aa5a6d5cd51a1 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 7 Feb 2026 17:29:34 -0600 Subject: [PATCH 06/14] @KeshavAnandCode I need your help tomorrow to edit auto actions so they are more sensor based --- .../teamcode/autonomous/Auto_LT_Close.java | 16 ++- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 134 ++++++++++++------ .../ftc/teamcode/constants/Back_Poses.java | 4 +- .../utils/PositionalServoProgrammer.java | 12 +- 4 files changed, 106 insertions(+), 60 deletions(-) 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 43321d4..d98acde 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 @@ -106,10 +106,10 @@ import java.util.Objects; public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.015; + public static double shoot0SpinSpeedIncrease = 0.02; public static double spindexerSpeedIncrease = 0.03; - public static double finalSpindexerSpeedIncrease = 0.025; + public static double finalSpindexerSpeedIncrease = 0.03; // These values are ADDED to turrDefault public static double redObeliskTurrPos1 = 0.12; @@ -348,6 +348,8 @@ public class Auto_LT_Close extends LinearOpMode { TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); + 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(); @@ -622,6 +624,8 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); robot.hood.setPosition(hoodPos); @@ -693,6 +697,8 @@ public class Auto_LT_Close extends LinearOpMode { robot.hood.setPosition(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; @@ -762,6 +768,8 @@ public class Auto_LT_Close extends LinearOpMode { robot.hood.setPosition(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; @@ -992,7 +1000,7 @@ public class Auto_LT_Close extends LinearOpMode { robot.transfer.setPower(1); - startCycle(); + startAuto(); if (ballCycles > 0){ cycleStackClose(); @@ -1022,7 +1030,7 @@ public class Auto_LT_Close extends LinearOpMode { } - void startCycle() { + void startAuto() { assert shoot0 != null; Actions.runBlocking( 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 8e9709a..61dcfb8 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 @@ -49,7 +49,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Far extends LinearOpMode { - public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; + public static double shoot0Vel = 3300, shoot0Hood = 0.48 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; public static double shoot0XTolerance = 1.0; @@ -69,16 +69,18 @@ public class Auto_LT_Far extends LinearOpMode { Turret turret; Targeting targeting; Targeting.Settings targetingSettings; - private double firstSpindexShootPos = autoSpinStartPos; - private boolean shootForward = true; - private double xShoot, yShoot, hShoot; + double firstSpindexShootPos = autoSpinStartPos; + boolean shootForward = true; + 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; + int rearSlotGreen = 0; + int mostGreenSlot = 0; + double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; + double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0; public static double firstShootTime = 0.3; + public static double flywheel0Time = 3.5; + public static double shoot0Time = 2; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -97,12 +99,28 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); - drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); + 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); + TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Hood", robot.hood.getPosition()); TELE.addData("motif", motif_id); @@ -214,6 +232,8 @@ public class Auto_LT_Far extends LinearOpMode { TELE.addData("Hood", robot.hood.getPosition()); TELE.update(); + 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(); @@ -314,6 +334,24 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); + 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 (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { @@ -369,6 +407,9 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; @@ -467,6 +508,8 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; + double voltage = robot.voltage.getVoltage(); + flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); robot.hood.setPosition(hoodPos); @@ -538,6 +581,8 @@ public class Auto_LT_Far extends LinearOpMode { robot.hood.setPosition(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; @@ -607,6 +652,8 @@ public class Auto_LT_Far extends LinearOpMode { robot.hood.setPosition(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; @@ -627,6 +674,9 @@ public class Auto_LT_Far extends LinearOpMode { }; } + // initialize path variables here + TrajectoryActionBuilder leave3Ball = null; + @Override public void runOpMode() throws InterruptedException { @@ -662,8 +712,6 @@ public class Auto_LT_Far extends LinearOpMode { robot.light.setPosition(1); - TrajectoryActionBuilder leave = null; - while (opModeInInit()) { // Recalibration in initialization @@ -673,9 +721,10 @@ public class Auto_LT_Far extends LinearOpMode { gamepad2.rumble(1000); } - robot.hood.setPosition(shoot0Hood); turret.manualSetTurret(turretShootPos); + robot.hood.setPosition(shoot0Hood); + if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; } @@ -706,10 +755,9 @@ public class Auto_LT_Far extends LinearOpMode { turretShootPos = turrDefault + blueTurretShootPos; } - leave = drive.actionBuilder(teleEnd) + leave3Ball = drive.actionBuilder(teleEnd) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); - TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Start Position", teleEnd); @@ -726,35 +774,9 @@ public class Auto_LT_Far extends LinearOpMode { robot.transfer.setPower(1); - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shoot0Vel, - shoot0Hood, - 5, - 0.501, - 0.501, - shoot0XTolerance, - 0.501 - ) + startAuto(); - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - assert leave != null; - Actions.runBlocking(leave.build()); + leave3Ball(); // Actual way to end autonomous in to find final position while (opModeIsActive()) { @@ -772,4 +794,30 @@ public class Auto_LT_Far extends LinearOpMode { } } + + void startAuto(){ + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shoot0Vel, + shoot0Hood, + flywheel0Time, + 0.501, + 0.501, + shoot0XTolerance, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) + ); + } + + void leave3Ball(){ + assert leave3Ball != null; + Actions.runBlocking(leave3Ball.build()); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 7927364..0488a76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class Back_Poses { - public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50; - public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50; + public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50; + public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 18e229a..92e8500 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -35,7 +35,7 @@ public class PositionalServoProgrammer extends LinearOpMode { waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ - if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){ + if (spindexPos != 0.501){ robot.spin1.setPosition(spindexPos); robot.spin2.setPosition(1-spindexPos); } @@ -52,16 +52,6 @@ public class PositionalServoProgrammer extends LinearOpMode { if (light !=0.501){ robot.light.setPosition(light); } - // To check configuration of spindexer: - // Set "mode" to 1 and spindexPow to 0.1 - // If the spindexer is turning clockwise, the servos are reversed. Swap the configuration of the two servos, DO NOT TOUCH THE ACTUAL CODE - // Do the previous test again to confirm - // Set "mode" to 0 but keep spindexPos at 0.501 - // Manually turn the spindexer until "spindexer pos" is set close to 0 - // Check each spindexer voltage: - // If "spindexer voltage 1" is closer to 0 than "spindexer voltage 2," then you are done! - // If "spindexer voltage 2" is closer to 0 than "spindexer voltage 1," swap the two spindexer analog inputs in the configuration, DO NOT TOUCH THE ACTUAL CODE - //TODO: @KeshavAnandCode do the above please TELE.addData("spindexer pos", servo.getSpinPos()); TELE.addData("turret pos", robot.turr1.getPosition()); From ef1c7b0e6be4ed85cbf8bde50f77b807e75c770b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 7 Feb 2026 19:02:41 -0600 Subject: [PATCH 07/14] implemented loop time efficiencies and added turret tracking to shooter test --- .../teamcode/autonomous/Auto_LT_Close.java | 58 +++++++-------- .../autonomous/Auto_LT_Close_12Ball.java | 4 +- .../autonomous/Auto_LT_Close_GateOpen.java | 4 +- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 56 ++++++--------- .../ftc/teamcode/teleop/TeleopV3.java | 25 ++++--- .../ftc/teamcode/tests/LimelightTest.java | 2 +- .../ftc/teamcode/tests/ShooterTest.java | 53 +++++++++++++- .../teamcode/utils/MeasuringLoopTimes.java | 66 +++++++++++++++++ .../ftc/teamcode/utils/Servos.java | 49 +++++++++++-- .../ftc/teamcode/utils/Spindexer.java | 72 ++++++++++--------- .../ftc/teamcode/utils/Turret.java | 14 ++-- 11 files changed, 274 insertions(+), 129 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java 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 d98acde..9f6f396 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 @@ -213,8 +213,7 @@ public class Auto_LT_Close extends LinearOpMode { stamp = System.currentTimeMillis(); } ticker++; - robot.transferServo.setPosition(transferServo_out); - + servos.setTransferPos(transferServo_out); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -246,8 +245,7 @@ public class Auto_LT_Close extends LinearOpMode { spindexerWiggle *= -1.0; - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); spindexer.detectBalls(true, true); @@ -323,8 +321,7 @@ public class Auto_LT_Close extends LinearOpMode { // TELE.update(); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setSpinPos(firstSpindexShootPos); return true; } else { @@ -390,20 +387,18 @@ public class Auto_LT_Close extends LinearOpMode { if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -471,27 +466,24 @@ public class Auto_LT_Close extends LinearOpMode { if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); + double prevSpinPos = servos.getSpinCmdPos(); if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + servos.setSpinPos(prevSpinPos + spindexSpeed); } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); + servos.setSpinPos(prevSpinPos - spindexSpeed); } } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -563,8 +555,7 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; motif = turret.detectObelisk(); - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); + turret.setTurret(turrPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -627,7 +618,7 @@ public class Auto_LT_Close extends LinearOpMode { double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); + servos.setHoodPos(hoodPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -695,7 +686,7 @@ public class Auto_LT_Close extends LinearOpMode { turret.trackGoal(deltaPose); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -766,7 +757,7 @@ public class Auto_LT_Close extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -819,14 +810,13 @@ public class Auto_LT_Close extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(turrDefault); + turret.setTurret(turrDefault); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); limelightUsed = false; @@ -834,8 +824,8 @@ public class Auto_LT_Close extends LinearOpMode { while (opModeInInit()) { - robot.hood.setPosition(shoot0Hood); - turret.manualSetTurret(turrDefault); + servos.setHoodPos(shoot0Hood); + turret.setTurret(turrDefault); if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index ad11f86..b0fd2fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -169,7 +169,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); drive.updatePoseEstimate(); @@ -611,7 +611,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(0.4); + turret.setTurret(0.4); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java index 86cd3b9..bd45591 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java @@ -207,7 +207,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { ticker++; robot.transferServo.setPosition(transferServo_out); - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); drive.updatePoseEstimate(); @@ -752,7 +752,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(0.4); + turret.setTurret(0.4); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); 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 61dcfb8..8f1c859 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 @@ -97,8 +97,7 @@ public class Auto_LT_Far extends LinearOpMode { stamp = System.currentTimeMillis(); } ticker++; - robot.transferServo.setPosition(transferServo_out); - + servos.setTransferPos(transferServo_out); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -130,8 +129,7 @@ public class Auto_LT_Far extends LinearOpMode { spindexerWiggle *= -1.0; - robot.spin1.setPosition(spindexer_intakePos1 + spindexerWiggle); - robot.spin2.setPosition(1 - spindexer_intakePos1 - spindexerWiggle); + servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); spindexer.detectBalls(true, true); @@ -207,8 +205,7 @@ public class Auto_LT_Far extends LinearOpMode { // TELE.update(); robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setSpinPos(firstSpindexShootPos); return true; } else { @@ -274,20 +271,18 @@ public class Auto_LT_Far extends LinearOpMode { if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + double prevSpinPos = servos.getSpinCmdPos(); + servos.setSpinPos(prevSpinPos + spindexSpeed); } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -355,27 +350,24 @@ public class Auto_LT_Far extends LinearOpMode { if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { - robot.transferServo.setPosition(transferServo_in); - robot.spin1.setPosition(firstSpindexShootPos); - robot.spin2.setPosition(1 - firstSpindexShootPos); + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); } else { - robot.transferServo.setPosition(transferServo_in); + servos.setTransferPos(transferServo_in); shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); + double prevSpinPos = servos.getSpinCmdPos(); if (shootForward) { - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); + servos.setSpinPos(prevSpinPos + spindexSpeed); } else { - robot.spin1.setPosition(prevSpinPos - spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos + spindexSpeed); + servos.setSpinPos(prevSpinPos - spindexSpeed); } } return true; } else { - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); spindexer.resetSpindexer(); spindexer.processIntake(); @@ -447,8 +439,7 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; motif = turret.detectObelisk(); - robot.turr1.setPosition(turrPos); - robot.turr2.setPosition(1 - turrPos); + turret.setTurret(turrPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -511,7 +502,7 @@ public class Auto_LT_Far extends LinearOpMode { double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); + servos.setHoodPos(hoodPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; @@ -579,7 +570,7 @@ public class Auto_LT_Far extends LinearOpMode { turret.trackGoal(deltaPose); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -650,7 +641,7 @@ public class Auto_LT_Far extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - robot.hood.setPosition(targetingSettings.hoodAngle); + servos.setHoodPos(targetingSettings.hoodAngle); double voltage = robot.voltage.getVoltage(); flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage); @@ -701,14 +692,13 @@ public class Auto_LT_Far extends LinearOpMode { turret = new Turret(robot, TELE, robot.limelight); - turret.manualSetTurret(turrDefault); + turret.setTurret(turrDefault); drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + servos.setSpinPos(autoSpinStartPos); - robot.transferServo.setPosition(transferServo_out); + servos.setTransferPos(transferServo_out); robot.light.setPosition(1); @@ -721,7 +711,7 @@ public class Auto_LT_Far extends LinearOpMode { gamepad2.rumble(1000); } - turret.manualSetTurret(turretShootPos); + turret.setTurret(turretShootPos); robot.hood.setPosition(shoot0Hood); 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 c09cafd..ab30899 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 @@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; @@ -54,6 +55,7 @@ public class TeleopV3 extends LinearOpMode { Targeting targeting; Targeting.Settings targetingSettings; Drivetrain drivetrain; + MeasuringLoopTimes loopTimes; double autoHoodOffset = 0.0; int shooterTicker = 0; boolean intake = false; @@ -92,6 +94,9 @@ public class TeleopV3 extends LinearOpMode { drivetrain = new Drivetrain(robot, drive); + loopTimes = new MeasuringLoopTimes(); + loopTimes.init(); + PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); @@ -113,7 +118,7 @@ public class TeleopV3 extends LinearOpMode { waitForStart(); if (isStopRequested()) return; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); while (opModeIsActive()) { @@ -133,7 +138,7 @@ public class TeleopV3 extends LinearOpMode { if (gamepad1.right_bumper) { shootAll = false; - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); } @@ -193,9 +198,9 @@ public class TeleopV3 extends LinearOpMode { //HOOD: if (targetingHood) { - robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); + servo.setHoodPos(targetingSettings.hoodAngle + autoHoodOffset); } else { - robot.hood.setPosition(hoodDefaultPos); + servo.setHoodPos(hoodDefaultPos); } if (gamepad2.dpadUpWasPressed()) { @@ -265,10 +270,10 @@ public class TeleopV3 extends LinearOpMode { // spindexer.shootAll(); // TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { - robot.transferServo.setPosition(transferServo_in); + servo.setTransferPos(transferServo_in); //TELE.addLine("shoot"); } else { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); @@ -280,7 +285,7 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad1.left_stick_button) { - robot.transferServo.setPosition(transferServo_out); + servo.setTransferPos(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); @@ -293,6 +298,7 @@ public class TeleopV3 extends LinearOpMode { for (LynxModule hub : allHubs) { hub.clearBulkCache(); } + loopTimes.loop(); // // TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); // TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); @@ -328,7 +334,10 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); - TELE.addData("Voltage", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub) + TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub) + TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); + TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); + TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 6cbbbc4..3880604 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -60,7 +60,7 @@ public class LimelightTest extends LinearOpMode { if (turretMode){ if (turretPos != 0.501){ - turret.manualSetTurret(turretPos); + turret.setTurret(turretPos); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 63e85bc..dcb2d73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -1,23 +1,30 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; 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.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; +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 @TeleOp @@ -37,10 +44,12 @@ public class ShooterTest extends LinearOpMode { public static boolean shoot = false; public static boolean intake = false; + public static boolean turretTrack = true; Robot robot; Flywheel flywheel; Servos servo; - + MecanumDrive drive; + Turret turret; double shootStamp = 0.0; boolean shootAll = false; @@ -50,6 +59,7 @@ public class ShooterTest extends LinearOpMode { public double hoodAdjust = 0.0; public static double hoodAdjustFactor = 1.0; private int shooterTicker = 0; + public static double spinSpeed = 0.02; Spindexer spindexer ; @Override @@ -61,17 +71,51 @@ public class ShooterTest extends LinearOpMode { flywheel = new Flywheel(hardwareMap); spindexer = new Spindexer(hardwareMap); servo = new Servos(hardwareMap); + drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); + turret = new Turret(robot, TELE, robot.limelight); + Turret.limelightUsed = true; waitForStart(); + robot.limelight.start(); + if (isStopRequested()) return; while (opModeIsActive()) { + if (redAlliance){ + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + + //TURRET TRACKING + drive.updatePoseEstimate(); + + 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); + + if (turretTrack){ + turret.trackGoal(deltaPose); + } else if (turretPos != 0.501){ + turret.setTurret(turretPos); + } + double voltage = robot.voltage.getVoltage(); if (mode == 0) { @@ -120,8 +164,11 @@ public class ShooterTest extends LinearOpMode { robot.transferServo.setPosition(transferServo_in); shooterTicker++; double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); - robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); + + if (prevSpinPos < 0.9){ + robot.spin1.setPosition(prevSpinPos + spinSpeed); + robot.spin2.setPosition(1 - prevSpinPos - spinSpeed); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java new file mode 100644 index 0000000..d7d0d1e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MeasuringLoopTimes.java @@ -0,0 +1,66 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.qualcomm.robotcore.util.ElapsedTime; + +public class MeasuringLoopTimes { + private ElapsedTime elapsedtime; + private double minLoopTime = 999999999999.0; + + private double maxLoopTime = 0.0; + private double mainLoopTime = 0.0; + + private double MeasurementStart = 0.0; + private double currentTime = 0.0; + + private double avgLoopTime = 0.0; + private int avgLoopTimeTicker = 0; + private double avgLoopTimeSum = 0; + + private double getTimeSeconds () + { + return (double) System.currentTimeMillis()/1000.0; + } + + public void init() { + elapsedtime = new ElapsedTime(); + elapsedtime.reset(); + + MeasurementStart = getTimeSeconds(); + } + + + public double getAvgLoopTime() { + return avgLoopTime; + } + + public double getMaxLoopTimeOneMin() { + return maxLoopTime; + } + + public double getMinLoopTimeOneMin() { + return minLoopTime; + } + + public void loop() { + currentTime = getTimeSeconds(); + if ((MeasurementStart + 15.0) < currentTime) + { + minLoopTime = 9999999.0; + maxLoopTime = 0.0; + MeasurementStart = currentTime; + + avgLoopTime = avgLoopTimeSum / (double) avgLoopTimeTicker; + avgLoopTimeSum = 0.0; + avgLoopTimeTicker = 0; + } + + mainLoopTime = elapsedtime.milliseconds(); + elapsedtime.reset(); + + avgLoopTimeSum += mainLoopTime; + avgLoopTimeTicker++; + minLoopTime = Math.min(minLoopTime,mainLoopTime); + maxLoopTime = Math.max(maxLoopTime,mainLoopTime); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 324a36f..a8a2a00 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -18,6 +18,15 @@ public class Servos { PIDFController spinPID; PIDFController turretPID; + private double prevSpinPos = 0.0; + private boolean firstSpinPos = true; + + private double prevTransferPos = 0.0; + private boolean firstTransferPos = true; + + private double prevHoodPos = 0.0; + private boolean firstHoodPos = true; + public Servos(HardwareMap hardwareMap) { robot = new Robot(hardwareMap); spinPID = new PIDFController(spinP, spinI, spinD, spinF); @@ -32,8 +41,42 @@ public class Servos { return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3); } - public double setSpinPos(double pos) { + public double getSpinCmdPos() { + return prevSpinPos; + } + public boolean servoPosEqual(double pos1, double pos2) { + return (Math.abs(pos1 - pos2) < 0.005); + } + + public double setTransferPos(double pos) { + if (firstTransferPos || !servoPosEqual(pos, prevTransferPos)) { + robot.transferServo.setPosition(pos); + firstTransferPos = false; + } + + prevTransferPos = pos; + return pos; + } + + public double setSpinPos(double pos) { + if (firstSpinPos || !servoPosEqual(pos, prevSpinPos)) { + robot.spin1.setPosition(pos); + robot.spin2.setPosition(1-pos); + firstSpinPos = false; + } + + prevSpinPos = pos; + return pos; + } + + public double setHoodPos(double pos){ + if (firstHoodPos || !servoPosEqual(pos, prevHoodPos)) { + robot.hood.setPosition(pos); + firstHoodPos = false; + } + + prevHoodPos = pos; return pos; } @@ -48,8 +91,4 @@ public class Servos { public double setTurrPos(double pos) { return 1.0; } - - public boolean turretEqual(double pos) { - return true; - } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index ea5aa6e..af42039 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; @@ -184,11 +186,9 @@ public class Spindexer { if (detectRearColor) { // Detect which color - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; + NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue); // FIXIT - Add filtering to improve accuracy. if (gP >= 0.38) { @@ -196,7 +196,7 @@ public class Spindexer { } else { ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple } - } + } } // Position 2 // Find which ball position this is in the spindexer @@ -205,11 +205,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; + NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); if (gP >= 0.4) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -234,11 +232,9 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; if (detectFrontColor) { - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; + NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); - double gP = green / (green + red + blue); + double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); if (gP >= 0.42) { ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green @@ -269,8 +265,7 @@ public class Spindexer { } // Has code to unjam spindexer private void moveSpindexerToPos(double pos) { - robot.spin1.setPosition(pos); - robot.spin2.setPosition(1-pos); + servos.setSpinPos(pos); // double currentPos = servos.getSpinPos(); // if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ @@ -281,13 +276,14 @@ public class Spindexer { // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } // } -// prevPos = currentPos; +// prevPos = pos; } public void stopSpindexer() { } + private double prevLight = 0.0; public void ballCounterLight(){ int counter = 0; if (!ballPositions[0].isEmpty){ @@ -299,15 +295,21 @@ public class Spindexer { if (!ballPositions[2].isEmpty){ counter++; } + + double light; if (counter == 3){ - robot.light.setPosition(Light3); + light = Light3; } else if (counter == 2){ - robot.light.setPosition(Light2); + light = Light2; } else if (counter == 1){ - robot.light.setPosition(Light1); + light = Light1; } else { - robot.light.setPosition(Light0); + light = Light0; } + if (light != prevLight){ + robot.light.setPosition(light); + } + prevLight = light; } public boolean slotIsEmpty(int slot){ @@ -325,7 +327,7 @@ public class Spindexer { case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; - moveSpindexerToPos(intakePositions[0]); + servos.setSpinPos(intakePositions[0]); currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; break; case UNKNOWN_MOVE: @@ -336,7 +338,7 @@ public class Spindexer { unknownColorDetect = 0; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; case UNKNOWN_DETECT: @@ -355,7 +357,7 @@ public class Spindexer { } else { // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); } break; case FINDNEXT: @@ -387,7 +389,7 @@ public class Spindexer { //commandedIntakePosition = bestFitMotif(); currentIntakeState = Spindexer.IntakeState.FULL; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); break; case MOVING: @@ -403,7 +405,7 @@ public class Spindexer { //detectBalls(false, false); } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + servos.setSpinPos(intakePositions[commandedIntakePosition]); } break; @@ -416,7 +418,7 @@ public class Spindexer { } // Maintain Position spindexerWiggle *= -1.0; - moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); + servos.setSpinPos(intakePositions[commandedIntakePosition]+spindexerWiggle); break; case SHOOT_ALL_PREP: @@ -425,7 +427,7 @@ public class Spindexer { commandedIntakePosition = 0; if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + servos.setSpinPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; @@ -437,7 +439,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: @@ -458,7 +460,7 @@ public class Spindexer { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; } - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -467,7 +469,7 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); + servos.setSpinPos(outakePositions[commandedIntakePosition]); } break; @@ -492,14 +494,14 @@ public class Spindexer { } // Keep moving the spindexer spindexerOuttakeWiggle *= -1.0; - moveSpindexerToPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); + servos.setSpinPos(outakePositions[commandedIntakePosition]+spindexerOuttakeWiggle); break; case SHOOT_PREP_CONTINOUS: if (servos.spinEqual(spinStartPos)){ currentIntakeState = Spindexer.IntakeState.SHOOT_CONTINOUS; } else { - moveSpindexerToPos(spinStartPos); + servos.setSpinPos(spinStartPos); } break; @@ -510,11 +512,11 @@ public class Spindexer { if (servos.getSpinPos() > spinEndPos){ currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = robot.spin1.getPosition() + shootAllSpindexerSpeedIncrease; + double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; if (spinPos > spinEndPos + 0.03){ spinPos = spinEndPos + 0.03; } - moveSpindexerToPos(spinPos); + servos.setSpinPos(spinPos); } break; @@ -565,7 +567,7 @@ public class Spindexer { //break; case NONE: return 0; - //break; + //break; } return 0; } 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 9f01c43..8ded7eb 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 @@ -71,10 +71,13 @@ public class Turret { return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault; } - - public void manualSetTurret(double pos) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); + private double prevTurrPos = 0.501; + public void setTurret(double pos) { + if (prevTurrPos != 0.501 && prevTurrPos != pos){ + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1-pos); + } + prevTurrPos = pos; } public boolean turretEqual(double pos) { @@ -272,8 +275,7 @@ public class Turret { } // Set servo positions - robot.turr1.setPosition(turretPos + manualOffset); - robot.turr2.setPosition(1.0 - turretPos - manualOffset); + setTurret(turretPos + manualOffset); /* ---------------- TELEMETRY ---------------- */ From 4488fabecf7f6b1b69711ac20b81ed918956dabc Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 8 Feb 2026 23:02:42 -0600 Subject: [PATCH 08/14] auton progress with errors --- .../teamcode/autonomous/Auto_LT_Close.java | 75 +-------- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 155 ++++++++++++++++-- .../{ => disabled}/Auto_LT_Close_12Ball.java | 2 +- .../Auto_LT_Close_GateOpen.java | 2 +- .../ftc/teamcode/constants/Back_Poses.java | 15 +- .../ftc/teamcode/constants/Front_Poses.java | 1 - 6 files changed, 164 insertions(+), 86 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{ => disabled}/Auto_LT_Close_12Ball.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{ => disabled}/Auto_LT_Close_GateOpen.java (99%) 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 9f6f396..7e13cf4 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 @@ -1,67 +1,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; @@ -849,6 +789,7 @@ public class Auto_LT_Close extends LinearOpMode { if (gamepad2.squareWasPressed()){ robot.limelight.start(); robot.limelight.pipelineSwitch(1); + gamepad2.rumble(500); } if (redAlliance) { @@ -1052,8 +993,8 @@ public class Auto_LT_Close extends LinearOpMode { shootAllVelocity, shootAllHood, intake1Time, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), @@ -1113,8 +1054,8 @@ public class Auto_LT_Close extends LinearOpMode { pickup2.build(), manageShooterAuto( intake2Time, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), @@ -1171,8 +1112,8 @@ public class Auto_LT_Close extends LinearOpMode { pickup3.build(), manageShooterAuto( intake3Time, - x2b, - y2b, + 0.501, + 0.501, pickup1XTolerance, pickup1YTolerance ), 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 8f1c859..1b34049 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 @@ -1,13 +1,7 @@ package org.firstinspires.ftc.teamcode.autonomous; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX; -import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.*; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; @@ -31,6 +25,7 @@ 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; @@ -81,6 +76,12 @@ public class Auto_LT_Far extends LinearOpMode { public static double firstShootTime = 0.3; public static double flywheel0Time = 3.5; public static double shoot0Time = 2; + boolean gatePickup = false; + boolean stack3 = true; + double xStackPickupA, yStackPickupA, hStackPickupA; + double xStackPickupB, yStackPickupB, hStackPickupB; + public static int pickupStackSpeed = 15; + int prevMotif = 0; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -667,6 +668,9 @@ public class Auto_LT_Far extends LinearOpMode { // initialize path variables here TrajectoryActionBuilder leave3Ball = null; + TrajectoryActionBuilder leaveFromShoot = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; @Override public void runOpMode() throws InterruptedException { @@ -694,23 +698,34 @@ public class Auto_LT_Far extends LinearOpMode { turret.setTurret(turrDefault); - drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); + drive = new MecanumDrive(hardwareMap, autoStart); servos.setSpinPos(autoSpinStartPos); servos.setTransferPos(transferServo_out); - robot.light.setPosition(1); - while (opModeInInit()) { // Recalibration in initialization drive.updatePoseEstimate(); - if (gamepad2.square) { - teleEnd = drive.localizer.getPose(); // use this position as starting position + if (gamepad2.triangle) { + autoStart = drive.localizer.getPose(); // use this position as starting position gamepad2.rumble(1000); } + if (gamepad2.squareWasPressed()){ + robot.limelight.start(); + robot.limelight.pipelineSwitch(1); + gamepad2.rumble(500); + } + + if (gamepad2.leftBumperWasPressed()){ + gatePickup = !gatePickup; + } + if (gamepad2.rightBumperWasPressed()){ + stack3 = !stack3; + } + turret.setTurret(turretShootPos); robot.hood.setPosition(shoot0Hood); @@ -734,6 +749,18 @@ public class Auto_LT_Far extends LinearOpMode { yLeave = rLeaveY; hLeave = rLeaveH; + xShoot = rShootX; + yShoot = rShootY; + hShoot = rShootH; + + xStackPickupA = rStackPickupAX; + yStackPickupA = rStackPickupAY; + hStackPickupA = rStackPickupAH; + + xStackPickupB = rStackPickupBX; + yStackPickupB = rStackPickupBY; + hStackPickupB = rStackPickupBH; + turretShootPos = turrDefault + redTurretShootPos; } else { robot.light.setPosition(0.6); @@ -742,15 +769,40 @@ public class Auto_LT_Far extends LinearOpMode { yLeave = bLeaveY; hLeave = bLeaveH; + xShoot = bShootX; + yShoot = bShootY; + hShoot = bShootH; + + xStackPickupA = bStackPickupAX; + yStackPickupA = bStackPickupAY; + hStackPickupA = bStackPickupAH; + + xStackPickupB = bStackPickupBX; + yStackPickupB = bStackPickupBY; + hStackPickupB = bStackPickupBH; + turretShootPos = turrDefault + blueTurretShootPos; } - leave3Ball = drive.actionBuilder(teleEnd) + leave3Ball = drive.actionBuilder(autoStart) .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + leaveFromShoot = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) + .strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), + new TranslationalVelConstraint(pickupStackSpeed)); + + shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); - TELE.addData("Start Position", teleEnd); + TELE.addData("Gate Cycle?", gatePickup); + TELE.addData("Pickup Stack?", stack3); + TELE.addData("Start Position", autoStart); TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.update(); } @@ -762,11 +814,21 @@ public class Auto_LT_Far extends LinearOpMode { // Currently only shoots; keep this start and modify times and then add extra paths if (opModeIsActive()) { + double stamp = getRuntime(); + robot.transfer.setPower(1); startAuto(); - leave3Ball(); + if (stack3){ + cycleStackFar(); + } + + if (gatePickup || stack3){ + leave(); + } else { + leave3Ball(); + } // Actual way to end autonomous in to find final position while (opModeIsActive()) { @@ -810,4 +872,67 @@ public class Auto_LT_Far extends LinearOpMode { assert leave3Ball != null; Actions.runBlocking(leave3Ball.build()); } + + void leave(){ + assert leaveFromShoot != null; + Actions.runBlocking(leaveFromShoot.build()); + } + + void cycleStackFar(){ + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + intake3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + intake(intake3Time), + detectObelisk( + intake3Time, + 0.501, + 0.501, + 0.501, + 0.501, + obeliskTurrPos3 + ) + + ) + ); + + motif = turret.getObeliskID(); + + if (motif == 0) motif = prevMotif; + prevMotif = motif; + + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(colorSenseTime, shoot3Time, motif) + ) + ); + + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + finalShootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) + ) + + ); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java index b0fd2fa..b4ca67a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java index bd45591..c32dc38 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java index 0488a76..c091ccd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -1,10 +1,23 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; @Config public class Back_Poses { - public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50; + public static double rLeaveX = 90, rLeaveY = 80, rLeaveH = 50.1; public static double bLeaveX = 90, bLeaveY = -80, bLeaveH = -50; + public static double rShootX = 95, rShootY = 85, rShootH = 90; + public static double bShootX = 95, bShootY = -85, bShootH = -90; + + public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140; + public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; + + public static double rStackPickupBX = 50, rStackPickupBY = 78, rStackPickupBH = 140.1; + public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; + + public static Pose2d autoStart = new Pose2d(0, 0, 0); // TODO: find this position + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index bb7d29b..63f7f6e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -43,5 +43,4 @@ public class Front_Poses { public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); - public static Pose2d teleEnd = new Pose2d(0, 0, 0); } From 830c2b1481892f578ddaf50a8fafdd0c02859d94 Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Tue, 10 Feb 2026 00:17:26 -0600 Subject: [PATCH 09/14] Further speed up loop times. We are now running 50% faster but need to retune the turret PID and update the shoot all speed everywhere. --- .../ftc/teamcode/utils/Flywheel.java | 24 ++++++++++--------- .../ftc/teamcode/utils/Servos.java | 2 +- .../ftc/teamcode/utils/Spindexer.java | 6 ++--- .../ftc/teamcode/utils/Turret.java | 23 ++++++++++++++---- 4 files changed, 35 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index b03db69..7cbd38a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -12,6 +12,7 @@ public class Flywheel { public double velo1 = 0.0; public double velo2 = 0.0; double targetVelocity = 0.0; + double previousTargetVelocity = 0.0; double powPID = 0.0; boolean steady = false; public Flywheel (HardwareMap hardwareMap) { @@ -55,20 +56,21 @@ public class Flywheel { private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;} public double manageFlywheel(double commandedVelocity) { - targetVelocity = commandedVelocity; - // Add code here to set PIDF based on desired RPM + if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { + targetVelocity = commandedVelocity; + // Add code here to set PIDF based on desired RPM + //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); - robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); - robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); - - // Record Current Velocity - velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); - velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); - velo = Math.max(velo1,velo2); + // Record Current Velocity + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + velo = Math.max(velo1, velo2); + } // really should be a running average of the last 5 steady = (Math.abs(targetVelocity - velo) < 200.0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 99944e3..e2f611d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -44,7 +44,7 @@ public class Servos { return prevSpinPos; } - public boolean servoPosEqual(double pos1, double pos2) { + public static boolean servoPosEqual(double pos1, double pos2) { return (Math.abs(pos1 - pos2) < 0.005); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index d4e0d66..5b14dcf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -49,6 +49,7 @@ public class Spindexer { public double spindexerOuttakeWiggle = 0.01; private double prevPos = 0.0; public double spindexerPosOffset = 0.00; + double shootWaitMax = 4; public Types.Motif desiredMotif = Types.Motif.NONE; // For Use enum RotatedBallPositionNames { @@ -467,7 +468,6 @@ public class Spindexer { break; case SHOOTWAIT: - double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition==2) { @@ -505,8 +505,8 @@ public class Spindexer { if (servos.getSpinPos() > spinEndPos){ currentIntakeState = IntakeState.FINDNEXT; } else { - double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease; - if (spinPos > spinEndPos + 0.03){ + double spinPos = servos.getSpinCmdPos() + 0.005; //shootAllSpindexerSpeedIncrease; + if (spinPos > spinEndPos + 0.03) { // 0.03 @ 48ms loops times spinPos = spinEndPos + 0.03; } servos.setSpinPos(spinPos); 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 61e05b7..b8b5416 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 @@ -38,7 +38,8 @@ public class Turret { // TODO: tune these values for limelight public static double clampTolerance = 0.03; - public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; + //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; + public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; @@ -56,6 +57,9 @@ public class Turret { private double permanentOffset = 0.0; private PIDController bearingPID; + private double prevTurretPos = 0.0; + private boolean firstTurretPos = true; + public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; this.robot = rob; @@ -74,8 +78,18 @@ public class Turret { } public void manualSetTurret(double pos) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); + setTurretPos(pos); + } + + public double setTurretPos(double pos) { + if (firstTurretPos || !Servos.servoPosEqual(pos, prevTurretPos)) { + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1 - pos); + firstTurretPos = false; + } + + prevTurretPos = pos; + return pos; } public boolean turretEqual(double pos) { @@ -273,8 +287,7 @@ public class Turret { } // Set servo positions - robot.turr1.setPosition(turretPos + manualOffset); - robot.turr2.setPosition(1.0 - turretPos - manualOffset); + setTurretPos(turretPos + manualOffset); /* ---------------- TELEMETRY ---------------- */ From 48b5925b15d892c85e585ffb140304215f7cf965 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 10 Feb 2026 18:37:06 -0600 Subject: [PATCH 10/14] added loop time fixes --- .../firstinspires/ftc/teamcode/utils/Flywheel.java | 12 ++++++------ .../org/firstinspires/ftc/teamcode/utils/Servos.java | 2 +- .../firstinspires/ftc/teamcode/utils/Spindexer.java | 2 +- .../org/firstinspires/ftc/teamcode/utils/Turret.java | 3 ++- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index b03db69..9a07420 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -55,14 +55,14 @@ public class Flywheel { private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;} public double manageFlywheel(double commandedVelocity) { - targetVelocity = commandedVelocity; - // Add code here to set PIDF based on desired RPM - robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); - robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); - robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); - robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + if (targetVelocity != commandedVelocity){ + robot.shooter1.setVelocity(RPM_to_TPS(commandedVelocity)); + robot.shooter2.setVelocity(RPM_to_TPS(commandedVelocity)); + } + + targetVelocity = commandedVelocity; // Record Current Velocity velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index a8a2a00..e150405 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -45,7 +45,7 @@ public class Servos { return prevSpinPos; } - public boolean servoPosEqual(double pos1, double pos2) { + public static boolean servoPosEqual(double pos1, double pos2) { return (Math.abs(pos1 - pos2) < 0.005); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index af42039..08a52e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -49,6 +49,7 @@ public class Spindexer { public double spindexerOuttakeWiggle = 0.01; private double prevPos = 0.0; public double spindexerPosOffset = 0.00; + public static int shootWaitMax = 4; public Types.Motif desiredMotif = Types.Motif.NONE; // For Use enum RotatedBallPositionNames { @@ -474,7 +475,6 @@ public class Spindexer { break; case SHOOTWAIT: - double shootWaitMax = 4; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { if (commandedIntakePosition==2) { 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 8ded7eb..a00b76b 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 @@ -37,7 +37,8 @@ public class Turret { // TODO: tune these values for limelight public static double clampTolerance = 0.03; - public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; + //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; + public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; From f5b8b2fd8999672e457c7dbc880471d3bc2c0e8a Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Tue, 10 Feb 2026 20:29:33 -0600 Subject: [PATCH 11/14] Tweak PID for Limelight to improve turret Aiming. --- .../main/java/org/firstinspires/ftc/teamcode/utils/Turret.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b8b5416..cab0f48 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 @@ -39,7 +39,7 @@ public class Turret { public static double clampTolerance = 0.03; //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125; - public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090; + public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; From cc83872a9540aabc7b12cae66b28ffccfa2962d3 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 10 Feb 2026 20:31:40 -0600 Subject: [PATCH 12/14] stash before merge --- .../ftc/teamcode/autonomous/Auto_LT_Close.java | 14 +++++++------- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 14 +++++++------- .../ftc/teamcode/constants/Poses_V2.java | 2 ++ .../ftc/teamcode/constants/ShooterVars.java | 2 ++ .../ftc/teamcode/teleop/TeleopV3.java | 7 +++---- .../ftc/teamcode/utils/Spindexer.java | 7 +++++++ 6 files changed, 28 insertions(+), 18 deletions(-) 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 7e13cf4..3e8e9c4 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 @@ -201,7 +201,7 @@ public class Auto_LT_Close extends LinearOpMode { rearSlotGreen++; } - robot.intake.setPower(1); + spindexer.setIntakePower(1); decideGreenSlot = true; @@ -259,7 +259,7 @@ public class Auto_LT_Close extends LinearOpMode { } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { // TELE.addData("MostGreenSlot", mostGreenSlot); // TELE.update(); - robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); + spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); servos.setSpinPos(firstSpindexShootPos); @@ -294,14 +294,14 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); - robot.intake.setPower(-0.3); + spindexer.setIntakePower(-0.3); if (ticker == 1) { stamp = getRuntime(); } ticker++; - robot.intake.setPower(0); + spindexer.setIntakePower(0); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -373,14 +373,14 @@ public class Auto_LT_Close extends LinearOpMode { teleStart = drive.localizer.getPose(); - robot.intake.setPower(-0.3); + spindexer.setIntakePower(-0.3); if (ticker == 1) { stamp = getRuntime(); } ticker++; - robot.intake.setPower(0); + spindexer.setIntakePower(0); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -448,7 +448,7 @@ public class Auto_LT_Close extends LinearOpMode { ticker++; spindexer.processIntake(); - robot.intake.setPower(1); + spindexer.setIntakePower(1); spindexer.ballCounterLight(); drive.updatePoseEstimate(); 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 1b34049..826a7dc 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 @@ -146,7 +146,7 @@ public class Auto_LT_Far extends LinearOpMode { rearSlotGreen++; } - robot.intake.setPower(1); + spindexer.setIntakePower(1); decideGreenSlot = true; @@ -204,7 +204,7 @@ public class Auto_LT_Far extends LinearOpMode { } else if ((System.currentTimeMillis() - stamp) < (time * 1000)) { // TELE.addData("MostGreenSlot", mostGreenSlot); // TELE.update(); - robot.intake.setPower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); + spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); servos.setSpinPos(firstSpindexShootPos); @@ -239,14 +239,14 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); - robot.intake.setPower(-0.3); + spindexer.setIntakePower(-0.3); if (ticker == 1) { stamp = getRuntime(); } ticker++; - robot.intake.setPower(0); + spindexer.setIntakePower(0); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -318,14 +318,14 @@ public class Auto_LT_Far extends LinearOpMode { teleStart = drive.localizer.getPose(); - robot.intake.setPower(-0.3); + spindexer.setIntakePower(-0.3); if (ticker == 1) { stamp = getRuntime(); } ticker++; - robot.intake.setPower(0); + spindexer.setIntakePower(0); drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); @@ -393,7 +393,7 @@ public class Auto_LT_Far extends LinearOpMode { ticker++; spindexer.processIntake(); - robot.intake.setPower(1); + spindexer.setIntakePower(1); spindexer.ballCounterLight(); drive.updatePoseEstimate(); 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 53fabef..cd02540 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,7 +1,9 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +@Disabled @Config public class Poses_V2 { 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 bd096b9..a3f9347 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 @@ -1,7 +1,9 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +@Disabled @Config public class ShooterVars { 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 ab30899..8a2ddf5 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 @@ -239,13 +239,12 @@ public class TeleopV3 extends LinearOpMode { // RIGHT_BUMPER if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { - robot.intake.setPower(1); + spindexer.setIntakePower(1); } else if (gamepad1.cross) { - robot.intake.setPower(-1); + spindexer.setIntakePower(-1); } else { - robot.intake.setPower(0); - + spindexer.setIntakePower(0); } // LEFT_BUMPER diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 08a52e8..1ffbc93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -625,4 +625,11 @@ public class Spindexer { public BallColor GetRearCenterColor () { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor; } + private double prevPow = 0.501; + public void setIntakePower(double pow){ + if (prevPow != 0.501 && prevPow != pow){ + robot.intake.setPower(pow); + } + prevPow = pow; + } } From a0e98c9f694798c481a37abef271f908eed69b2e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 10 Feb 2026 22:16:38 -0600 Subject: [PATCH 13/14] loop times at 30ms --- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 116 +++++++++--------- .../ftc/teamcode/teleop/TeleopV3.java | 3 +- .../ftc/teamcode/utils/Flywheel.java | 6 + 3 files changed, 65 insertions(+), 60 deletions(-) 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 826a7dc..c6eebf2 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 @@ -821,7 +821,7 @@ public class Auto_LT_Far extends LinearOpMode { startAuto(); if (stack3){ - cycleStackFar(); + //cycleStackFar(); } if (gatePickup || stack3){ @@ -878,61 +878,61 @@ public class Auto_LT_Far extends LinearOpMode { Actions.runBlocking(leaveFromShoot.build()); } - void cycleStackFar(){ - Actions.runBlocking( - new ParallelAction( - pickup3.build(), - manageShooterAuto( - intake3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - intake(intake3Time), - detectObelisk( - intake3Time, - 0.501, - 0.501, - 0.501, - 0.501, - obeliskTurrPos3 - ) - - ) - ); - - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot3.build(), - prepareShootAll(colorSenseTime, shoot3Time, motif) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - finalShootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) - ) - - ); - } +// void cycleStackFar(){ +// Actions.runBlocking( +// new ParallelAction( +// pickup3.build(), +// manageShooterAuto( +// intake3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// intake(intake3Time), +// detectObelisk( +// intake3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501, +// obeliskTurrPos3 +// ) +// +// ) +// ); +// +// motif = turret.getObeliskID(); +// +// if (motif == 0) motif = prevMotif; +// prevMotif = motif; +// +// Actions.runBlocking( +// new ParallelAction( +// manageFlywheelAuto( +// shoot3Time, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shoot3.build(), +// prepareShootAll(colorSenseTime, shoot3Time, motif) +// ) +// ); +// +// Actions.runBlocking( +// new ParallelAction( +// manageShooterAuto( +// finalShootAllTime, +// 0.501, +// 0.501, +// 0.501, +// 0.501 +// ), +// shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) +// ) +// +// ); +// } } \ No newline at end of file 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 798cf86..15770f6 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 @@ -118,6 +118,7 @@ public class TeleopV3 extends LinearOpMode { if (isStopRequested()) return; servo.setTransferPos(transferServo_out); + robot.transfer.setPower(1); while (opModeIsActive()) { @@ -141,8 +142,6 @@ public class TeleopV3 extends LinearOpMode { } - robot.transfer.setPower(1); - //TURRET TRACKING double robX = drive.localizer.getPose().position.x; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index 7cbd38a..b5e017f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -38,6 +38,8 @@ public class Flywheel { } // Set the robot PIDF for the next cycle. + private double prevF = 0.501; + public static int voltagePIDFDifference = 5; public void setPIDF(double p, double i, double d, double f) { shooterPIDF1.p = p; shooterPIDF1.i = i; @@ -47,6 +49,10 @@ public class Flywheel { shooterPIDF2.i = i; shooterPIDF2.d = d; shooterPIDF2.f = f; + if (Math.abs(prevF - f) > voltagePIDFDifference){ + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + } } // Convert from RPM to Ticks per Second From 6edc2075c07d293bb72de3554cf1ba6456375a08 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 12 Feb 2026 19:51:33 -0600 Subject: [PATCH 14/14] stash --- .../teamcode/autonomous/actions/Actions.java | 648 ++++++++++++++++++ .../teamcode/constants/ServoPositions.java | 10 + 2 files changed, 658 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java new file mode 100644 index 0000000..b337c56 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/Actions.java @@ -0,0 +1,648 @@ +package org.firstinspires.ftc.teamcode.autonomous.actions; + +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +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.Targeting.turretInterpolate; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Pose2d; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.Drivetrain; +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; + +public class Actions{ + Robot robot; + MultipleTelemetry TELE; + Servos servos; + Flywheel flywheel; + MecanumDrive drive; + Spindexer spindexer; + Targeting targeting; + Targeting.Settings targetingSettings; + Turret turret; + private int driverSlotGreen = 0; + private int passengerSlotGreen = 0; + private int rearSlotGreen = 0; + private int mostGreenSlot = 0; + private double firstSpindexShootPos = spinStartPos; + private boolean shootForward = true; + public static double firstShootTime = 0.3; + public int motif = 0; + + public Actions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){ + this.robot = rob; + this.drive = dri; + this.TELE = tel; + this.servos = ser; + this.flywheel = fly; + this.spindexer = spi; + this.targeting = tar; + this.targetingSettings = tS; + this.turret = tur; + } + 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++; + servos.setTransferPos(transferServo_out); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + 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); + + 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; + + servos.setSpinPos(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++; + } + + spindexer.setIntakePower(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(); + spindexer.setIntakePower(-((System.currentTimeMillis() - stamp - colorSenseTime)) / 1000); + + servos.setSpinPos(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 + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + 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.3); + + if (ticker == 1) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.setIntakePower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + 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 && servos.getSpinPos() < spinEndPos) || 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; + + } + + } + }; + } + + 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(); + + spindexer.setIntakePower(-0.3); + + if (ticker == 1) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.setIntakePower(0); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + 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) { + + if (System.currentTimeMillis() - stamp < firstShootTime) { + servos.setTransferPos(transferServo_out); + servos.setSpinPos(firstSpindexShootPos); + } else { + servos.setTransferPos(transferServo_in); + shooterTicker++; + double prevSpinPos = servos.getSpinCmdPos(); + + if (shootForward) { + servos.setSpinPos(prevSpinPos + spindexSpeed); + } else { + servos.setSpinPos(prevSpinPos - spindexSpeed); + } + } + + return true; + + } else { + servos.setTransferPos(transferServo_out); + + 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(); + spindexer.setIntakePower(1); + + spindexer.ballCounterLight(); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Full?", spindexer.isFull()); + TELE.update(); + + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); + } + }; + } + + 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(); + + turret.setTurret(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++; + + 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; + 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); + + 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; + 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); + + 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; + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Hood", robot.hood.getPosition()); + TELE.update(); + + return !shouldFinish; + + } + }; + } +} + + 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 0acbe4e..225a0fb 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 @@ -32,4 +32,14 @@ public class ServoPositions { public static double turret_redClose = 0.42; public static double turret_blueClose = 0.38; + // 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.1; + public static double blueTurretShootPos = -0.14; + }