League Tournament Success

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

@@ -42,7 +42,7 @@ import java.util.Objects;
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
 public class Auto_LT_Far_Indexed extends LinearOpMode {
-    public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
+    public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
     public static double autoSpinStartPos = 0.2;
     public static double shoot0SpinSpeedIncrease = 0.015;
 
@@ -63,8 +63,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
     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 redTurretShootPos = turrDefault ;
-    public static double blueTurretShootPos = turrDefault - 0.14;
+    public static double blueTurretShootPos = turrDefault ;
     double turretShootPos = 0.0;
 
     public static double finalShootAllTime = 3.0;
@@ -96,6 +96,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
     public static double colorSenseTime = 1.2;
 
     public static double firstShootTime = 0.3;
+
+    public static int fwdTime = 200, strafeTime = 2300;
     public int motif = 0;
 
     Robot robot;
@@ -265,7 +267,7 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
         };
     }
 
-    public static int sleepTime = 300;
+    public static int sleepTime = 1300;
 
     public Action shootAll(int vel, double shootTime, double spindexSpeed) {
         return new Action() {
@@ -711,7 +713,7 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
 
         turret.manualSetTurret(turrDefault);
 
-        drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+        drive = new MecanumDrive(hardwareMap, teleEnd);
 
         robot.spin1.setPosition(autoSpinStartPos);
         robot.spin2.setPosition(1 - autoSpinStartPos);
@@ -729,7 +731,7 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
         while (opModeInInit()) {
 
             robot.hood.setPosition(shoot0Hood);
-            turret.manualSetTurret(turrDefault);
+            turret.manualSetTurret(turretShootPos);
 
             if (gamepad2.crossWasPressed()) {
                 redAlliance = !redAlliance;
@@ -751,8 +753,7 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
             blueObeliskTurrPos2 = turrDefault - 0.13;
             blueObeliskTurrPos3 = turrDefault - 0.14;
 
-            redTurretShootPos = turrDefault + 0.12;
+            redTurretShootPos = turrDefault + 0.05;
-            blueTurretShootPos = turrDefault - 0.14;
 
 
 
@@ -815,8 +816,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
                             manageFlywheel(
                                     shoot0Vel,
                                     shoot0Hood,
-                                    flywheel0Time,
+                                    9,
-                                    x1,
+                                    0.501,
                                     0.501,
                                     shoot0XTolerance,
                                     0.501
@@ -825,8 +826,12 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
                     )
             );
 
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
             Actions.runBlocking(
-                    shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
+                    shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
             );
 
             robot.frontLeft.setPower(-0.4);
@@ -834,14 +839,32 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
             robot.backLeft.setPower(-0.4);
             robot.backRight.setPower(-0.4);
 
-            sleep (sleepTime);
+            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);
+
+            while(opModeIsActive()) {
 
                 drive.updatePoseEstimate();
 
@@ -849,8 +872,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
 
                 TELE.addLine("finished");
                 TELE.update();
+            }
 
-            sleep(2000);
 
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -291,6 +291,8 @@ public class TeleopV3 extends LinearOpMode {
             }
 
             if (gamepad2.square) {
+                drive.updatePoseEstimate();
+
                 teleEnd = drive.localizer.getPose();
                 gamepad2.rumble(1000);