working auto

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

@@ -97,12 +97,12 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
     public static double normalIntakeTime = 3.0;
     public static double shoot1Turr = 0.57;
     public static double shoot0XTolerance = 1.0;
-    public static double turretShootPos = 0.5;
+    public static double turretShootPos = 0.53;
     public static double shootAllTime = 1.8;
     public static double shoot0Time = 1.6;
     public static double intake1Time = 3.0;
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 20;
+    public static double pickup1Speed = 25;
     // ---- SECOND SHOT / PICKUP ----
     public static double shoot1Vel = 2300;
     public static double shoot1Hood = 0.93;
@@ -175,6 +175,10 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("Hood", robot.hood.getPosition());
+                TELE.update();
+
                 return (System.currentTimeMillis() - stamp) < (time * 1000);
 
             }
@@ -193,8 +197,8 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                TELE.addData("Velocity", velo);
+                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addLine("shooting");
+                TELE.addData("Hood", robot.hood.getPosition());
                 TELE.update();
 
                 flywheel.manageFlywheel(vel);
@@ -258,8 +262,8 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                TELE.addData("Velocity", velo);
+                TELE.addData("Velocity", flywheel.getVelo());
-                TELE.addLine("shooting");
+                TELE.addData("Hood", robot.hood.getPosition());
                 TELE.update();
 
                 velo = flywheel.getVelo();
@@ -331,6 +335,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("Hood", robot.hood.getPosition());
+                TELE.update();
 
                 return (System.currentTimeMillis() - stamp) < (intakeTime * 1000);
 
@@ -381,6 +388,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("Hood", robot.hood.getPosition());
+                TELE.update();
 
                 return !shouldFinish;
 
@@ -430,6 +440,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("Hood", robot.hood.getPosition());
+                TELE.update();
 
                 return !shouldFinish;
 
@@ -497,6 +510,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("Hood", robot.hood.getPosition());
+                TELE.update();
 
                 return !shouldFinish;
 
@@ -563,6 +579,9 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
+                TELE.addData("Velocity", flywheel.getVelo());
+                TELE.addData("Hood", robot.hood.getPosition());
+                TELE.update();
 
                 return !shouldFinish;
 

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

@@ -14,17 +14,17 @@ public class Poses {
 
     public static Pose2d goalPose = new Pose2d(-10, 0, 0);
 
-    public static double rx1 = 20, ry1 = 0, rh1 = 0;
+    public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1);
     public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
-    public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140);
+    public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140.1);
 
     public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140);
 
     public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140);
-    public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140);
+    public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1);
 
-    public static double rx4a = 68, ry4a = 59, rh4a = Math.toRadians(140);
+    public static double rx4a = 72, ry4a = 65, rh4a = Math.toRadians(145);
-    public static double rx4b = 40, ry4b = 87, rh4b = Math.toRadians(140);
+    public static double rx4b = 37, ry4b = 85, rh4b = Math.toRadians(145.1);
 
     public static double bx1 = 40, by1 = 7, bh1 = 0;
     public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140);
@@ -38,8 +38,8 @@ public class Poses {
     public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
 
-    public static double rShootX = 40, rShootY = 0, rShootH = Math.toRadians(50);
+    public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50);
-    public static double rxPrep = 50, ryPrep = 10, rhPrep = Math.toRadians(50);
+    public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50);
 
     public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140);
     public static double bxPrep = 50, byPrep = -10, bhPrep = Math.toRadians(140);