stash

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

@@ -28,7 +28,7 @@ import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
-@Autonomous
+@Autonomous(preselectTeleOp = "TeleopV2")
 public class Red_V2 extends LinearOpMode {
 
     Robot robot;
@@ -43,9 +43,9 @@ public class Red_V2 extends LinearOpMode {
 
     double velo = 0.0;
     double targetVelocity = 0.0;
-    public static double intake1Time = 6.5;
+    public static double intake1Time = 2.9;
 
-    public static double intake2Time = 6.5;
+    public static double intake2Time = 2.9;
 
     public static double colorDetect = 3.0;
 
@@ -131,8 +131,12 @@ public class Red_V2 extends LinearOpMode {
                 TELE.addData("Velocity", velo);
                 TELE.update();
 
+
                 if (last && !steady){
                     stamp = getRuntime();
+                    drive.updatePoseEstimate();
+
+                    teleStart = drive.localizer.getPose();
                     return false;
                 } else if (steady) {
                     stamp = getRuntime();
@@ -230,6 +234,11 @@ public class Red_V2 extends LinearOpMode {
                 TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 return !spindexPosEqual(spindexer);
             }
         };
@@ -284,6 +293,10 @@ public class Red_V2 extends LinearOpMode {
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
 
                 if (ticker == 1) {
                     transferStamp = getRuntime();
@@ -339,9 +352,12 @@ public class Red_V2 extends LinearOpMode {
                 TELE.addLine("Intaking");
                 TELE.update();
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 robot.intake.setPower(1);
                 if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
-                    robot.intake.setPower(0);
                     return false;
                 } else {
                     return true;
@@ -350,6 +366,27 @@ public class Red_V2 extends LinearOpMode {
         };
     }
 
+    public Action intakeReject() {
+        return new Action() {
+            double stamp = 0.0;
+            int ticker = 0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if (ticker == 0) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                if (getRuntime() - stamp < 0.3){
+                    return true;
+                }else {
+                    robot.intake.setPower(0);
+                    return false;
+                }
+            }
+        };
+    }
+
     public Action ColorDetect() {
         return new Action() {
             double stamp = 0.0;
@@ -374,6 +411,10 @@ public class Red_V2 extends LinearOpMode {
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 if (s1D < 40) {
 
                     double green = robot.color1.getNormalizedColors().green;
@@ -477,14 +518,14 @@ public class Red_V2 extends LinearOpMode {
         while (opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()) {
-                hoodDefault -= 0.01;
+                hoodAuto-= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()) {
-                hoodDefault += 0.01;
+                hoodAuto += 0.01;
             }
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
 
             robot.turr1.setPosition(turret_detectRed);
             robot.turr2.setPosition(1 - turret_detectRed);
@@ -499,13 +540,14 @@ public class Red_V2 extends LinearOpMode {
             TELE.update();
         }
 
+
         waitForStart();
 
         if (isStopRequested()) return;
 
         if (opModeIsActive()) {
 
-            robot.hood.setPosition(hoodStart);
+            robot.hood.setPosition(hoodAuto);
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -514,6 +556,9 @@ public class Red_V2 extends LinearOpMode {
                             Obelisk()
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
@@ -522,7 +567,11 @@ public class Red_V2 extends LinearOpMode {
 
             shootingSequence();
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -530,21 +579,32 @@ public class Red_V2 extends LinearOpMode {
                             intake(intake1Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
                             shoot1.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
                     )
             );
 
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
             powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
             shootingSequence();
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -552,12 +612,17 @@ public class Red_V2 extends LinearOpMode {
                             intake(intake2Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
                             shoot2.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
+
                     )
             );