Auton, hopefully pintpoint works ig

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

@@ -13,16 +13,22 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferSe
 import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
 
+import androidx.annotation.NonNull;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+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.PoseVelocity2d;
 import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 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.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -64,13 +70,13 @@ 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 = 30;
+    public static double pickupStackGateSpeed = 13;
     public static double intake2TimeGate = 3;
     public static double shoot2GateTime = 1.7;
     public static double endGateTime = 22;
-    public static double waitToPickupGateWithPartner = 0.7;
+    public static double waitToPickupGateWithPartner = 0.01;
     public static double waitToPickupGateSolo = 0.01;
-    public static double intakeGateTime = 8;
+    public static double intakeGateTime = 5;
     public static double shootGateTime = 1.7;
     public static double shoot1GateTime = 1.7;
     public static double intake1GateTime = 3.3;
@@ -167,6 +173,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
         while (opModeInInit()) {
 
+
             if (gateCycle) {
                 servos.setHoodPos(hoodGate0Start);
             } else {
@@ -195,6 +202,10 @@ public class Auto_LT_Close extends LinearOpMode {
             }
 
             if (gamepad2.squareWasPressed()) {
+
+                drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
+
+
                 if (!gateCycle) {
                     turret.pipelineSwitch(1);
                 } else if (redAlliance) {
@@ -203,6 +214,9 @@ public class Auto_LT_Close extends LinearOpMode {
                     turret.pipelineSwitch(2);
                 }
                 robot.limelight.start();
+
+
+
                 gamepad2.rumble(500);
             }
 
@@ -320,37 +334,14 @@ public class Auto_LT_Close extends LinearOpMode {
 
             if (gateCycle) {
                 shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                        .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
+                        .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
             } else {
                 shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                        .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
+                        .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
             }
 
             if (gateCycle) {
-                pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
+                pickup2 = shoot0.endTrajectory().fresh()
-                        .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
-                        .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
-                                new TranslationalVelConstraint(pickupStackGateSpeed));
-            } else {
-                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));
-            }
-
-            if (gateCycle) {
-                shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
-                        .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
-            } else 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));
-            }
-
-            if (gateCycle) {
-                pickup2 = drive.actionBuilder(new Pose2d(xShoot0, yShoot0, Math.toRadians(hShoot0)))
                         .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
                         .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
                                 new TranslationalVelConstraint(pickupStackGateSpeed));
@@ -362,7 +353,7 @@ public class Auto_LT_Close extends LinearOpMode {
             }
 
             if (gateCycle) {
-                shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
+                shoot2 = pickup2.endTrajectory().fresh()
                         .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
             } else if (ballCycles < 3) {
                 shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
@@ -372,6 +363,43 @@ public class Auto_LT_Close extends LinearOpMode {
                         .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
             }
 
+            gateCyclePickup = shoot2.endTrajectory().fresh()
+                    .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
+                    .waitSeconds(waitToPickupGate)
+                    .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
+
+            gateCycleShoot = gateCyclePickup.endTrajectory().fresh()
+                    .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
+
+
+            if (gateCycle) {
+                pickup1 = gateCycleShoot.endTrajectory().fresh()
+                        .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
+                        .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
+                                new TranslationalVelConstraint(pickupStackGateSpeed));
+            } else {
+                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));
+            }
+
+
+            if (gateCycle) {
+                shoot1 = pickup1.endTrajectory().fresh()
+                        .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
+            } else 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));
+            }
+
+
+
+
+
             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),
@@ -386,14 +414,10 @@ public class Auto_LT_Close extends LinearOpMode {
                 waitToPickupGate = waitToPickupGateSolo;
             }
 
-            gateCyclePickup = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
-                    .strafeToLinearHeading(new Vector2d(pickupGateAX, pickupGateAY), Math.toRadians(pickupGateAH))
-                    .waitSeconds(waitToPickupGate)
-                    .strafeToLinearHeading(new Vector2d(pickupGateBX, pickupGateBY), Math.toRadians(pickupGateBH));
 
-            gateCycleShoot = drive.actionBuilder(new Pose2d(pickupGateBX, pickupGateBY, Math.toRadians(pickupGateBH)))
-                    .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
 
+
+            teleStart = drive.localizer.getPose();
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
             TELE.addData("Ball Cycles", ballCycles);
@@ -411,6 +435,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
             robot.transfer.setPower(1);
 
+
             if (gateCycle) {
                 startAutoGate();
                 shoot();
@@ -421,6 +446,7 @@ public class Auto_LT_Close extends LinearOpMode {
                     cycleGateIntake();
                     if (getRuntime() - stamp < lastShootTime) {
                         cycleGateShoot();
+                        shoot();
                     }
                 }
                 cycleStackCloseIntakeGate();
@@ -487,6 +513,8 @@ public class Auto_LT_Close extends LinearOpMode {
         );
     }
 
+
+
     void startAutoGate() {
         assert shoot0 != null;
 
@@ -498,7 +526,7 @@ public class Auto_LT_Close extends LinearOpMode {
                                 xShoot0,
                                 yShoot0,
                                 hShoot0,
-                                false
+                                true
                         )
                 )
         );
