IOoverclocked a whole bunch of chaso @Daniel you got this bro

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java

@@ -29,6 +29,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
 import com.acmerobotics.roadrunner.ftc.PinpointIMU;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -50,9 +51,9 @@ public class Auto_LT_Close extends LinearOpMode {
 
     public static double velGate0End = 2700, hoodGate0End = 0.35;
     public static double hood0MoveTime = 2;
-    public static double spindexerSpeedIncrease = 0.016;
+    public static double spindexerSpeedIncrease = 0.014;
 
-    public static double shootAllTime = 4;
+    public static double shootAllTime = 3.5;
     public static double intake1Time = 3.3;
     public static double intake2Time = 3.8;
 
@@ -70,18 +71,22 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double colorSenseTime = 1.2;
     public static double waitToShoot0 = 0.5;
     public static double waitToPickupGate2 = 0.3;
-    public static double pickupStackGateSpeed = 13;
+    public static double pickupStackGateSpeed = 19;
-    public static double intake2TimeGate = 3;
+    public static double intake2TimeGate = 5;
     public static double shoot2GateTime = 1.7;
     public static double endGateTime = 22;
-    public static double waitToPickupGateWithPartner = 0.01;
+    public static double waitToPickupGateWithPartner = 0.7;
     public static double waitToPickupGateSolo = 0.01;
-    public static double intakeGateTime = 5;
+    public static double intakeGateTime = 5.6;
-    public static double shootGateTime = 1.7;
+    public static double shootGateTime = 1.5;
     public static double shoot1GateTime = 1.7;
     public static double intake1GateTime = 3.3;
     public static double lastShootTime = 27;
 
+    public static double openGateX = 26;
+    public static double openGateY = 48;
+    public  static double openGateH = Math.toRadians(155);
+
     Robot robot;
     MultipleTelemetry TELE;
     MecanumDrive drive;
@@ -171,6 +176,9 @@ public class Auto_LT_Close extends LinearOpMode {
 
         robot.light.setPosition(1);
 
+        hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU();
+
+
         while (opModeInInit()) {
 
 
@@ -201,8 +209,11 @@ public class Auto_LT_Close extends LinearOpMode {
                 ballCycles--;
             }
 
+
+
             if (gamepad2.squareWasPressed()) {
 
+
                 drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
 
 
@@ -352,7 +363,11 @@ public class Auto_LT_Close extends LinearOpMode {
                                 new TranslationalVelConstraint(pickup1Speed));
             }
 
-            if (gateCycle) {
+            if (gateCycle&& withPartner) {
+                shoot2 = pickup2.endTrajectory().fresh()
+                        .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH))
+                        .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH));
+            } else if (gateCycle) {
                 shoot2 = pickup2.endTrajectory().fresh()
                         .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
             } else if (ballCycles < 3) {
@@ -366,7 +381,10 @@ public class Auto_LT_Close extends LinearOpMode {
             gateCyclePickup = shoot2.endTrajectory().fresh()
                     .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
                     .waitSeconds(waitToPickupGate)
-                    .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
+                    .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH))
+                    .waitSeconds(0.1)
+                    .strafeToLinearHeading(new Vector2d(pickupGateCX, pickupGateCY), Math.toRadians(pickupGateCH),
+                            new TranslationalVelConstraint(13));
 
             gateCycleShoot = gateCyclePickup.endTrajectory().fresh()
                     .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
@@ -455,6 +473,8 @@ public class Auto_LT_Close extends LinearOpMode {
                     cycleStackCloseShootGate();
                 }
 
+                shoot();
+
             } else {
                 startAuto();
                 shoot();
@@ -526,7 +546,7 @@ public class Auto_LT_Close extends LinearOpMode {
                                 xShoot0,
                                 yShoot0,
                                 hShoot0,
-                                true
+                                false
                         )
                 )
         );
@@ -693,7 +713,7 @@ public class Auto_LT_Close extends LinearOpMode {
                 new ParallelAction(
                         pickup2.build(),
                         autoActions.intake(
-                                intake2Time,
+                                intake2TimeGate,
                                 x3b,
                                 y3b,
                                 h3b
@@ -711,7 +731,7 @@ public class Auto_LT_Close extends LinearOpMode {
                                 motif,
                                 xShootGate,
                                 yShootGate,
-                                hShootGate)
+                                pickupGateAH)
                 )
         );
     }
