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 b06233d..5f89a54 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 @@ -62,17 +62,20 @@ public class Auto_LT_Close extends LinearOpMode { public static double shoot2Time = 2.5; public static double shoot3Time = 2.5; public static double colorSenseTime = 1.2; - public int motif = 0; public static double waitToShoot0 = 0.5; public static double waitToPickupGate2 = 0.3; public static double pickupStackGateSpeed = 50; public static double intake2TimeGate = 3; public static double shoot2GateTime = 3; - public static double endGateTime = 25; + public static double endGateTime = 22; public static double waitToPickupGateWithPartner = 2; public static double waitToPickupGateSolo = 1; public static double intakeGateTime = 5; public static double shootGateTime = 3; + public static double shoot1GateTime = 3; + public static double intake1GateTime = 3.3; + public static double lastIntakeTime = 27; + public static double lastShootTime = 27; Robot robot; MultipleTelemetry TELE; @@ -85,8 +88,9 @@ public class Auto_LT_Close extends LinearOpMode { Targeting.Settings targetingSettings; AutoActions autoActions; Light light; - double x1, y1, h1; + int motif = 0; + double x1, y1, h1; double x2a, y2a, h2a, t2a; double x2b, y2b, h2b, t2b; @@ -405,6 +409,14 @@ public class Auto_LT_Close extends LinearOpMode { cycleGateShoot(); } + if (getRuntime() - stamp < lastIntakeTime){ + cycleStackCloseIntakeGate(); + } + + if (getRuntime() - stamp < lastShootTime){ + cycleStackCloseShootGate(); + } + } else { startAuto(); shoot(); @@ -709,4 +721,39 @@ public class Auto_LT_Close extends LinearOpMode { ) ); } + + void cycleStackCloseIntakeGate(){ + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + autoActions.intake( + intake1GateTime, + xShootGate, + yShootGate, + hShootGate + ) + ) + ); + } + + void cycleStackCloseShootGate(){ + servos.setSpinPos(spinStartPos); + Actions.runBlocking( + new ParallelAction( + shoot1.build(), + new SequentialAction( + new ParallelAction( + autoActions.manageShooterAuto( + shoot1GateTime, + xLeaveGate, + yLeaveGate, + hLeaveGate + ), + autoActions.Wait(shoot1GateTime) + ), + autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) + ) + ); + } } \ No newline at end of file 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 c3bd77b..45186cf 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 @@ -54,8 +54,8 @@ public class Front_Poses { public static double rPickupGateAX = 36, rPickupGateAY = 50, rPickupGateAH = 140; public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140; - public static double rPickupGateBX = 46, rPickupGateBY = 60, rPickupGateBH = 140; - public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -140; + public static double rPickupGateBX = 46, rPickupGateBY = 60, rPickupGateBH = 180; + public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -180; public static Pose2d teleStart = new Pose2d(0, 0, 0); }