@@ -656,6 +684,11 @@ public class Auto_LT_Close extends LinearOpMode {
     }
 
     void cycleStackMiddleGate() {
+        drive.updatePoseEstimate();
+        pickup2 = drive.actionBuilder(drive.localizer.getPose())
+                .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
+                .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
+                        new TranslationalVelConstraint(pickupStackGateSpeed));
         Actions.runBlocking(
                 new ParallelAction(
                         pickup2.build(),
@@ -684,6 +717,12 @@ 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(),
@@ -698,7 +737,13 @@ public class Auto_LT_Close extends LinearOpMode {
     }
 
     void cycleGateShoot() {
+
+        drive.updatePoseEstimate();
         servos.setSpinPos(spinStartPos);
+
+        gateCycleShoot = drive.actionBuilder(drive.localizer.getPose())
+                .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
+
         Actions.runBlocking(
                 new ParallelAction(
                         gateCycleShoot.build(),
@@ -714,6 +759,14 @@ public class Auto_LT_Close extends LinearOpMode {
     }
 
     void cycleStackCloseIntakeGate() {
+        drive.updatePoseEstimate();
+
+        pickup1 = drive.actionBuilder(drive.localizer.getPose())
+                .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
+                .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
+                        new TranslationalVelConstraint(pickupStackGateSpeed));
+
+
         Actions.runBlocking(
                 new ParallelAction(
                         pickup1.build(),
@@ -729,6 +782,11 @@ public class Auto_LT_Close extends LinearOpMode {
 
     void cycleStackCloseShootGate(){
         servos.setSpinPos(spinStartPos);
+        drive.updatePoseEstimate();
+
+        shoot1 = drive.actionBuilder(drive.localizer.getPose())
+                .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
+
         Actions.runBlocking(
                 new ParallelAction(
                         shoot1.build(),

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

@@ -10,10 +10,10 @@ public class Front_Poses {
     public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
     public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
 
-    public static double rx2a = 41, ry2a = 18, rh2a = 140;
+    public static double rx2a = 41, ry2a = 18, rh2a = 155;
     public static double bx2a = 41, by2a = -18, bh2a = -140;
 
-    public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
+    public static double rx2b = 21, ry2b = 34, rh2b = 155.1;
     public static double bx2b = 23, by2b = -36, bh2b = -140.1;
 
     public static double rx3a = 55, ry3a = 39, rh3a = 140;
@@ -42,8 +42,8 @@ public class Front_Poses {
     public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
     public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
 
-    public static double rShoot0X = 40, rShoot0Y = 0.1, rShoot0H = 0.1;
+    public static double rShoot0X = 53, rShoot0Y = 10.1, rShoot0H = 80.1;
-    public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1;
+    public static double bShoot0X = 53, bShoot0Y = -10.1, bShoot0H = -80.1;
 
     public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90;
     public static double bShootGateX = 50, bShootGateY = -10, bShootGateH = -90;
@@ -51,10 +51,10 @@ 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 = 24, rPickupGateAY = 50, rPickupGateAH = 140;
+    public static double rPickupGateAX = 26, rPickupGateAY = 48, rPickupGateAH = 140;
     public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140;
 
-    public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180;
+    public static double rPickupGateBX = 35, rPickupGateBY = 65, rPickupGateBH = 180;
     public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);

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

@@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.13; //0.13;
+    public static double spindexer_intakePos1 = 0.22; //0.13;
 
-    public static double spindexer_intakePos2 = 0.32; //0.33;//0.5;
+    public static double spindexer_intakePos2 = 0.41; //0.33;//0.5;
 
-    public static double spindexer_intakePos3 = 0.5; //0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.60; //0.53;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.78; //0.65; //0.24;
+    public static double spindexer_outtakeBall3 = 0.88; //0.65; //0.24;
-    public static double spindexer_outtakeBall3b = 0.22; //0.65; //0.24;
+    public static double spindexer_outtakeBall3b = 0.31; //0.65; //0.24;
 
     public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6;
-    public static double spindexer_outtakeBall1 = 0.42; //0.27; //0.4;
+    public static double spindexer_outtakeBall1 = 0.51; //0.27; //0.4;
-    public static double spinStartPos = 0.05;
+    public static double spinStartPos = 0.14;
     public static double spinEndPos = 0.95;
 
     public static double shootAllSpindexerSpeedIncrease = 0.014;

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

@@ -210,9 +210,9 @@ public final class MecanumDrive {
         public double maxAngAccel = 2 * Math.PI;
 
         // path controller gains
-        public double axialGain = 6;
+        public double axialGain = 6.0;
-        public double lateralGain = 6;
+        public double lateralGain = 6.0;
-        public double headingGain = 6; // shared with turn
+        public double headingGain = 6.0; // shared with turn
 
         public double axialVelGain = 0.0;
         public double lateralVelGain = 0.0;
@@ -352,9 +352,9 @@ public final class MecanumDrive {
 
             Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
 
-            if ((t >= timeTrajectory.duration && error.position.norm() < 2
+            if ((t >= timeTrajectory.duration && error.position.norm() < 1
                     && robotVelRobot.linearVel.norm() < 0.5)
-                    || t >= timeTrajectory.duration + 0.5) {
+                    || t >= timeTrajectory.duration + 0.25) {
                 leftFront.setPower(0);
                 leftBack.setPower(0);
                 rightBack.setPower(0);

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

@@ -49,6 +49,8 @@ public final class PinpointLocalizer implements Localizer {
         txWorldPinpoint = initialPose;
     }
 
+
+
     @Override
     public void setPose(Pose2d pose) {
         txWorldPinpoint = pose.times(txPinpointRobot.inverse());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java

@@ -187,7 +187,7 @@ public class Spindexer {
         distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
 
         // Position 1
-        if (distanceRearCenter < 65) {
+        if (distanceRearCenter < 48) {
 
             // Mark Ball Found
             newPos1Detection = true;
@@ -209,7 +209,7 @@ public class Spindexer {
         // Position 2
         // Find which ball position this is in the spindexer
         spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()];
-        if (distanceFrontDriver < 56) {
+        if (distanceFrontDriver < 50) {
             // reset FoundEmpty because looking for 3 in a row before reset
             ballPositions[spindexerBallPos].foundEmpty = 0;
             if (detectFrontColor) {