2+1(blue works not red)

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java

@@ -90,13 +90,42 @@ public class MeepMeepTesting {
                                 .lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
                                 .build()
                 );
+
+        RoadRunnerBotEntity RedFrontStage = new DefaultBotBuilder(meepMeep)
+                .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
+                .followTrajectorySequence(drive ->
+                        drive.trajectorySequenceBuilder(new Pose2d(-36, -61.5, Math.toRadians(90)))
+                                //Score then pick up 1 white
+                                .lineToLinearHeading(new Pose2d(-39.7, -33.5, Math.toRadians(90)))
+                                .lineToLinearHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,1.5))).waitSeconds(.01)
+
+                                //Spline to board
+                                .setTangent(Math.toRadians(-90))
+                                .splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
+
+                                //Go back to white stack
+                                .splineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
+                                .lineToConstantHeading(new Pose2d(-45, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
+                                .splineToConstantHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
+
+                                //Go back to board
+                                .setTangent(Math.toRadians(-90))
+                                .splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
+                                .lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
+                                .splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
+                                .build()
+                );
+
         meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
                 .setDarkMode(true)
                 .setBackgroundAlpha(0.95f)
                 //.addEntity(myBot)
                 //.addEntity(BlueFrontStage)
                 //.addEntity(BlueFrontStage3)
-                .addEntity(BlueFrontStage1)
+                //.addEntity(BlueFrontStage1)
+                .addEntity(RedFrontStage)
                 .start();
     }
 }

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

