finished

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

@@ -29,7 +29,7 @@ import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
-@Autonomous
+@Autonomous(preselectTeleOp = "TeleopV2")
 public class Blue_V2 extends LinearOpMode {
 
     Robot robot;
@@ -44,9 +44,9 @@ public class Blue_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;
 
@@ -187,6 +187,39 @@ public class Blue_V2 extends LinearOpMode {
         };
     }
 
+    public Action intakeReject() {
+        return new Action() {
+            double stamp = 0.0;
+            int ticker = 0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                double position = 0.0;
+
+                if ((getRuntime() % 0.3) > 0.15) {
+                    position = spindexer_intakePos1 + 0.02;
+                } else {
+                    position = spindexer_intakePos1 - 0.02;
+                }
+
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1 - position);
+
+                if (ticker == 0) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                if (getRuntime() - stamp < 0.3){
+                    return true;
+                }else {
+                    robot.intake.setPower(0);
+                    return false;
+                }
+            }
+        };
+    }
+
     public Action spindex (double spindexer, double vel){
         return new Action() {
             double currentPos = 0.0;
@@ -359,7 +392,6 @@ public class Blue_V2 extends LinearOpMode {
 
                 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;
@@ -567,7 +599,8 @@ public class Blue_V2 extends LinearOpMode {
                     new ParallelAction(
                             shoot1.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
                     )
             );
 
@@ -599,7 +632,8 @@ public class Blue_V2 extends LinearOpMode {
                     new ParallelAction(
                             shoot2.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
                     )
             );
 

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;
 
@@ -358,7 +358,6 @@ public class Red_V2 extends LinearOpMode {
 
                 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;
@@ -367,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;
@@ -567,7 +587,8 @@ public class Red_V2 extends LinearOpMode {
                     new ParallelAction(
                             shoot1.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
                     )
             );
 
@@ -599,7 +620,9 @@ public class Red_V2 extends LinearOpMode {
                     new ParallelAction(
                             shoot2.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
+
                     )
             );
 

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

@@ -26,7 +26,7 @@ public class Poses {
 
 
 
-    public static double rx1 = 46, ry1 = -4, rh1 = 0;
+    public static double rx1 = 45, ry1 = -7, rh1 = 0;
 
     public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
 
@@ -36,16 +36,16 @@ public class Poses {
 
     public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
 
-    public static double bx1 = 46, by1 = 4, bh1 = 0;
+    public static double bx1 = 45, by1 = 6, bh1 = 0;
 
-    public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140);
+    public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140);
 
-    public static double bx2b = 31, by2b = -32, bh2b = Math.toRadians(-140);
+    public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140);
 
-    public static double bx3a = 58, by3a = -42, bh3a = Math.toRadians(-140);
+    public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140);
 
     public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140);
 
-    public static Pose2d teleStart = new Pose2d(rx1, 10, 0);
+    public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1);
 
 }

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

