Before LM1

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

@@ -22,6 +22,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
 import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
 import org.firstinspires.ftc.teamcode.subsystems.Transfer;
@@ -77,12 +78,14 @@ public class Red extends LinearOpMode {
 
         shooter.setShooterMode("MANUAL");
 
-        shooter.sethoodPosition(0.54);
+        shooter.sethoodPosition(0.53);
 
         transfer = new Transfer(robot);
 
         transfer.setTransferPosition(transferServo_out);
 
+        Intake intake = new Intake(robot);
+
 
 
 
@@ -94,11 +97,36 @@ public class Red extends LinearOpMode {
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
 
+        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .turnTo(Math.toRadians(135))
+                .strafeToLinearHeading(new Vector2d(x2, y2), h2 );
+
+
+        TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+
+                .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b )
+
+                .strafeToLinearHeading(new Vector2d(x3, y3), h3 );
+
+
+        TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
+
+
+        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .strafeToLinearHeading(new Vector2d(x1, y1-10), h1 );
+
         while(opModeInInit()) {
 
+            shooter.setTurretPosition(0.33);
+
             aprilTag.initTelemetry();
 
             aprilTag.update();
+            shooter.update();
 
             TELE.update();
         }
@@ -123,7 +151,6 @@ public class Red extends LinearOpMode {
                     )
             );
 
-            shooter.moveTurret(angle);
 
             transfer.setTransferPower(1);
 
@@ -133,83 +160,251 @@ public class Red extends LinearOpMode {
 
             double stamp = getRuntime();
 
-            boolean gpp =false;
 
-            boolean pgp = false;
+             stamp = getRuntime();
-            boolean ppg = false;
 
-            aprilTag.turnTelemetryOn(true);
+            while (getRuntime()-stamp<4.5) {
-
-            while(getRuntime()-stamp <4.5) {
-
-                aprilTag.update();
 
 
-                gpp = aprilTag.isDetected(21);
 
-                 pgp = aprilTag.isDetected(22);
+                shooter.moveTurret(0.31);
 
-                 ppg = aprilTag.isDetected(23);
+                double time = getRuntime()-stamp;
 
-                 TELE.update();
-            }
 
-            robot.turr1.setPosition(0.9);
-
-            robot.turr2.setPosition(0.1);
-
-            int ticker = 0;
-
-            if (gpp){
                 if (time < 0.3) {
 
-                    ticker = 0;
-
 
                     transfer.transferOut();
                     transfer.setTransferPower(1);
-                } else if (time < 2) {
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
 
                     spindexer.outtake2();
 
 
 
-                } else if (time < 2.5) {
+                } else if (time < 3.0) {
-
-
-
-                    transfer.transferIn();
-                } else if (time < 4) {
-                    transfer.transferOut();
-
-                    spindexer.outtake1();
-
-
-
-                    ticker++;
-                } else if (time < 4.5) {
 
 
                     transfer.transferIn();
-                } else if (time < 6) {
+                } else if (time < 4.0) {
 
                     transfer.transferOut();
 
                     spindexer.outtake1();
 
-                } else if (time < 6.5) {
+                } else if (time < 4.4) {
                     transfer.transferIn();
                 } else {
 
                     transfer.transferOut();
+                    spindexer.outtake3();
 
                     shooter.setManualPower(0);
 
                 }
 
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
             }
 
+            spindexer.outtake3();
 
+            robot.intake.setPower(1);
+
+            Actions.runBlocking(
+                    traj2.build()
+            );
+
+
+            Actions.runBlocking(
+                    traj3.build()
+            );
+
+            shooter.setManualPower(1);
+
+
+
+
+            robot.intake.setPower(0);
+
+            spindexer.outtake3();
+
+
+            stamp = getRuntime();
+
+            shooter.setManualPower(1);
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.31);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(0);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+            robot.intake.setPower(1);
+
+            Actions.runBlocking(
+                    traj4.build()
+            );
+
+
+            Actions.runBlocking(
+                    traj5.build()
+            );
+
+            shooter.setManualPower(1);
+
+
+
+
+            robot.intake.setPower(0);
+
+
+            stamp = getRuntime();
+
+            shooter.setManualPower(1);
+
+            while (getRuntime()-stamp<4.5) {
+
+
+
+                shooter.moveTurret(0.31);
+
+                double time = getRuntime()-stamp;
+
+
+                if (time < 0.3) {
+
+
+                    transfer.transferOut();
+                    transfer.setTransferPower(1);
+                } else if (time < 1) {
+
+                    spindexer.outtake3();
+
+
+                } else if (time < 1.4) {
+
+
+                    transfer.transferIn();
+                } else if (time < 2.6) {
+                    transfer.transferOut();
+
+                    spindexer.outtake2();
+
+
+
+                } else if (time < 3.0) {
+
+
+                    transfer.transferIn();
+                } else if (time < 4.0) {
+
+                    transfer.transferOut();
+
+                    spindexer.outtake1();
+
+                } else if (time < 4.4) {
+                    transfer.transferIn();
+                } else {
+
+                    transfer.transferOut();
+                    spindexer.outtake3();
+
+                    shooter.setManualPower(0);
+
+                }
+
+                transfer.update();
+
+                shooter.update();
+
+                spindexer.update();
+
+            }
+
+            spindexer.outtake3();
+
+            Actions.runBlocking(
+                    traj6.build()
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addLine("finished");
+
+            TELE.update();
+
+            sleep (2000);
 
 
         }

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

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.constants;
 
 
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
 
 @Config
 public class Poses {
@@ -14,6 +15,15 @@ public class Poses {
 
     public static double x1 = 50, y1 = 0, h1 = 0;
 
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
+
+    public static double x2_b = 75, y2_b = 25, h2_b = Math.toRadians(135);
+
+
+    public static double x3 = 45, y3 = 53, h3 = Math.toRadians(135);
+
+    public static Pose2d teleStart = new Pose2d(x1,-10,0);
+
 
 
 }

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

@@ -73,9 +73,9 @@ public final class MecanumDrive {
         public double kA = 0.000046;
 
         // path profile parameters (in inches)
-        public double maxWheelVel = 80;
+        public double maxWheelVel = 120;
         public double minProfileAccel = -30;
-        public double maxProfileAccel = 80;
+        public double maxProfileAccel = 120;
 
         // turn profile parameters (in radians)
         public double maxAngVel = 2* Math.PI; // shared with path

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -118,7 +120,7 @@ public class TeleopV1 extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
 
-        drive  = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
+        drive  = new MecanumDrive(hardwareMap, teleStart);
 
 
 
@@ -216,7 +218,7 @@ public class TeleopV1 extends LinearOpMode {
 
         if (isStopRequested()) return;
 
-        drive  = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
+        drive  = new MecanumDrive(hardwareMap, teleStart);
 
 
         while(opModeIsActive()){

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

@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.teleop;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+
+
+@TeleOp
+@Config
+public class WebcamTest extends LinearOpMode {
+
+    AprilTag webcam;
+
+    MultipleTelemetry TELE;
+
+    Robot robot;
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        webcam = new AprilTag(robot, TELE);
+
+        webcam.turnTelemetryOn(true);
+
+
+
+
+        while(opModeInInit()){
+
+            webcam.initTelemetry();
+
+            TELE.update();
+
+        };
+
+        if(isStopRequested()) return;
+
+
+        while (opModeIsActive()){
+
+            webcam.update();
+
+            TELE.update();
+
+        }
+
+
+    }
+}