RED SIDE AUTO WORKDS COMPLETELY

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -1,7 +1,8 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
-import static org.firstinspires.ftc.teamcode.util.Configurables.ARMDOWN;
+import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE;
-import static org.firstinspires.ftc.teamcode.util.Configurables.ARMUP;
+import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE;
+import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST;
 import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN;
 import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE;
 import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK;
@@ -30,13 +31,20 @@ import org.firstinspires.ftc.teamcode.vision.Camera;
 import lombok.Getter;
 
 public class Robot {
-    @Getter private MecanumDrive drive;
+    @Getter
-    @Getter private Intake intake;
+    private MecanumDrive drive;
-    @Getter private Arm arm;
+    @Getter
-    @Getter private Wrist wrist;
+    private Intake intake;
-    @Getter private Claw claw;
+    @Getter
-    @Getter private Hang hang;
+    private Arm arm;
-    @Getter private Camera camera;
+    @Getter
+    private Wrist wrist;
+    @Getter
+    private Claw claw;
+    @Getter
+    private Hang hang;
+    @Getter
+    private Camera camera;
 
     public Robot init(HardwareMap hardwareMap) {
         this.drive = new MecanumDrive(hardwareMap);
@@ -49,7 +57,7 @@ public class Robot {
         return this;
     }
 
-    public static class Intake{
+    public static class Intake {
         private DcMotor intake = null;
 
         public Intake init(HardwareMap hardwareMap) {
@@ -59,11 +67,17 @@ public class Robot {
             return this;
         }
 
-        public void spinIn() {this.intake.setPower(0.7);}
+        public void spinIn() {
+            this.intake.setPower(0.7);
+        }
 
-        public void spinOut() {this.intake.setPower(-0.7);}
+        public void spinOut() {
+            this.intake.setPower(-0.7);
+        }
 
-        public void stop() {this.intake.setPower(0);}
+        public void stop() {
+            this.intake.setPower(0);
+        }
 
     }
 
@@ -95,12 +109,12 @@ public class Robot {
         private Servo rightArm;
 
 
-        public Arm init(HardwareMap hardwareMap){
+        public Arm init(HardwareMap hardwareMap) {
             this.leftArm = hardwareMap.get(Servo.class, LEFTARM);
             this.rightArm = hardwareMap.get(Servo.class, RIGHTARM);
             this.rightArm.setDirection(Servo.Direction.REVERSE);
-            this.rightArm.setPosition(ARMUP);
+            this.rightArm.setPosition(ARMREST);
-            this.leftArm.setPosition(ARMUP);
+            this.leftArm.setPosition(ARMREST);
             return this;
         }
 
@@ -110,20 +124,25 @@ public class Robot {
         }
 
         public void armScore() {
-            this.leftArm.setPosition(ARMDOWN);
+            this.leftArm.setPosition(ARMSCORE);
-            this.rightArm.setPosition(ARMDOWN);
+            this.rightArm.setPosition(ARMSCORE);
+        }
+
+        public void armAccurateScore() {
+            this.leftArm.setPosition(ARMACCSCORE);
+            this.rightArm.setPosition(ARMACCSCORE);
         }
 
         public void armRest() {
-            this.leftArm.setPosition(ARMUP);
+            this.leftArm.setPosition(ARMREST);
-            this.rightArm.setPosition(ARMUP);
+            this.rightArm.setPosition(ARMREST);
         }
     }
 
     public static class Wrist {
         private Servo wrist;
 
-        public Wrist init(HardwareMap hardwareMap){
+        public Wrist init(HardwareMap hardwareMap) {
             this.wrist = hardwareMap.get(Servo.class, WRIST);
             this.wrist.setPosition(WRISTPICKUP);
             return this;
@@ -141,8 +160,9 @@ public class Robot {
     public static class Claw {
         private Servo claw;
 
-        public Claw init (HardwareMap hardwareMap) {
+        public Claw init(HardwareMap hardwareMap) {
             this.claw = hardwareMap.get(Servo.class, CLAW);
+            this.claw.setPosition(CLOSE);
             return this;
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java

@@ -11,15 +11,32 @@ import org.firstinspires.ftc.teamcode.hardware.Robot;
 
 @Autonomous(name = "ThisIsTheLongestFlippingOpModeNameEverLolIamSeahorse!")
 public class Auto extends LinearOpMode {
+    protected Pose2d initialPosition;
+
+    protected Trajectory preloadOne;
+    protected Trajectory scoreOne;
+    protected Trajectory boardOne;
+    protected Trajectory backOffOne;
+
+
+    protected Trajectory preloadTwo;
+    protected Trajectory scoreTwo;
+    protected Trajectory backOffTwo;
+
+
+    protected Trajectory preloadThree;
+    protected Trajectory scoreThree;
+    protected Trajectory boardThree;
+    protected Trajectory backOffThree;
+
+
+    protected Trajectory park1;
+    protected Trajectory park2;
+
+
     private Robot robot;
     private String randomization;
     private int random;
-    protected Pose2d initialPosition;
-    protected Trajectory preloadOne;
-    protected Trajectory scoreOne;
-    protected Trajectory preloadTwo;
-    protected Trajectory preloadThree;
-
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -27,27 +44,68 @@ public class Auto extends LinearOpMode {
         this.robot = new Robot().init(hardwareMap);
         this.robot.getCamera().initTargetingCamera();
 
-    //Trajectories
+        //Trajectories
         this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
         this.robot.getDrive().setPoseEstimate(initialPosition);
 
-        //Preload One
+        //Randomization One
-            this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
+        this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                    .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
+                .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
-                    .build();
+                .build();
 
         this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
-                .lineToLinearHeading(new Pose2d(33,-30, Math.toRadians(360)))
+                .lineToLinearHeading(new Pose2d(33, -30, Math.toRadians(360)))
                 .build();
 
-        //Preload Two
+        this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
+                .lineToLinearHeading(new Pose2d(72, -26, Math.toRadians(360)))
+                .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
+                .addTemporalMarker(.2, robot.getWrist()::wristScore)
+                .build();
+
+        this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
+                .lineToLinearHeading(new Pose2d(60, -26, Math.toRadians(360)))
+                .build();
+
+
+        //Randomization Two
         this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(35,-27, Math.toRadians(270)))
+                .lineToLinearHeading(new Pose2d(35, -27, Math.toRadians(270)))
                 .build();
 
-        //Preload Three
+        this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
+                .lineToLinearHeading(new Pose2d(72, -34, Math.toRadians(360)))
+                .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
+                .addTemporalMarker(.2, robot.getWrist()::wristScore)
+                .build();
+
+        this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
+                .lineToLinearHeading(new Pose2d(60, -34, Math.toRadians(360)))
+                .build();
+
+        //Randomization Three
         this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(46,-35, Math.toRadians(270)))
+                .lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270)))
+                .build();
+
+        this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
+                .lineToLinearHeading(new Pose2d(72, -42, Math.toRadians(360)))
+                .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
+                .addTemporalMarker(.2, robot.getWrist()::wristScore)
+                .build();
+
+        this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
+                .lineToLinearHeading(new Pose2d(60, -40, Math.toRadians(360)))
+                .build();
+
+        //Park
+        this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
+                .lineToLinearHeading(new Pose2d(65, -10, Math.toRadians(360)))
+                .addTemporalMarker(.3, robot.getArm()::armRest)
+                .addTemporalMarker(.3, robot.getWrist()::wristPickup)
+                .build();
+        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
+                .lineToLinearHeading(new Pose2d(80, -10, Math.toRadians(360)))
                 .build();
 
         // Do super fancy chinese shit
@@ -61,17 +119,37 @@ public class Auto extends LinearOpMode {
             case "LEFT":
                 this.robot.getDrive().followTrajectory(preloadOne);
                 this.robot.getDrive().followTrajectory(scoreOne);
+                this.robot.getDrive().followTrajectory(boardOne);
+                this.robot.getClaw().open();
+                sleep(500);
+                this.robot.getDrive().followTrajectory(backOffOne);
+                sleep(500);
+                this.robot.getDrive().followTrajectory(park1);
+                this.robot.getDrive().followTrajectory(park2);
                 break;
             case "CENTER":
                 this.robot.getDrive().followTrajectory(preloadTwo);
+                this.robot.getDrive().followTrajectory(scoreTwo);
+                this.robot.getClaw().open();
+                sleep(500);
+                this.robot.getDrive().followTrajectory(backOffTwo);
+                sleep(500);
+                this.robot.getDrive().followTrajectory(park1);
+                this.robot.getDrive().followTrajectory(park2);
                 break;
             case "RIGHT":
                 this.robot.getDrive().followTrajectory(preloadThree);
+                this.robot.getDrive().followTrajectory(scoreThree);
+                this.robot.getClaw().open();
+                sleep(500);
+                this.robot.getDrive().followTrajectory(backOffThree);
+                sleep(500);
+                this.robot.getDrive().followTrajectory(park1);
+                this.robot.getDrive().followTrajectory(park2);
                 break;
         }
 
 
-
-        }
-
     }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java

@@ -21,6 +21,7 @@ public class MainTeleOp extends OpMode {
         boolean restArm = gamepad2.dpad_left || gamepad2.x;
         boolean pickupArm = gamepad2.dpad_down;
         boolean scoreArm = gamepad2.dpad_right || gamepad2.a;
+        boolean accurateScoreArm = gamepad2.y;
         boolean claw = gamepad2.b;
         boolean pickupWrist = gamepad2.left_bumper || gamepad2.x;
         boolean scoreWrist = gamepad2.right_bumper || gamepad2.a;
@@ -37,6 +38,8 @@ public class MainTeleOp extends OpMode {
             this.robot.getArm().armRest();
         } else if (scoreArm) {
             this.robot.getArm().armScore();
+        } else if (accurateScoreArm) {
+            this.robot.getArm().armAccurateScore();
         }
 //Claw
         if (claw) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java

@@ -18,8 +18,9 @@ public class Configurables {
     public static double LOCKSPEED = .25;
     public static double UNLOCK = .6;
     public static double LOCK = 0.4;
-    public static double ARMUP = .88;
+    public static double ARMREST = .88;
-    public static double ARMDOWN = 0.15;
+    public static double ARMSCORE = 0.15;
+    public static double ARMACCSCORE = 0.02;
     public static double PICKUP = .935;
     public static double WRISTPICKUP = 0.28;
     public static double WRISTSCORE = .96;