From 28816a6e3478dae0bf40dd3ff15e88010e1b8d0f Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 14 Feb 2026 17:16:25 -0600 Subject: [PATCH] close auto is pretty close to good --- .../teamcode/autonomous/Auto_LT_Close.java | 26 +++-- .../ftc/teamcode/autonomous/Auto_LT_Far.java | 41 +++---- .../autonomous/actions/AutoActions.java | 102 +++++++++++++----- 3 files changed, 117 insertions(+), 52 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 0130863..67cb2fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -32,7 +32,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double spindexerSpeedIncrease = 0.012; + public static double spindexerSpeedIncrease = 0.005; // These values are ADDED to turrDefault public static double redObeliskTurrPos1 = 0.12; @@ -59,12 +59,12 @@ public class Auto_LT_Close extends LinearOpMode { public static double flywheel0Time = 3.5; public static double pickup1Speed = 12; // ---- POSITION TOLERANCES ---- - public static double posXTolerance = 2.0; - public static double posYTolerance = 2.0; + public static double posXTolerance = 5.0; + public static double posYTolerance = 5.0; // ---- OBELISK DETECTION ---- - public static double shoot1Time = 2; - public static double shoot2Time = 2; - public static double shoot3Time = 2; + public static double shoot1Time = 2.5; + public static double shoot2Time = 2.5; + public static double shoot3Time = 2.5; public static double colorSenseTime = 1.2; public int motif = 0; @@ -354,6 +354,7 @@ public class Auto_LT_Close extends LinearOpMode { 0.501, 0.501, 0.501, + 0.501, false ), autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) @@ -374,6 +375,7 @@ public class Auto_LT_Close extends LinearOpMode { y1, posXTolerance, posYTolerance, + h1, false ) @@ -391,6 +393,7 @@ public class Auto_LT_Close extends LinearOpMode { y2b, posXTolerance, posYTolerance, + h2b, true ), autoActions.intake(intake1Time), @@ -413,12 +416,15 @@ public class Auto_LT_Close extends LinearOpMode { double posX; double posY; + double posH; if (ballCycles > 1){ posX = xShoot; posY = yShoot; + posH = hShoot; } else { posX = xLeave; posY = yLeave; + posH = hLeave; } Actions.runBlocking( @@ -429,6 +435,7 @@ public class Auto_LT_Close extends LinearOpMode { posY, posXTolerance, posYTolerance, + posH, false ), shoot1.build(), @@ -447,6 +454,7 @@ public class Auto_LT_Close extends LinearOpMode { y3b, posXTolerance, posYTolerance, + h3b, true ), autoActions.intake(intake2Time), @@ -469,12 +477,15 @@ public class Auto_LT_Close extends LinearOpMode { double posX; double posY; + double posH; if (ballCycles > 2){ posX = xShoot; posY = yShoot; + posH = hShoot; } else { posX = xLeave; posY = yLeave; + posH = hLeave; } Actions.runBlocking( @@ -485,6 +496,7 @@ public class Auto_LT_Close extends LinearOpMode { posY, posXTolerance, posYTolerance, + posH, false ), shoot2.build(), @@ -503,6 +515,7 @@ public class Auto_LT_Close extends LinearOpMode { y4b, posXTolerance, posYTolerance, + h4b, true ), autoActions.intake(intake3Time), @@ -531,6 +544,7 @@ public class Auto_LT_Close extends LinearOpMode { yLeave, posXTolerance, posYTolerance, + hLeave, false ), shoot3.build(), 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 039fa13..adf2cf5 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 @@ -81,9 +81,9 @@ public class Auto_LT_Far extends LinearOpMode { boolean stack3 = true; double xStackPickupA, yStackPickupA, hStackPickupA; double xStackPickupB, yStackPickupB, hStackPickupB; - public static int pickupStackSpeed = 15; + public static int pickupStackSpeed = 12; int prevMotif = 0; - + public static double spindexerSpeedIncrease = 0.005; // initialize path variables here @@ -236,8 +236,6 @@ 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(); @@ -246,6 +244,8 @@ public class Auto_LT_Far extends LinearOpMode { //cycleStackFar(); } + //TODO: insert code here for gate auto + if (gatePickup || stack3){ leave(); } else { @@ -269,22 +269,22 @@ public class Auto_LT_Far extends LinearOpMode { } -// void shoot(){ -// Actions.runBlocking( -// new ParallelAction( -// autoActions.manageShooterAuto( -// shootAllTime, -// 0.501, -// 0.501, -// 0.501, -// 0.501, -// false -// ), -// autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) -// ) -// -// ); -// } + void shoot(){ + Actions.runBlocking( + new ParallelAction( + autoActions.manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501, + false + ), + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + + ); + } void startAuto(){ Actions.runBlocking( @@ -295,6 +295,7 @@ public class Auto_LT_Far extends LinearOpMode { 0.501, 0.501, 0.501, + 0.501, false ) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 3f856d0..6748024 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -2,7 +2,6 @@ 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; @@ -21,6 +20,7 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -50,6 +50,7 @@ public class AutoActions{ private boolean shootForward = true; public static double firstShootTime = 0.3; public int motif = 0; + double spinEndPos = ServoPositions.spinEndPos; public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur){ this.robot = rob; @@ -72,6 +73,36 @@ public class AutoActions{ boolean decideGreenSlot = false; + void spin1PosFirst(){ + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + spinEndPos = spindexer_outtakeBall3 + 0.1; + } + + void spin2PosFirst(){ + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = false; + spinEndPos = spindexer_outtakeBall3b - 0.1; + } + + void reverseSpin2PosFirst(){ + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = true; + spinEndPos = 0.95; + } + + void spin3PosFirst(){ + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + spinEndPos = spindexer_outtakeBall1 - 0.1; + } + + void oddSpin3PosFirst(){ + firstSpindexShootPos = spindexer_outtakeBall3b; + shootForward = true; + spinEndPos = spindexer_outtakeBall2 + 0.1; + } + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -122,39 +153,29 @@ public class AutoActions{ if (motif_id == 21) { if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; + spin1PosFirst(); } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; + spin2PosFirst(); } else { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; + spin3PosFirst(); } } else if (motif_id == 22) { if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = false; + spin2PosFirst(); } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; + spin3PosFirst(); } else { - firstSpindexShootPos = spindexer_outtakeBall2; - shootForward = true; + reverseSpin2PosFirst(); } } else { if (mostGreenSlot == 1) { - firstSpindexShootPos = spindexer_outtakeBall3; - shootForward = false; + spin3PosFirst(); } else if (mostGreenSlot == 2) { - firstSpindexShootPos = spindexer_outtakeBall3b; - shootForward = true; + oddSpin3PosFirst(); } else { - firstSpindexShootPos = spindexer_outtakeBall1; - shootForward = true; + spin1PosFirst(); } - } return true; @@ -216,7 +237,7 @@ public class AutoActions{ turret.trackGoal(deltaPose); - if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { + if ((System.currentTimeMillis() - stamp < shootTime*1000 && servos.getSpinPos() < 0.85) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(spinStartPos)) { servos.setSpinPos(spinStartPos); @@ -243,6 +264,7 @@ public class AutoActions{ }; } + private boolean doneShooting = false; public Action shootAllAuto(double shootTime, double spindexSpeed) { return new Action() { int ticker = 1; @@ -271,9 +293,16 @@ public class AutoActions{ double prevSpinPos = servos.getSpinCmdPos(); - if (System.currentTimeMillis() - stamp < shootTime*1000 || (prevSpinPos < spinEndPos && shooterTicker < 2)) { + boolean end; + if (shootForward){ + end = prevSpinPos > spinEndPos; + } else { + end = prevSpinPos < spinEndPos; + } - if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker == 0) { + if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) { + + if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) { servos.setTransferPos(transferServo_out); servos.setSpinPos(firstSpindexShootPos); } else { @@ -295,6 +324,7 @@ public class AutoActions{ spindexer.resetSpindexer(); spindexer.processIntake(); + doneShooting = true; return false; @@ -443,6 +473,7 @@ public class AutoActions{ double posY, double posXTolerance, double posYTolerance, + double posH, boolean whileIntaking ) { @@ -454,6 +485,8 @@ public class AutoActions{ double stamp = 0.0; int ticker = 0; + int shootingTicker = 0; + double shootingStamp = 0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { @@ -477,14 +510,21 @@ public class AutoActions{ double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + Pose2d deltaPose; + if (posX != 0.501){ + deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); + } else { + deltaPose = new Pose2d(robotX, robotY, robotHeading); + } double distanceToGoal = Math.sqrt(dx * dx + dy * dy); targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - if (!detectingObelisk){turret.trackGoal(deltaPose);} + if (!detectingObelisk){ + turret.trackGoal(deltaPose); + } servos.setHoodPos(targetingSettings.hoodAngle); @@ -504,7 +544,17 @@ public class AutoActions{ teleStart = currentPose; - return !shouldFinish; + if (doneShooting && shootingTicker == 0){ + shootingTicker++; + shootingStamp = System.currentTimeMillis(); + } + + if (System.currentTimeMillis() - shootingStamp > 100 || shouldFinish){ + doneShooting = false; + return false; + } else { + return true; + } } };