auton is updated - to be tested

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
+import static org.firstinspires.ftc.teamcode.constants.Color.*;
 import static org.firstinspires.ftc.teamcode.constants.Poses.*;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
@@ -32,7 +33,7 @@ import java.util.List;
 
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
-public class Red_V3 extends LinearOpMode {
+public class Auto_V3 extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
     MecanumDrive drive;
@@ -40,8 +41,8 @@ public class Red_V3 extends LinearOpMode {
     Servos servo;
 
     double velo = 0.0;
-    public static double intake1Time = 2.9;
+    public static double intake1Time = 2.7;
-    public static double intake2Time = 2.9;
+    public static double intake2Time = 3.0;
     public static double colorDetect = 3.0;
     boolean gpp = false;
     boolean pgp = false;
@@ -51,6 +52,7 @@ public class Red_V3 extends LinearOpMode {
     int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
     int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
     int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
+    public static double holdTurrPow = 0.1; // power to hold turret in place
 
     public Action initShooter(int vel) {
         return new Action() {
@@ -59,7 +61,6 @@ public class Red_V3 extends LinearOpMode {
                 velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
-                robot.transfer.setPower(1);
 
                 TELE.addData("Velocity", velo);
                 TELE.update();
@@ -69,37 +70,6 @@ public class Red_V3 extends LinearOpMode {
         };
     }
 
-    public Action steadyShooter(int vel, boolean last) {
-        return new Action() {
-            double stamp = 0.0;
-            boolean steady = false;
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                steady = flywheel.getSteady();
-                robot.shooter1.setPower(powPID);
-                robot.shooter2.setPower(powPID);
-                robot.transfer.setPower(1);
-
-                TELE.addData("Velocity", velo);
-                TELE.update();
-                detectTag();
-
-                if (last && !steady) {
-                    stamp = getRuntime();
-                    drive.updatePoseEstimate();
-                    teleStart = drive.localizer.getPose();
-                    return false;
-                } else if (steady) {
-                    stamp = getRuntime();
-                    return true;
-                } else {
-                    return true;
-                }
-            }
-        };
-    }
-
     public Action Obelisk() {
         return new Action() {
             int id = 0;
@@ -134,7 +104,11 @@ public class Red_V3 extends LinearOpMode {
                     double turretPID = servo.setTurrPos(turret_red);
                     robot.turr1.setPower(turretPID);
                     robot.turr2.setPower(-turretPID);
-                    robot.limelight.pipelineSwitch(3);
+                    if (redAlliance){
+                        robot.limelight.pipelineSwitch(3);
+                    } else {
+                        robot.limelight.pipelineSwitch(2);
+                    }
                     return !servo.turretEqual(turret_red);
                 } else {
                     return true;
@@ -148,11 +122,11 @@ public class Red_V3 extends LinearOpMode {
             double spinPID = 0.0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-
                 powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
+
                 spinPID = servo.setSpinPos(spindexer);
                 robot.spin1.setPower(spinPID);
                 robot.spin2.setPower(-spinPID);
@@ -163,7 +137,13 @@ public class Red_V3 extends LinearOpMode {
                 drive.updatePoseEstimate();
                 teleStart = drive.localizer.getPose();
 
-                return !servo.spinEqual(spindexer);
+                if (servo.spinEqual(spindexer)){
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
+                    return false;
+                } else {
+                    return true;
+                }
             }
         };
     }
@@ -201,9 +181,10 @@ public class Red_V3 extends LinearOpMode {
                     TELE.update();
                     transferIn = true;
                     return true;
-                } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut &&
+                } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) {
-                           transferIn) {
                     robot.transferServo.setPosition(transferServo_out);
+                    robot.turr1.setPower(holdTurrPow);
+                    robot.turr2.setPower(holdTurrPow);
                     TELE.addData("Velocity", velo);
                     TELE.addLine("shot once");
                     TELE.update();
@@ -218,9 +199,10 @@ public class Red_V3 extends LinearOpMode {
 
     public Action intake(double intakeTime) {
         return new Action() {
-            double position = 0.0;
+            double position = spindexer_intakePos1;
             double stamp = 0.0;
             int ticker = 0;
+            double pow = 1.0;
 
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -229,17 +211,38 @@ public class Red_V3 extends LinearOpMode {
                 }
                 ticker++;
 
+                robot.intake.setPower(pow);
+
                 double s1D = robot.color1.getDistance(DistanceUnit.MM);
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
-                if ((getRuntime() % 0.3) > 0.15) {
+                if (!servo.spinEqual(position)){
-                    position = spindexer_intakePos1 + 0.02;
+                    double spinPID = servo.setSpinPos(position);
-                } else {
+                    robot.spin1.setPower(spinPID);
-                    position = spindexer_intakePos1 - 0.02;
+                    robot.spin2.setPower(-spinPID);
+                }
+
+                if (s1D < 43 && servo.spinEqual(position) && getRuntime() - stamp > 0.5){
+                    if (s2D > 60){
+                        if (servo.spinEqual(spindexer_intakePos1)){
+                            position = spindexer_intakePos2;
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                            position = spindexer_intakePos3;
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                            position = spindexer_intakePos1;
+                        }
+                    } else if (s3D > 33){
+                        if (servo.spinEqual(spindexer_intakePos1)){
+                            position = spindexer_intakePos3;
+                        } else if (servo.spinEqual(spindexer_intakePos2)){
+                            position = spindexer_intakePos1;
+                        } else if (servo.spinEqual(spindexer_intakePos3)){
+                            position = spindexer_intakePos2;
+                        }
+                    }
+                    stamp = getRuntime();
                 }
-                robot.spin1.setPower(position);
-                robot.spin2.setPower(1 - position);
 
                 TELE.addData("Velocity", velo);
                 TELE.addLine("Intaking");
@@ -250,7 +253,9 @@ public class Red_V3 extends LinearOpMode {
                 teleStart = drive.localizer.getPose();
 
                 robot.intake.setPower(1);
-                if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
+                if ((s1D < 43.0 && s2D < 60.0 && s3D < 33.0) || getRuntime() - stamp > intakeTime) {
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
                     return false;
                 } else {
                     return true;
@@ -259,11 +264,10 @@ public class Red_V3 extends LinearOpMode {
         };
     }
 
-    public Action intakeReject() {
+    public Action ColorDetect(int vel) {
         return new Action() {
             double stamp = 0.0;
             int ticker = 0;
-
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -271,36 +275,10 @@ public class Red_V3 extends LinearOpMode {
                 }
                 ticker++;
 
-                if (getRuntime() - stamp < 0.3) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                    return true;
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                } else {
+                robot.shooter1.setPower(powPID);
-                    robot.intake.setPower(0);
+                robot.shooter2.setPower(powPID);
-                    return false;
-                }
-            }
-        };
-    }
-
-    public Action ColorDetect() {
-        return new Action() {
-            double stamp = 0.0;
-            int ticker = 0;
-            double position = 0.0;
-
-            @Override
-            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
-                    stamp = getRuntime();
-                }
-                ticker++;
-
-                if ((getRuntime() % 0.3) > 0.15) {
-                    position = spindexer_intakePos1 + 0.02;
-                } else {
-                    position = spindexer_intakePos1 - 0.02;
-                }
-                robot.spin1.setPower(position);
-                robot.spin2.setPower(1 - position);
 
                 double s1D = robot.color1.getDistance(DistanceUnit.MM);
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -310,7 +288,7 @@ public class Red_V3 extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
-                if (s1D < 40) {
+                if (s1D < 43) {
 
                     double green = robot.color1.getNormalizedColors().green;
                     double red = robot.color1.getNormalizedColors().red;
@@ -325,7 +303,7 @@ public class Red_V3 extends LinearOpMode {
                     }
                 }
 
-                if (s2D < 40) {
+                if (s2D < 60) {
 
                     double green = robot.color2.getNormalizedColors().green;
                     double red = robot.color2.getNormalizedColors().red;
@@ -340,7 +318,7 @@ public class Red_V3 extends LinearOpMode {
                     }
                 }
 
-                if (s3D < 30) {
+                if (s3D < 33) {
 
                     double green = robot.color3.getNormalizedColors().green;
                     double red = robot.color3.getNormalizedColors().red;
@@ -389,23 +367,21 @@ public class Red_V3 extends LinearOpMode {
         robot.limelight.start();
 
         TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
-                .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+                .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
 
-        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
-                .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a)
+                .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
-                .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b);
+                .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
 
-        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b))
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
-                .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+                .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
 
-        TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
+        TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
+                .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
 
-                .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a)
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
-
+                .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
-                .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b);
-
-        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
-                .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
 
         while (opModeInInit()) {
 
@@ -417,15 +393,53 @@ public class Red_V3 extends LinearOpMode {
                 hoodAuto += 0.01;
             }
 
+            if (gamepad2.crossWasPressed()){
+                redAlliance = !redAlliance;
+            }
+
+            if (redAlliance){
+                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                        .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+
+                pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
+                        .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a)
+                        .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b);
+
+                shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b))
+                        .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+
+                pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1))
+                        .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a)
+                        .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b);
+
+                shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
+                        .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
+            } else {
+                shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                        .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+                pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                        .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
+                        .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
+
+                shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b))
+                        .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+
+                pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
+                        .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a)
+                        .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b);
+
+                shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b))
+                        .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
+            }
+
             robot.hood.setPosition(hoodAuto);
 
             robot.transferServo.setPosition(transferServo_out);
 