@@ -24,13 +24,13 @@ public class ServoPositions {
 
     public static double hoodDefault = 0.6;
 
-    public static double hoodAuto = 0.59;
+    public static double hoodAuto = 0.55;
 
     public static double hoodHigh = 0.21;
 
     public static double hoodLow = 1.0;
 
-    public static double turret_red = 0.4;
+    public static double turret_red = 0.42;
     public static double turret_blue = 0.38;
 
     public static double turret_detectRed = 0.2;

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

@@ -73,13 +73,13 @@ public final class MecanumDrive {
         public double kA = 0.000046;
 
         // path profile parameters (in inches)
-        public double maxWheelVel = 120;
-        public double minProfileAccel = -30;
-        public double maxProfileAccel = 120;
+        public double maxWheelVel = 180;
+        public double minProfileAccel = -40;
+        public double maxProfileAccel = 180;
 
         // turn profile parameters (in radians)
-        public double maxAngVel = 2* Math.PI; // shared with path
-        public double maxAngAccel = 2* Math.PI;
+        public double maxAngVel = 4* Math.PI; // shared with path
+        public double maxAngAccel = 4* Math.PI;
 
         // path controller gains
         public double axialGain = 4;

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

@@ -43,7 +43,6 @@ public class TeleopV2 extends LinearOpMode {
     public static double velMultiplier = 20;
     public static double shootStamp2 = 0.0;
 
-
     public double vel = 3000;
     public boolean autoVel = true;
     public double manualOffset = 0.0;
@@ -89,7 +88,6 @@ public class TeleopV2 extends LinearOpMode {
     boolean outtake3 = false;
     boolean overrideTurr = false;
 
-
     List<Integer> shootOrder = new ArrayList<>();
     boolean emergency = false;
     private double lastEncoderRevolutions = 0.0;
@@ -171,15 +169,11 @@ public class TeleopV2 extends LinearOpMode {
                 emergency = false;
                 overrideTurr = false;
 
-
             }
 
             if (gamepad1.leftBumperWasPressed()) {
                 intake = false;
-                reject = true;
-                shootAll = false;
-                overrideTurr = false;
-
+                emergency = !emergency;
 
             }
 
@@ -336,8 +330,11 @@ public class TeleopV2 extends LinearOpMode {
 
             double offset;
 
-            double robotX = drive.localizer.getPose().position.x - xOffset;
-            double robotY = drive.localizer.getPose().position.y - xOffset;
+            double robX = drive.localizer.getPose().position.x;
+            double robY = drive.localizer.getPose().position.y;
+
+            double robotX = robX - xOffset;
+            double robotY = robY - yOffset;
             double robotHeading = drive.localizer.getPose().heading.toDouble();
 
             double goalX = -10;
@@ -431,13 +428,11 @@ public class TeleopV2 extends LinearOpMode {
             if (gamepad2.left_stick_x > 0.5) {
                 manualTurret = false;
             } else if (gamepad2.left_stick_x < -0.5) {
-                manualTurret = true;
                 manualOffset = 0;
-                autoHoodOffset = 0;
+                manualTurret = false;
                 if (gamepad2.left_bumper) {
-                    xOffset = robotX;
-                    yOffset = robotY;
-                    headingOffset = robotHeading;
+                    drive = new MecanumDrive(hardwareMap, new Pose2d(2, 0, 0));
+                    sleep(1200);
                 }
             }
 
@@ -457,7 +452,7 @@ public class TeleopV2 extends LinearOpMode {
 
             if (emergency) {
                 intake = false;
-                reject = false;
+                reject = true;
 
                 if (getRuntime() % 3 > 1.5) {
                     robot.spin1.setPosition(0);
@@ -468,12 +463,7 @@ public class TeleopV2 extends LinearOpMode {
                     robot.spin2.setPosition(0);
                 }
 
-                if (getRuntime() % 1 < 0.5) {
-                    robot.transferServo.setPosition(transferServo_in);
-                } else {
-                    robot.transferServo.setPosition(transferServo_out);
-
-                }
+                robot.transferServo.setPosition(transferServo_out);
 
                 robot.transfer.setPower(1);
 
@@ -494,25 +484,22 @@ public class TeleopV2 extends LinearOpMode {
                     if (d20 != null) {
                         overrideTurr = true;
                         double bearing = d20.ftcPose.bearing;
-                        double finalPos =robot.turr1.getPosition() -  (bearing/1300);
+
+                        double finalPos = robot.turr1.getPosition() - (bearing / 1300);
                         robot.turr1.setPosition(finalPos);
-                        robot.turr2.setPosition(1-finalPos);
+                        robot.turr2.setPosition(1 - finalPos);
 
                         TELE.addData("Bear", bearing);
 
-
-
-
-
                     }
 
                     if (d24 != null) {
                         overrideTurr = true;
 
                         double bearing = d24.ftcPose.bearing;
-                        double finalPos = turretPos() +  (bearing/720);
+                        double finalPos = robot.turr1.getPosition() - (bearing / 1300);
                         robot.turr1.setPosition(finalPos);
-                        robot.turr2.setPosition(1-finalPos);
+                        robot.turr2.setPosition(1 - finalPos);
 
                     }
 
@@ -577,10 +564,9 @@ public class TeleopV2 extends LinearOpMode {
                     outtake1 = false;
                     outtake2 = false;
                     outtake3 = false;
+
                     overrideTurr = false;
 
-
-
                 }
 
             }
@@ -660,17 +646,17 @@ public class TeleopV2 extends LinearOpMode {
 
                 // Fastest order (example: slot 3 → 2 → 1)
 
-                if (ballIn(3)){
+                if (ballIn(3)) {
                     shootOrder.add(3);
 
                 }
 
-                if (ballIn(2)){
+                if (ballIn(2)) {
                     shootOrder.add(2);
 
                 }
 
-                if (ballIn(1)){
+                if (ballIn(1)) {
                     shootOrder.add(1);
 
                 }
@@ -690,43 +676,46 @@ public class TeleopV2 extends LinearOpMode {
                     shootOrder.add(1);
                 }
 
-
-                shootAll = true;
-                shootPos = drive.localizer.getPose();
-
-            }
-
-            // Right bumper shoots all balls fastest, ignoring colors
-            if (gamepad2.leftBumperWasPressed()) {
-                shootOrder.clear();
-                shootStamp = getRuntime();
-
-                outtake1 = false;
-                outtake2 = false;
-                outtake3 = false;
-
-                // Fastest order (example: slot 3 → 2 → 1)
-
-                if (ballIn(3)) {
-                    shootOrder.add(3);
-                }
-
-                if (ballIn(2)) {
-                    shootOrder.add(2);
-                }
-                if (ballIn(1)) {
-                    shootOrder.add(1);
-                }
                 shootAll = true;
                 shootPos = drive.localizer.getPose();
 
             }
 
+//            // Right bumper shoots all balls fastest, ignoring colors
+//            if (gamepad2.leftBumperWasPressed()) {
+//                shootOrder.clear();
+//                shootStamp = getRuntime();
+//
+//                outtake1 = false;
+//                outtake2 = false;
+//                outtake3 = false;
+//
+//                // Fastest order (example: slot 3 → 2 → 1)
+//
+//                if (ballIn(3)) {
+//                    shootOrder.add(3);
+//                }
+//
+//                if (ballIn(2)) {
+//                    shootOrder.add(2);
+//                }
+//                if (ballIn(1)) {
+//                    shootOrder.add(1);
+//                }
+//                shootAll = true;
+//                shootPos = drive.localizer.getPose();
+//
+//            }
+//
             if (gamepad2.crossWasPressed()) {
-                emergency = !emergency;
+                emergency = true;
 
             }
 
+            if (gamepad2.leftBumperWasPressed()) {
+                emergency = false;
+            }
+
             //MISC:
 
             drive.updatePoseEstimate();
@@ -943,7 +932,7 @@ public class TeleopV2 extends LinearOpMode {
     }
 
     public double turretPos() {
-        return (scalar*((robot.turr1Pos.getVoltage()-restPos) /3.3 ));
+        return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3));
     }
 
 }