@@ -719,10 +739,6 @@ public class Auto_LT_Close extends LinearOpMode {
     void cycleGateIntake() {
         drive.updatePoseEstimate();
 
-        gateCyclePickup = drive.actionBuilder(drive.localizer.getPose())
-                .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
-                .waitSeconds(waitToPickupGate)
-                .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
         Actions.runBlocking(
                 new ParallelAction(
                         gateCyclePickup.build(),
@@ -742,7 +758,7 @@ public class Auto_LT_Close extends LinearOpMode {
         servos.setSpinPos(spinStartPos);
 
         gateCycleShoot = drive.actionBuilder(drive.localizer.getPose())
-                .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
+                .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH));
 
         Actions.runBlocking(
                 new ParallelAction(
@@ -751,7 +767,7 @@ public class Auto_LT_Close extends LinearOpMode {
                                 shootGateTime,
                                 xShootGate,
                                 yShootGate,
-                                hShootGate,
+                                pickupGateAH,
                                 false
                         )
                 )

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java

@@ -63,7 +63,7 @@ public class Auto_LT_Far extends LinearOpMode {
     boolean stack3 = true;
     double xStackPickupA, yStackPickupA, hStackPickupA;
     double xStackPickupB, yStackPickupB, hStackPickupB;
-    public static int pickupStackSpeed = 12;
+    public static int pickupStackSpeed = 17;
     public static int pickupGateSpeed = 25;
     int prevMotif = 0;
     public static double spindexerSpeedIncrease = 0.014;
@@ -74,8 +74,8 @@ public class Auto_LT_Far extends LinearOpMode {
     public static double shootStackTime = 2;
     public static double shootGateTime = 2.5;
     public static double colorSenseTime = 1;
-    public static double intakeStackTime = 2.5;
+    public static double intakeStackTime = 4.5;
-    public static double intakeGateTime = 2;
+    public static double intakeGateTime = 8;
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
@@ -184,6 +184,8 @@ public class Auto_LT_Far extends LinearOpMode {
                 if (gamepad2.squareWasPressed()){
                     turret.pipelineSwitch(4);
                     robot.limelight.start();
+                    drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
+
                     gamepad2.rumble(500);
                 }
             } else {
@@ -221,6 +223,8 @@ public class Auto_LT_Far extends LinearOpMode {
                 if (gamepad2.squareWasPressed()){
                     turret.pipelineSwitch(2);
                     robot.limelight.start();
+                    drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
+
                     gamepad2.rumble(500);
                 }
             }
@@ -240,7 +244,10 @@ public class Auto_LT_Far extends LinearOpMode {
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
 
             pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
-                    .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH),
+                    .strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH))
+                    .waitSeconds(0.2)
+                    .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB))
+                    .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC),
                             new TranslationalVelConstraint(pickupGateSpeed));
 
             shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH)))

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java

@@ -10,14 +10,18 @@ public class Back_Poses {
     public static double rShootX = 100, rShootY = 55, rShootH = 90;
     public static double bShootX = 100, bShootY = -55, bShootH = -90;
 
-    public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
+    public static double rStackPickupAX = 73, rStackPickupAY = 51, rStackPickupAH = 140;
     public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
 
-    public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1;
+    public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1;
     public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
 
-    public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140;
+    public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140;
     public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140;
+    public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140;
+    public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190;
+
+
 
     public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
     public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java

@@ -51,11 +51,12 @@ public class Front_Poses {
     public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
     public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
 
-    public static double rPickupGateAX = 26, rPickupGateAY = 48, rPickupGateAH = 140;
+    public static double rPickupGateAX = 31, rPickupGateAY = 53, rPickupGateAH = 150;
-    public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140;
+    public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -150;
 
-    public static double rPickupGateBX = 35, rPickupGateBY = 65, rPickupGateBH = 180;
+    public static double rPickupGateBX = 38, rPickupGateBY = 62, rPickupGateBH = 210;
     public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
+    public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java

@@ -354,7 +354,7 @@ public final class MecanumDrive {
 
             if ((t >= timeTrajectory.duration && error.position.norm() < 1
                     && robotVelRobot.linearVel.norm() < 0.5)
-                    || t >= timeTrajectory.duration + 0.25) {
+                    || t >= timeTrajectory.duration + 0.01) {
                 leftFront.setPower(0);
                 leftBack.setPower(0);
                 rightBack.setPower(0);