-            robot.spin1.setPower(spindexer_intakePos1);
-            robot.spin2.setPower(1 - spindexer_intakePos1);
-
             TELE.addData("Velocity", velo);
             TELE.addData("Turret Pos", servo.getTurrPos());
+            TELE.addData("Spin Pos", servo.getSpinPos());
             TELE.update();
         }
 
@@ -448,13 +462,12 @@ public class Red_V3 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+            robot.transfer.setPower(1);
-            velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-            robot.shooter1.setPower(powPID);
-            robot.shooter2.setPower(powPID);
 
             shootingSequence();
 
+            robot.transfer.setPower(0);
+
             robot.hood.setPosition(hoodAuto);
 
             drive.updatePoseEstimate();
@@ -474,9 +487,7 @@ public class Red_V3 extends LinearOpMode {
             Actions.runBlocking(
                     new ParallelAction(
                             shoot1.build(),
-                            ColorDetect(),
+                            ColorDetect(AUTO_CLOSE_VEL)
-                            steadyShooter(AUTO_CLOSE_VEL, true),
-                            intakeReject()
                     )
             );
 
@@ -484,12 +495,12 @@ public class Red_V3 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+            robot.transfer.setPower(1);
-            velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-            robot.shooter1.setPower(powPID);
-            robot.shooter2.setPower(powPID);
 
             shootingSequence();
+
+            robot.transfer.setPower(0);
+
             drive.updatePoseEstimate();
 
             teleStart = drive.localizer.getPose();
@@ -507,20 +518,16 @@ public class Red_V3 extends LinearOpMode {
             Actions.runBlocking(
                     new ParallelAction(
                             shoot2.build(),
-                            ColorDetect(),
+                            ColorDetect(AUTO_CLOSE_VEL)
-                            steadyShooter(AUTO_CLOSE_VEL, true),
-                            intakeReject()
-
                     )
             );
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
+            robot.transfer.setPower(1);
-            velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-            robot.shooter1.setPower(powPID);
-            robot.shooter2.setPower(powPID);
 
             shootingSequence();
 
+            robot.transfer.setPower(0);
+
             drive.updatePoseEstimate();
 
             teleStart = drive.localizer.getPose();
@@ -534,7 +541,7 @@ public class Red_V3 extends LinearOpMode {
         }
 
     }
-    //TODO: adjust this
+    //TODO: adjust this according to Teleop numbers
     public void detectTag() {
         LLResult result = robot.limelight.getLatestResult();
         if (result != null) {

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

@@ -12,20 +12,8 @@ public class Poses {
 
     public static double relativeGoalHeight = goalHeight - turretHeight;
 
-    public static double x1 = 50, y1 = 0, h1 = 0;
-
-    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
-
-    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
-
-    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
-
-    public static double tx = 0, ty = 0, th = 0;
-
     public static Pose2d goalPose = new Pose2d(-15, 0, 0);
 
-
-
     public static double rx1 = 45, ry1 = -7, rh1 = 0;
 
     public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java

@@ -205,7 +205,6 @@ public class IntakeTest extends LinearOpMode {
         }
     }
 
-
     boolean ballIn(int slot) {
         List<Double> times;