@@ -49,7 +49,7 @@ public class Arm {
     public static double wristScore_highMacro = 0.3;
 
     public enum Position {
-        INTAKE, SCORE, SCOREHI
+        INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT
     }
 
     public enum DoorPosition {
@@ -61,7 +61,7 @@ public class Arm {
         doorServo = hardwareMap.get(Servo.class, "Door");
         lAServo = hardwareMap.get(Servo.class, "LeftArm");
         rAServo = hardwareMap.get(Servo.class, "RightArm");
-       // elbow = hardwareMap.get(Servo.class, "Elbow");
+        elbow = hardwareMap.get(Servo.class, "Elbow");
 //        lAServo.setDirection(Servo.Direction.REVERSE);
         rAServo.setDirection(Servo.Direction.REVERSE);
         doorServo.setDirection(Servo.Direction.REVERSE);
@@ -106,11 +106,13 @@ public class Arm {
 
     public void setElbowPos (Position pos){
         if (pos == Position.INTAKE) {
-            wristPos = elbow_mid;
+            elbowPos = elbow_mid;
         } else if (pos == Position.SCORE) {
-            wristPos = elbow_right;
+            wristPos = elbow_mid;
-        } else if (pos == Position.SCOREHI) {
+        } else if (pos == Position.SCORELEFT) {
             wristPos = elbow_left;
+        } else if (pos == Position.SCORERIGHT) {
+            wristPos = elbow_right;
 
         }
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java

@@ -43,7 +43,7 @@ public abstract class AutoBase extends LinearOpMode {
                 telemetry.addLine("Initialized");
                 // Detection vndafds = robot.camera.getProp() <- returns a detection
                 // int fdsf = detection.getCenter().x <- x value on the screen between -50,50
-                double PropDetection = robot.camera.getProp().getCenter().x;
+                  double PropDetection = robot.camera.getProp().getCenter().x;
 
                 if (PropDetection <= -DetectionTest ) {
                     teamPropLocation = 1;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java

@@ -125,11 +125,11 @@ public class BlueFrontPreload extends AutoBase {
                         case (2):
                             //team prop location 2
                             builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
-
+                            break;
                         case (3):
                             //team prop location 3
                             builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
-
+                            break;
                     }
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java

@@ -11,6 +11,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
 
@@ -19,13 +20,13 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
 import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 @Config
-@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
+@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Red Teleop")
 public class RedFrontPreload extends AutoBase {
     public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
 
     public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
     public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
-    public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
+    public static final Pose2d DROP_2_PART_2 = new Pose2d(-37.7,-42, Math.toRadians(90));
     public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
     public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
 
@@ -35,13 +36,13 @@ public class RedFrontPreload extends AutoBase {
 
     public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
 
-    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -32, Math.toRadians(187));//187
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(47, -32, Math.toRadians(187));//187
 
     public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
 
     public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
 
-    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-55, -40, Math.toRadians(180));
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
 
     public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
 
@@ -51,7 +52,7 @@ public class RedFrontPreload extends AutoBase {
 
     public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0));
 
-    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(12, -58.5, Math.toRadians(0));
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(0));
 
     public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
 
@@ -124,11 +125,11 @@ public class RedFrontPreload extends AutoBase {
                         case (2):
                             //team prop location 2
                             builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01);
-
+                            break;
                         case (3):
                             //team prop location 3
                             builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01);
-
+                            break;
                     }
 
 
@@ -165,7 +166,7 @@ public class RedFrontPreload extends AutoBase {
                             builder.setTangent(Math.toRadians(-90));
                             builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
                             builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
-                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180));
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec().plus(new Vector2d((3.8))),Math.toRadians(180));
                             break;
                         case 3:
                             builder.setTangent(Math.toRadians(-90));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java

@@ -0,0 +1,304 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
+@Config
+@Autonomous(name = "Red FrontPreload (2+2)", group = "Competition", preselectTeleOp = "Red Teleop")
+public class RedFrontStageme extends AutoBase {
+    public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
+
+    public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
+    public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
+
+    public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
+    public static final Pose2d DROP_2_PART_3 = new Pose2d(-40.7,-42, Math.toRadians(180));
+    public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
+    public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
+
+    public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
+
+    public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -32, Math.toRadians(187));//187
+
+    public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
+
+    public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
+
+    public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
+
+    public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
+
+    public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
+
+    public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(180));
+
+    public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(180));
+
+    public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
+
+    public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180));
+
+    @Override
+    public void createTrajectories() {
+        // set start position
+        Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
+        robot.drive.setPoseEstimate(start);
+        robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
+    }
+
+    @Override
+    public void followTrajectories() {
+        TrajectorySequenceBuilder builder = null;
+        switch (macroState) {
+            case 0:
+                builder = this.robot.getTrajectorySequenceBuilder();
+                switch (teamPropLocation) {
+                    case 1:
+                        builder.lineToLinearHeading(DROP_1M);
+                        builder.lineToLinearHeading(DROP_1);
+                        break;
+                    case 2:
+                        builder.lineToLinearHeading(DROP_2);
+                        break;
+                    case 3:
+                        builder.lineToLinearHeading(DROP_3M);
+//                        builder.lineToLinearHeading(DROP_3);
+                        break;
+                }
+                this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                macroState++;
+                break;
+            // DRIVE TO TAPE
+            case 1:
+
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+            // RUN INTAKE
+            case 2:
+                // intake
+                if (getRuntime() < macroTime + 0.1) {
+                }
+                else{
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    robot.intake.setDcMotor(0);
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.lineToLinearHeading(DROP_1_PART_2);
+                            break;
+                        case 2:
+                            builder.lineToLinearHeading(DROP_2_PART_2);
+                            builder.lineToLinearHeading(DROP_2_PART_3);
+                            break;
+                        case 3:
+                            builder.lineToLinearHeading(DROP_3);
+                            break;
+                    }
+                    //robot.arm.setDoor(OPEN);
+                    macroTime = getRuntime();
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                // if intake is done move on
+                break;
+            case 3:
+                // if drive is done move on
+                if (!robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    macroState++;
+                }
+                break;
+
+            case 4:
+                if (getRuntime() > macroTime + 0.1) {
+                    //robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.1);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+            case 5:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+            //extending the macro and about to score
+            case 6:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            case 7:
+                robot.resetMacro(0,getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
+                    builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
+                    builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            case 8:
+                robot.arm.setDoor(OPEN);
+                robot.intake.setDcMotor(0.44);
+                robot.intake.setpos(STACK5);
+                macroTime = getRuntime();
+                macroState++;
+                break;
+
+            case 9:
+                if (getRuntime() > macroTime + 0.06) {
+                    robot.arm.setDoor(CLOSE);
+                    robot.intake.setDcMotor(-0.5);
+                    builder = this.robot.getTrajectorySequenceBuilder();
+
+                    switch (teamPropLocation) {
+                        case 1:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
+                            break;
+                        case 2:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                        case 3:
+                            builder.setTangent(Math.toRadians(-90));
+                            builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
+                            builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
+                            builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
+                            break;
+                    }
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                    macroTime = getRuntime();
+                }
+                break;
+
+            case 10:
+                // if drive is done move on
+                if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
+                    macroTime = getRuntime();
+                    robot.macroState = 0;
+                    robot.extendMacro(Slides.mini_tier1-30, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            case 11:
+                if (robot.macroState != 0) {
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
+                }
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+            case 12:
+                robot.resetMacro(0, getRuntime());
+                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+                    builder = this.robot.getTrajectorySequenceBuilder();
+                    builder.lineToLinearHeading(PARK_1);
+                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+                    macroState++;
+                }
+                break;
+
+            //Park
+
+//            case 7:
+//                robot.resetMacro(0, getRuntime());
+//                if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
+//                    builder = this.robot.getTrajectorySequenceBuilder();
+//                    builder.lineToLinearHeading(PARK_1);
+//                    this.robot.drive.followTrajectorySequenceAsync(builder.build());
+//                    macroState++;
+//                }
+//                break;
+//
+//            case 8:
+//                // if drive is done move on
+//                if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
+//                    macroState++;
+//                }
+//                break;
+//
+//            case 9:
+//                robot.resetMacro(0, getRuntime());
+//                if (robot.macroState == 0) {
+//                    macroState = -1;
+//                }
+
+            case 19:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState == 0) {
+                    macroState = -1;
+                }
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java

@@ -179,6 +179,9 @@ public class BlueTeleop extends OpMode {
         }
 
 
+        //Elbowpos
+
+