tweaks all around, deleted a lot of unused files, started a cycle auto

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

@@ -5,79 +5,102 @@ import com.arcrobotics.ftclib.controller.PController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
-import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
-
 @Config
 public class Arm {
-
     //make each servo
-    private Servo dServo;
     private Servo rAServo;
     private Servo lAServo;
+    private Servo doorServo;
     private Servo wristServo;
     private Slides.Position pos = Slides.Position.DOWN;
     private PController armController;
     private double armControllerTarget;
-    public static double ARM_KP = 0.01;
 
+    public static double armTolerance = 0.03;
+    public static double armSpeed = 0.022;
+    private double armPos;
+    private double armTempPos;
+    private double doorPos;
+    private double wristPos;
+    public static double ARM_KP = 0.08;
+
+    public static double doorOpenPos = 0.09;
+    public static double doorClosePos = 0.26;
+
+    public static double armStart = 0.24;
+    public static double armScore = 0.95;
+    public static double wristStart = 0.29;
+    public static double wristScore = 0.5;
 
     public enum Position {
-        INTAKE, SCORE;
+        INTAKE, SCORE
+    }
+
+    public enum DoorPosition {
+        OPEN, CLOSE
     }
 
     public Arm(HardwareMap hardwareMap) {
         wristServo = hardwareMap.get(Servo.class, "Wrist Servo");
-        dServo = hardwareMap.get(Servo.class, "Door Servo");
+        doorServo = hardwareMap.get(Servo.class, "Door Servo");
         lAServo = hardwareMap.get(Servo.class, "Left Arm Servo");
         rAServo = hardwareMap.get(Servo.class, "Right Arm Servo ");
 //        lAServo.setDirection(Servo.Direction.REVERSE);
         rAServo.setDirection(Servo.Direction.REVERSE);
-        dServo.setDirection(Servo.Direction.REVERSE);
+        doorServo.setDirection(Servo.Direction.REVERSE);
 //        wristServo.setDirection(Servo.Direction.REVERSE);
 
         this.armController = new PController(ARM_KP);
 
         setArmPos(Position.INTAKE);
+        lAServo.setPosition(armStart);
+        rAServo.setPosition(armStart);
+//        armTempPos = armPos;
+
         setWristPos(Position.INTAKE);
+        setDoor(DoorPosition.CLOSE);
     }
 
-    public static double doorOpenpos = 0.1;
-    public static double doorClosePos = 0.85;
-
-    public static double armStart = 0.15;
-    public static double armScore = 1;
-    public static double wristStart = 0.93;
-    public static double wristScore = 0.98;
-
     public void setArmPos(Position pos) {
         this.armControllerTarget = pos == Position.INTAKE
                 ? armStart
                 : armScore;
+//        if (pos == Position.INTAKE) {
+//            armPos = armStart;
+//        } else if (pos == Position.SCORE) {
+//            armPos = armScore;
+//        }
+    }
+    public boolean armAtPosition() {
+        return armController.atSetPoint();
+//        return armTempPos == armPos;
     }
 
     public void setWristPos(Position pos) {
         if (pos == Position.INTAKE) {
-            wristServo.setPosition(wristStart);
+            wristPos = wristStart;
         } else if (pos == Position.SCORE) {
-            wristServo.setPosition(wristScore);
+            wristPos = wristScore;
         }
     }
 
-    public void setDoor(DoorPos.DoorPosition pos) {
+    public void setDoor(DoorPosition pos) {
-        if (pos == DoorPos.DoorPosition.OPEN) {
+        if (pos == DoorPosition.OPEN) {
-            dServo.setPosition(doorOpenpos);
+            doorPos = doorOpenPos;
-        } else if (pos == DoorPos.DoorPosition.CLOSE) {
+        } else if (pos == DoorPosition.CLOSE) {
-            dServo.setPosition(doorClosePos);
+            doorPos = doorClosePos;
         }
     }
 
     public String getTelemetry() {
-        return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition();
+        return String.format("Wrist: %s\nDoor: %s\nLeft Arm: %s\nRight Arm: %s\nError: %s",
+                wristServo.getPosition(), doorServo.getPosition(), lAServo.getPosition(), rAServo.getPosition(), armController.getPositionError());
+        //return "Wrist Servo: "+wristServo.getPosition()+"\nLeft Arm Servo: "+lAServo.getPosition()+"\nRight Arm Servo: "+rAServo.getPosition()+"\nDoor Servo: "+ doorServo.getPosition();
     }
 
     public void update() {
         this.armController.setSetPoint(this.armControllerTarget);
-        this.armController.setTolerance(0.001);
+        this.armController.setTolerance(armTolerance);
         this.armController.setP(ARM_KP);
 
         double output = 0;
@@ -86,6 +109,23 @@ public class Arm {
 
             this.lAServo.setPosition(this.lAServo.getPosition() + output);
             this.rAServo.setPosition(this.lAServo.getPosition() + output);
+        } else {
+            lAServo.setPosition(armControllerTarget);
+            rAServo.setPosition(armControllerTarget);
         }
+//        if (Math.abs(armTempPos-armPos) <= armTolerance) {
+//            armTempPos = armPos;
+//        } else {
+//            if (armTempPos < armPos) {
+//                armTempPos += armSpeed;
+//            } else if (armTempPos > armPos) {
+//                armTempPos -= armSpeed;
+//            }
+//        }
+//        lAServo.setPosition(armTempPos);
+//        rAServo.setPosition(armTempPos);
+
+        doorServo.setPosition(doorPos);
+        wristServo.setPosition(wristPos);
     }
 }

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

@@ -1,8 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-public class ArmPos {
-
-    public enum APosition {
-        SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP;
-    }
-}

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

@@ -17,20 +17,22 @@ import org.openftc.apriltag.AprilTagDetection;
 import org.openftc.easyopencv.OpenCvCamera;
 import org.openftc.easyopencv.OpenCvCameraFactory;
 
+import com.acmerobotics.dashboard.FtcDashboard;
+
 import java.util.ArrayList;
 
 // Class for the camera
 public class Camera {
+    private HardwareMap hardwareMap;
+    private OpenCvCamera targetingCamera;
+    private TargetingPipeline targetingPipeline;
+    private boolean targetingCameraInitialized;
 
     private float decimation;
     private boolean needToSetDecimation;
     int numFramesWithoutDetection = 0;
     private boolean signalWebcamInitialized;
     private OpenCvCamera signalWebcam;
-    private HardwareMap hardwareMap;
-    private OpenCvCamera targetingCamera;
-    private TargetingPipeline targetingPipeline;
-    private boolean targetingCameraInitialized;
     AprilTagDetectionPipeline aprilTagDetectionPipeline;
     ArrayList<AprilTagDetection> detections;
     static final double FEET_PER_METER = 3.28084;
@@ -57,6 +59,7 @@ public class Camera {
             public void onOpened() {
                 targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION);
                 targetingCameraInitialized = true;
+                FtcDashboard.getInstance().startCameraStream(signalWebcam, 0);
             }
 
             @Override
@@ -85,8 +88,8 @@ public class Camera {
 
     //return frame rate
     public int getFrameCount() {
-        if (signalWebcamInitialized) {
+        if (targetingCameraInitialized) {
-            return signalWebcam.getFrameCount();
+            return targetingCamera.getFrameCount();
         } else {
             return 0;
         }

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

@@ -1,7 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-public class DoorPos {
-    public enum DoorPosition {
-        OPEN, CLOSE
-    }
-}

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

@@ -1,7 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-public class Down {
-    public enum DownArm {
-        YES, NO
-    }
-}

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

@@ -11,7 +11,7 @@ public class DroneLauncher {
     public static double launchPos = 0.5;
 
     public DroneLauncher(HardwareMap hardwareMap) {
-        this.servo = hardwareMap.get(Servo.class, Constants.droneLauncher);
+        this.servo = hardwareMap.get(Servo.class, "Drone Launcher");
         this.servo.setPosition(initPos);
     }
 

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

@@ -1,7 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-public class Height {
-    public enum height {
-        ASCEND, HOLD, DESCEND, RESET
-    }
-}

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

@@ -1,7 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-public class HopperPos {
-    public enum hopperPos {
-        UP, DOWN
-    }
-}

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

@@ -1,5 +1,8 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -36,9 +39,9 @@ public class Intake {
     public static double motorPower = 0;
 
     public Intake(HardwareMap hardwareMap, Position up) {
-        lServo = hardwareMap.get(Servo.class, "Left Servo");
+        lServo = hardwareMap.get(Servo.class, "Left Intake Servo");
         lServo.setDirection(Servo.Direction.REVERSE);
-        rServo = hardwareMap.get(Servo.class, "Right Servo");
+        rServo = hardwareMap.get(Servo.class, "Right Intake Servo");
         dcMotor = hardwareMap.get(DcMotor.class, "Intake Motor");
     }
 

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

@@ -1,138 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-@Config
-public class KhangSlides {
-
-    //Create and assign motors
-    public KhangSlides(HardwareMap hardwareMap) {
-        rSMotor = hardwareMap.get(DcMotor.class, "Right Slide Motor");
-//        rSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-        rSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        rSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        lSMotor = hardwareMap.get(DcMotor.class, "Left Slide Motor");
-        lSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
-        lSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        lSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-    }
-
-    //make each motor
-    private DcMotor rSMotor;
-    private DcMotor lSMotor;
-
-    //Value for motor power and speed slides go at
-    public static double rSPower = 0.5;
-    public static double lSPower = 0.5;
-    public static double downpos = 0;
-    public static double tape1pos = 200;
-    public static double tape2pos = 350;
-    public static double tape3pos = 500;
-    public static double maxpos = 1000;
-
-    //Set default slide height to 0 aka starting position
-    public static double sHeight = 0;
-    public static double newSHeight = 0;
-
-//    Do stuff to get to a position when asked to go to a position
-    public void setSPos(SlidePos.SPosition height) {
-        if (height == SlidePos.SPosition.DOWN) {
-            sHeight = 0;
-            lSMotor.setTargetPosition((int) downpos);
-            lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            lSMotor.setPower(lSPower);
-            rSMotor.setTargetPosition((int) downpos);
-            rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            rSMotor.setPower(rSPower);
-        } else if (height == SlidePos.SPosition.TAPE1) {
-            sHeight = 300;
-            lSMotor.setTargetPosition((int) tape1pos);
-            lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            lSMotor.setPower(lSPower);
-            rSMotor.setTargetPosition((int) tape1pos);
-            rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            rSMotor.setPower(rSPower);
-        } else if (height == SlidePos.SPosition.TAPE2) {
-            sHeight = 500;
-            lSMotor.setTargetPosition((int) tape2pos);
-            lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            lSMotor.setPower(lSPower);
-            rSMotor.setTargetPosition((int) tape2pos);
-            rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            rSMotor.setPower(rSPower);
-        } else if (height == SlidePos.SPosition.TAPE3) {
-            sHeight = 700;
-            lSMotor.setTargetPosition((int) tape3pos);
-            lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            lSMotor.setPower(lSPower);
-            rSMotor.setTargetPosition((int) tape3pos);
-            rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            rSMotor.setPower(rSPower);
-        } else if (height == SlidePos.SPosition.MAX) {
-            sHeight = 1000;
-            lSMotor.setTargetPosition((int) maxpos);
-            lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            lSMotor.setPower(lSPower);
-            rSMotor.setTargetPosition((int) maxpos);
-            rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            rSMotor.setPower(rSPower);
-        }
-    }
-
-//    public void setHeight(Hieght.height send) {
-//        if (send == Hieght.height.ASCEND) {
-//            if (newSHeight <= 790) {
-//                newSHeight = newSHeight + 10;
-//                lSMotor.setTargetPosition((int) (newSHeight));
-//                lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                lSMotor.setPower(lSPower);
-//                rSMotor.setTargetPosition((int) (newSHeight));
-//                rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                rSMotor.setPower(rSPower);
-//            } else if (newSHeight >= 800) {
-//                newSHeight = 800;
-//                lSMotor.setTargetPosition((int) (newSHeight));
-//                lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                lSMotor.setPower(lSPower);
-//                rSMotor.setTargetPosition((int) (newSHeight));
-//                rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                rSMotor.setPower(rSPower);
-//            }
-//            } else if (send == Hieght.height.HOLD) {
-//                newSHeight = newSHeight + 0;
-//                lSMotor.setTargetPosition((int) (newSHeight));
-//                lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                lSMotor.setPower(lSPower);
-//                rSMotor.setTargetPosition((int) (newSHeight));
-//                rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                rSMotor.setPower(rSPower);
-//            } else if (send == Hieght.height.DESCEND) {
-//                if (newSHeight >= 10) {
-//                    newSHeight = newSHeight - 10;
-//                    lSMotor.setTargetPosition((int) (newSHeight));
-//                    lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                    lSMotor.setPower(lSPower);
-//                    rSMotor.setTargetPosition((int) (newSHeight));
-//                    rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                    rSMotor.setPower(rSPower);
-//                } else if (newSHeight <= 0) {
-//                    newSHeight = 0;
-//                    lSMotor.setTargetPosition((int) (newSHeight));
-//                    lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                    lSMotor.setPower(lSPower);
-//                    rSMotor.setTargetPosition((int) (newSHeight));
-//                    rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-//                    rSMotor.setPower(rSPower);
-//                }
-//            }
-//        }
-
-
-    //Return feed to the control hub for easy debug
-    public String getTelemetry() {
-        return "Left Slide Motor: "+lSMotor.getPower()+"Right Slide Motor: "+rSMotor.getPower();
-    }
-}

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

@@ -1,15 +1,13 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
 import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
-import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
-import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
 import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
 
 @Config
 public class Robot {
@@ -21,6 +19,7 @@ public class Robot {
     public Slides slides;
     public Arm arm;
     public DroneLauncher droneLauncher;
+
     public double macroStartTime = 0;
     public int macroState = 0;
     public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return
@@ -28,6 +27,10 @@ public class Robot {
 
     private boolean camEnabled = true;
 
+    public static double slideWait = 1.5;
+    public static double scoreWait = 1.25;
+    public static double armWait = 2.0;
+
     //Name the objects
     public Robot(HardwareMap hardwareMap) {
         drive = new SampleMecanumDrive(hardwareMap);
@@ -49,12 +52,11 @@ public class Robot {
                 macroState ++;
                 break;
             case(1):
-                if (runTime > macroStartTime + 1) {
+                if (runTime > macroStartTime + slideWait || slides.atTarget()) {
                     macroState ++;
                 }
                 break;
             case(2):
-                macroStartTime = runTime;
                 arm.setArmPos(Arm.Position.SCORE);
                 arm.setWristPos(Arm.Position.SCORE);
                 macroState = 0;
@@ -71,9 +73,8 @@ public class Robot {
                 arm.setDoor(OPEN);
                 macroState++;
                 break;
-                //Ind_sleeper
             case(1):
-                if (runTime > macroStartTime + 1) {
+                if (runTime > macroStartTime + scoreWait) {
                     macroState ++;
                 }
                 break;
@@ -84,12 +85,11 @@ public class Robot {
                 macroState++;
                 break;
             case(3):
-                if (runTime > macroStartTime + 1.1) {
+                if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
                     macroState ++;
                 }
                 break;
             case(4):
-                macroStartTime = runTime;
                 slides.setTarget(pos);
                 macroState = 0;
                 lastMacro = runningMacro;
@@ -104,6 +104,6 @@ public class Robot {
     }
 
     public String getTelemetry() {
-        return "Telemetry?";
+        return arm.getTelemetry();
     }
 }

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

@@ -1,8 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-public class SlidePos {
-
-    public enum SPosition {
-        DOWN, TAPE1, TAPE2, TAPE3, MAX, MANUAL;
-    }
-}

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

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.hardware.robby;
+package org.firstinspires.ftc.teamcode.hardware;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.arcrobotics.ftclib.controller.PIDController;
@@ -19,12 +19,12 @@ public class Slides {
     public static PIDController controller = new PIDController(p, i, d);
 
     public static int targetMin = -10;
-    public static int targetMax = 1000;
+    public static int targetMax = 555;
 
     public static int down = 0;
-    public static int tier1 = 200;
+    public static int tier1 = 350;
-    public static int tier2 = 350;
+    public static int tier2 = 450;
-    public static int tier3 = 500;
+    public static int tier3 = 550;
 
     private int target = 0;
 
@@ -36,13 +36,13 @@ public class Slides {
         slide = hardwareMap.get(DcMotor.class, "Right Slide Motor");
         slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-//        slide.setDirection(DcMotorSimple.Direction.REVERSE);
+        slide.setDirection(DcMotorSimple.Direction.REVERSE);
         slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         slide2 = hardwareMap.get(DcMotor.class, "Left Slide Motor");
         slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-        slide2.setDirection(DcMotorSimple.Direction.REVERSE);
+//        slide2.setDirection(DcMotorSimple.Direction.REVERSE);
         slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/PoseStorage.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.opmode.teleop;
+package org.firstinspires.ftc.teamcode.opmode;
 
 
 import com.acmerobotics.roadrunner.geometry.Pose2d;

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

@@ -3,8 +3,10 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous;
 import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.hardware.Arm;
+import org.firstinspires.ftc.teamcode.hardware.Intake;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
-import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
 import org.firstinspires.ftc.teamcode.util.CameraPosition;
 
 import java.util.ArrayList;
@@ -23,7 +25,7 @@ public abstract class AbstractAuto extends LinearOpMode {
     public void runOpMode() {
 //        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
-        telemetry.addLine("Initializing Robot...");
+        telemetry.addLine("Initializing...");
         telemetry.update();
 
         setCameraPosition();
@@ -32,9 +34,9 @@ public abstract class AbstractAuto extends LinearOpMode {
 
         makeTrajectories();
 
-//        while (robot.camera.getFrameCount() < 1) {
+        while (robot.camera.getFrameCount() < 1) {
-//            idle();
+            idle();
-//        }
+        }
 
         // wait for start
         while (!(isStarted() || isStopRequested())) {
@@ -222,6 +224,54 @@ public abstract class AbstractAuto extends LinearOpMode {
         });
     }
 
+    public void intakeStack(Intake.Position stackHeight1, Intake.Position stackHeight2) {
+        steps.add(new Step("Intaking Stack") {
+            @Override
+            public void start() {
+                robot.intake.setDcMotor(0.5);
+                robot.arm.setDoor(Arm.DoorPosition.OPEN);
+            }
+
+            @Override
+            public void whileRunning() {
+                switch(macroState) {
+                    case 0:
+                        macroStart = currentRuntime;
+                        robot.intake.setpos(stackHeight1);
+                        macroState++;
+                        break;
+                    case 1:
+                        if (currentRuntime > macroStart + 1.0) {
+                            macroState++;
+                        }
+                        break;
+                    case 2:
+                        robot.intake.setpos(stackHeight2);
+                        macroState++;
+                        break;
+                    case 3:
+                        if (currentRuntime > macroStart + 1.0) {
+                            macroState++;
+                        }
+                        break;
+                    case 4:
+                        macroState = 0;
+                }
+            }
+
+            @Override
+            public void end() {
+                robot.intake.setDcMotor(0);
+                robot.arm.setDoor(Arm.DoorPosition.CLOSE);
+            }
+
+            @Override
+            public boolean isFinished() {
+                return macroState == 0;
+            }
+        });
+    }
+
     public void runIntake(double power, double timeout) {
         steps.add(new Step("Running Intake" + timeout + " seconds", timeout) {
             @Override

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

@@ -4,7 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
-import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
 
 @Autonomous(name = "Blue Left Auto")
 public class BlueLeftAuto extends AbstractAuto {

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

@@ -0,0 +1,98 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.hardware.Intake;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
+import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
+
+@Autonomous(name = "Red Right Auto")
+public class RedRightAuto extends AbstractAuto {
+    public Trajectory scorePurple;
+    public Trajectory scoreYellow;
+    public Trajectory load1;
+    public Trajectory score1;
+    public Trajectory parkRobot;
+
+    @Override
+    public void makeTrajectories() {
+        Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
+
+        Pose2d dropMiddle = new Pose2d(12, -40.5, Math.toRadians(90));
+
+        Pose2d depositPreload = new Pose2d(46, -36, Math.toRadians(180));
+
+        Pose2d lineup1 = new Pose2d(12, -14, Math.toRadians(180));
+        Pose2d pickup1 = new Pose2d(-58, -14, Math.toRadians(180));
+
+        Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180));
+        Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180));
+
+        Pose2d park = new Pose2d(48, -12, Math.toRadians(180));
+
+        scorePurple = robot.drive.trajectoryBuilder(start)
+                .lineToLinearHeading(dropMiddle)
+                .build();
+
+        scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
+                .lineToLinearHeading(depositPreload)
+                .build();
+
+        load1 = robot.drive.trajectoryBuilder(scoreYellow.end())
+                .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
+                        SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .lineToLinearHeading(pickup1,
+                        SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .build();
+
+        score1 = robot.drive.trajectoryBuilder(load1.end(), true)
+                .lineToLinearHeading(driveup1,
+                        SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
+                        SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .build();
+
+        parkRobot = robot.drive.trajectoryBuilder(score1.end())
+                .lineToLinearHeading(park)
+                .build();
+
+        robot.drive.setPoseEstimate(start);
+    }
+
+    @Override
+    public void initializeSteps(int location) {
+        followTrajectory(scorePurple);
+        runIntake(-0.4, 0.5);
+//        followAndExtend(scoreYellow, Slides.Position.TIER1);
+//        followAndRetract(load1, Slides.Position.DOWN);
+        followTrajectory(scoreYellow);
+        followTrajectory(load1);
+        intakeStack(Intake.Position.STACK5, Intake.Position.STACK4);
+        followAndExtend(score1, Slides.Position.TIER1);
+        followAndRetract(parkRobot, Slides.Position.DOWN);
+    }
+
+    @Override
+    public void setAlliance() {
+    }
+
+    @Override
+    public void setCameraPosition() {
+    }
+
+    @Override
+    public boolean useCamera() {
+        return false;
+    }
+}

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

@@ -8,14 +8,9 @@ public abstract class Step {
     private final double timeout;
     private String telemetry;
 
-    // variables when moving
+    // macro variables
-    public double x;
+    public double macroStart;
-    public double y;
+    public int macroState;
-    public double stepTime;
-    double tempTime = stepTime;
-
-    // variables when shooting
-    public Detection teamProp;
 
     // Constructors
     public Step(String telemetry) {

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

@@ -1,304 +0,0 @@
-//package org.firstinspires.ftc.teamcode.opmode.teleop;
-//
-//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.BLUE;
-//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.RED;
-//import static java.lang.Math.PI;
-//
-//import com.acmerobotics.dashboard.config.Config;
-//import com.acmerobotics.roadrunner.geometry.Pose2d;
-//import com.arcrobotics.ftclib.controller.PIDController;
-//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-//
-//import org.firstinspires.ftc.teamcode.controller.Controller;
-//import org.firstinspires.ftc.teamcode.hardware.Arm;
-//import org.firstinspires.ftc.teamcode.hardware.Robot;
-//import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
-//
-//@Config
-//public abstract class AbstractTeleOp extends OpMode {
-//    private Robot robot;
-//    public Team team;
-//    Controller driver1;
-//    Controller driver2;
-//
-//    public static double normal = 0.5;
-//    public static double vazrooooming = 1.0;
-//    public static int heightIncrement = 20;
-//
-//    Pose2d robot_pos;
-//    double robot_x, robot_y, robot_heading;
-//
-//    // auto align variables
-//    double headingPID;
-//    public static double headingP = 0.01;
-//    public static double headingI = 0.03;
-//    public static double headingD = 0.0005;
-//    public static PIDController headingController = new PIDController(headingP, headingI, headingD);
-//
-//    double strafePID;
-//    public static double strafeP = 0.05;
-//    public static double strafeI = 0;
-//    public static double strafeD = 0.01;
-//    public static PIDController strafeController = new PIDController(strafeP, strafeI, strafeD);
-//
-//    double robot_y_pos;
-//    boolean fixed90Toggle = false;
-//    boolean fixed0Toggle = false;
-//
-////    public static double robot_width = 12;
-////    public static double robot_length = 12;
-////    public static double robot_radius = 6;
-////
-////    public static double groundJuncRadius = 6;
-////    public static double coneRadius = 4;
-////
-////    public static double armWait = 0.2;
-//
-////    private double timeSinceOpened = 0; // for claw
-////    private double timeSinceClosed = 0;
-//
-//    //    private int delayState = 0; // for arm
-////    private double delayStart = 0;
-////    private boolean doArmDelay = false; // for arm
-//    private boolean isAutoClose = true;
-//
-//    @Override
-//    public void init() {
-//        robot =  new Robot(hardwareMap);
-//        driver1 = new Controller(gamepad1);
-//        driver2 = new Controller(gamepad2);
-//        if (team == RED) {
-//            driver1.setColor(255, 0, 0);
-//            driver2.setColor(255, 0, 0);
-//        } else if (team == BLUE) {
-//            driver1.setColor(0, 0, 255);
-//            driver2.setColor(0, 0, 255);
-//        }
-//
-//        PoseStorage.AutoJustEnded = false;
-//    }
-//
-//    @Override
-//    public void init_loop() {
-//        ////init when camera is on and responding
-////        if (robot.camera.getFrameCount() < 1) {
-////            telemetry.addLine("Initializing Robot...");
-////        } else {
-////            telemetry.addLine("Initialized");
-////        }
-//        telemetry.addLine("Initialized");
-//        telemetry.addLine(robot.getTelemetry());
-//        telemetry.update();
-//    }
-//
-//    //find out what quarter you are in
-//    private int getQuadrant(double angle) {
-//        if (0 < angle && angle < PI/2.0) {
-//            return 1;
-//        } else if (PI/2.0 < angle && angle < PI) {
-//            return 2;
-//        } else if (PI < angle && angle < 3*PI/2.0) {
-//            return 3;
-//        } else {
-//            return 4;
-//        }
-//    }
-//
-//    @Override
-//    public void loop() {
-//        // robot position update
-//        robot_pos = robot.drive.getPoseEstimate();
-//        robot_x = robot_pos.getX();
-//        robot_y = robot_pos.getY();
-//        robot_heading = robot_pos.getHeading(); // in radians
-//
-//        // driver 1 controls
-//        driver1.update();
-//        driver2.update();
-//
-//        //create and set X, Y, and Z for drive inputs
-//        double x = -gamepad1.left_stick_y;
-//        double y = -gamepad1.left_stick_x;
-//        double z = -gamepad1.right_stick_x;
-//
-////         figure out if a toggle is present
-//        if (driver1.getRightBumper().isPressed()) {
-//            if (!fixed90Toggle) {
-//                robot_y_pos = robot.drive.getPoseEstimate().getX();
-//            }
-//            fixed90Toggle = true;
-//        } else {
-//            fixed90Toggle = false;
-//        }
-//
-//        if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) {
-//            if (!fixed0Toggle) {
-//                robot_y_pos = robot.drive.getPoseEstimate().getY();
-//            }
-//            fixed0Toggle = true;
-//        } else {
-//            fixed0Toggle = false;
-//        }
-//
-//        // heading pid
-//        headingController.setPID(headingP, headingI, headingD);
-//        int quadrant = getQuadrant(robot_heading);
-//        if (fixed0Toggle) {
-//            switch (quadrant) {
-//                case 1:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 0);//- right
-//                    break;
-//                case 2:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//+ left
-//                    break;
-//                case 3:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//- right
-//                    break;
-//                case 4:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 360);//+ left
-//                    break;
-//            }
-//        } else if (fixed90Toggle) {
-//            switch (quadrant) {
-//                case 1:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//- right
-//                    break;
-//                case 2:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//+ left
-//                    break;
-//                case 3:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//- right
-//                    break;
-//                case 4:
-//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//+ left
-//                    break;
-//            }
-//        } else {
-//            headingPID = 0;
-//        }
-//
-//        // strafe pid
-//        strafeController.setPID(strafeP, strafeI, strafeD);
-//        if (fixed0Toggle) {
-//            strafePID = strafeController.calculate(robot_y, robot_y_pos);
-//        } else if (fixed90Toggle) {
-//            strafePID = strafeController.calculate(robot_x, robot_y_pos);
-//        } else {
-//            strafePID = 0;
-//        }
-//
-//        telemetry.addLine("Wanted Position: "+robot_y_pos);
-//        telemetry.addLine("Actual Position: "+robot_y);
-//        telemetry.addLine("PID: "+strafePID);
-//
-//        // turbo
-//        if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) {
-//            x *= vazrooooming;
-//            y *= vazrooooming;
-//            z *= vazrooooming;
-//        } else {
-//            x *= normal;
-//            y *= normal;
-//            z *= normal;
-//        }
-//
-//        // actually set power
-//        if (fixed0Toggle || fixed90Toggle) {
-//            robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID));
-//        } else {
-//            robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
-//        }
-//
-//        // driver 2 controls
-//        // increment heights
-//        if (driver2.getDUp().isJustPressed()) {
-//            Slides.heightOffset += heightIncrement;
-//            robot.slides.setTarget(robot.slides.getTarget() + heightIncrement);
-//        } else if (driver2.getDDown().isJustPressed()) {
-//            Slides.heightOffset -= heightIncrement;
-//            robot.slides.setTarget(robot.slides.getTarget() - heightIncrement);
-//        }
-//
-//        switch (robot.runningMacro) {
-//            case(0): // manual mode
-//
-//                //track pad is rad
-//                // left right dpad is turn wrist
-//                robot.slides.increaseTarget(driver2.getLeftStick().getY());
-////                if (Math.abs(driver2.getRightStick().getY()) > 0.05) { // close claw if anything is moved
-////                    robot.claw.close();
-////                }
-//
-////                if (driver2.getRightBumper().isJustPressed()) {
-////                    robot.arm.goToScore();
-////                }
-////                if (driver2.getLeftBumper().isJustPressed()) {
-////                    robot.arm.goToIntake();
-////                }
-//
-//                // retract all the time
-//                if (driver2.getLeftStickButton().isJustPressed()) {
-//                    robot.runningMacro = 5;
-//                }
-//
-//                // to cancel macro and allow manual movement
-////                if (driver2.getLeftStick().getY() > 0.05 || driver2.getRightStick().getY() > 0.05) {
-////                    robot.runningMacro = 0;
-////                    robot.lastMacro = 0;
-////                }
-//
-//                if (driver2.getTouchpad().isJustPressed()) {
-//                    robot.runningMacro = 4;
-//                } else if (driver2.getX().isJustPressed()) { // high position [closed, bring up, bring out]
-//                    robot.runningMacro = 3;
-//                } else if (driver2.getY().isJustPressed()) { // middle position [middle goal level]
-//                    robot.runningMacro = 2;
-//                } else if (driver2.getB().isJustPressed() && !driver2.getStart().isJustPressed()) { // low position [low goal level]
-//                    robot.runningMacro = 1;
-//                } else if (driver2.getA().isJustPressed()) {
-//                    if (robot.lastMacro == 0 || robot.lastMacro == 4) { // if not running any macros or picking up cones
-//                    } else { // otherwise, I need to undo a macro
-//                        robot.runningMacro = 5;
-//                    }
-//                } else {
-//                    if (driver2.getRightBumper().isJustPressed()) {
-//                        isAutoClose = false;
-//                    }
-//                    if (driver2.getLeftBumper().isJustPressed()) {
-//                    }
-//                }
-//                break;
-//            case(1):
-////                robot.extendMacro(Slides.Position.LOW, Arm.APosition.SCORE, getRuntime());
-//                break;
-//            case(2):
-////                robot.extendMacro(Slides.Position.MEDIUM, Arm.APosition.SCORE, getRuntime());
-//                break;
-//            case(3):
-////                robot.extendMacro(Slides.Position.HIGH, Arm.APosition.SCORE, getRuntime());
-//                break;
-//            case(4):
-////                robot.extendMacro(Slides.Position.PICKUP, Arm.APosition.SDOWN, getRuntime());
-//                break;
-//            case (5): // macro reset
-//                robot.resetMacroNoDunk(0, getRuntime());
-//                driver1.rumble(100);
-//                break;
-//        }
-//
-//        // cancel the macros
-//        if (driver2.getRightStickButton().isJustPressed()) {
-//            robot.runningMacro = 0;
-//            robot.lastMacro = 0;
-//            robot.macroState = 0;
-//            robot.slides.cancel();
-//        }
-//
-//        // update and telemetry
-//        robot.update(getRuntime());
-//
-//        telemetry.addLine(robot.getTelemetry());
-//        telemetry.addLine(String.format("Last Macro: %s\nRunning Macro: %s\nMacroState: %s", robot.lastMacro, robot.runningMacro, robot.macroState));
-//        telemetry.update();
-//    }
-//}

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

@@ -1,11 +0,0 @@
-//package org.firstinspires.ftc.teamcode.opmode.teleop;
-//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-//
-//@TeleOp(name = "Opmode For Team Blue", group = "Competition")
-//public class BlueTeam extends AbstractTeleOp {
-//
-//    public BlueTeam() {
-//        team = Team.BLUE;
-//    }
-//}

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

@@ -1,257 +0,0 @@
-//package org.firstinspires.ftc.teamcode.opmode.teleop;
-//
-//import com.acmerobotics.dashboard.config.Config;
-//import com.acmerobotics.roadrunner.geometry.Pose2d;
-//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-//import com.qualcomm.robotcore.hardware.Servo;
-//import com.qualcomm.robotcore.util.ElapsedTime;
-//
-//import org.firstinspires.ftc.teamcode.controller.Controller;
-//import org.firstinspires.ftc.teamcode.hardware.DoorPos;
-//import org.firstinspires.ftc.teamcode.hardware.Down;
-//import org.firstinspires.ftc.teamcode.hardware.Hieght;
-//import org.firstinspires.ftc.teamcode.hardware.HopperPos;
-//import org.firstinspires.ftc.teamcode.hardware.Intake;
-//import org.firstinspires.ftc.teamcode.hardware.Robot;
-//
-//@Config
-//@TeleOp(name = "experimental opmode", group = "OpModes")
-//public class ExperimentalKhangMain extends OpMode {
-//
-//    //keep track of runtime for advanced macros
-//    private ElapsedTime runTime = new ElapsedTime();
-//
-//    //create and set start position of intake
-//    private Intake.Position Currentpos = Intake.Position.UP;
-//    //inform if slides are in lowest position or not
-//    private Down.DownArm DownQuestion = Down.DownArm.YES;
-//    //create and set start position of slides
-//    private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
-//    //create and set start position of arm
-//    private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN;
-//    //create and set start position of door, default close
-//    private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
-//    //create and set start position of hopper
-//    private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
-//    //create manual slide height varable and set it to hold position as start
-//    private Hieght.height CurrentHeight = Hieght.height.HOLD;
-//
-//    //robot position stuff
-//    Pose2d robot_pos;
-//    double robot_x, robot_y, robot_heading;
-//
-//    //create robot instance
-//    private Robot robot;
-//    //create servo for plane
-//    private Servo planeServo;
-//    //create controller 1 (driver)
-//    private Controller controller1;
-//    //create controller 2 (macro user and one with hard life)
-//    private Controller controller2;
-//    //set starting slide height to 0
-//    private double sHeight = 0;
-//
-//    @Override
-//    public void init() {
-//
-//        //reset runtime when opmode is initialized
-//        runTime.reset();
-//
-//        //make each servo, motor, and controller and configure them
-//        planeServo = hardwareMap.get(Servo.class, "Plane Servo");
-//        this.robot = new Robot(hardwareMap);
-//        controller1 = new Controller(gamepad1);
-//        controller2 = new Controller(gamepad2);
-//        //feedback to driver hub to know if init was successful
-//        telemetry.addLine("Started");
-//        //update to amke sure we receive last lien of code
-//        telemetry.update();
-//    }
-//
-//    @Override
-//    public void loop() {
-//
-//        // robot position update
-//        robot_pos = robot.drive.getPoseEstimate();
-//        robot_x = robot_pos.getX();
-//        robot_y = robot_pos.getY();
-//        robot_heading = robot_pos.getHeading(); // in radians
-//
-//        // Calculate the runtime in seconds
-//        double currentTime = runTime.seconds();
-//
-//        //set start of macro runtime
-//        double macroStartTime = 0;
-//
-//        //create and set X, Y, and Z for drive inputs
-//            double x = -gamepad1.left_stick_y;
-//            double y = -gamepad1.left_stick_x;
-//            double z = -gamepad1.right_stick_x;
-//            robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
-//
-//        //set intake to be pressure reactant to right trigger
-//        robot.intake.setDcMotor(gamepad2.right_trigger);
-//        //manual slide height control
-//        double sHeight = gamepad2.right_stick_y;
-//        //activate intake or not
-//        double intakeON = gamepad2.right_trigger;
-//        //graph input of Y joystick to make macros for slides
-//        double sY = -gamepad2.left_stick_y;
-//        //graph of X
-//        double sX = gamepad2.left_stick_x;
-//
-//        //reset value to set slides to starting value
-//        double reset = gamepad2.left_trigger;
-//
-//        //variable to determine shoot drone or not
-//        double shoot = gamepad1.right_trigger;
-//
-//        //update variables to constantly feed position each component should be in
-//            //intake
-//        robot.intake.setpos(Currentpos);
-//            //slide macro
-//        robot.slides.setSPos(CurrentSpos);
-//            //slide manual
-////        robot.slides.setHeight(CurrentHeight);
-//            //arm position
-//        robot.arm.setPos(CurrentApos);
-//            //door open or not
-//        robot.arm.openDoor(CurrentDpos);
-//            //update
-//        controller2.update();
-//
-//        //manual slide input reader
-////        if (sHeight >= 0.2) {
-////            CurrentHeight = Hieght.height.ASCEND;
-////        } else if (sHeight <= -0.2) {
-////            CurrentHeight = Hieght.height.DESCEND;
-////        } else {
-////            CurrentHeight = Hieght.height.HOLD;
-////        }
-//
-//        //read values to determine if plane should shoot or not
-//        if (shoot >= 0.9) {
-//            planeServo.setPosition(0.2);
-//        } else {
-//            planeServo.setPosition(0);
-//        }
-//
-//        //make door rise as intake goes on
-//        if (intakeON >= 0.01) {
-//            CurrentDpos = DoorPos.DoorPosition.OPEN;
-//        } else {
-//            CurrentDpos = DoorPos.DoorPosition.CLOSE;
-//        }
-//
-//        if (intakeON >= 0.35) {
-//            Currentpos = Intake.Position.STACK1;
-//        } else {
-//            Currentpos = Intake.Position.UP;
-//        }
-//
-//        //reset slide position
-//        if (reset >= 0.2) {
-//            CurrentSpos = CurrentSpos.DOWN;
-//        }
-//
-////        //control position of hopper
-////        if (controller2.getLeftBumper().isJustPressed()) {
-////            hopperpos = HopperPos.hopperPos.UP;
-////        } else if (controller2.getLeftBumper().isJustReleased()) {
-////            hopperpos = HopperPos.hopperPos.DOWN;
-////        }
-//
-//        //shift intake up one pixel
-//        if (controller2.getDLeft().isJustPressed()) {
-//            //prevent from going higher than highest legal value
-//            if (Currentpos != Intake.Position.UP) {
-//                Currentpos = Currentpos.nextPosition();
-//            }
-//        }
-//
-//        //shift intake down one pixel
-//        if (controller2.getDRight().isJustPressed()) {
-//            //prevent from going lower than lowest value
-//            if (Currentpos != Intake.Position.STACK1) {
-//                Currentpos = Currentpos.previousPosition();
-//            }
-//        }
-//
-//        //set intake to max position
-//        if (controller2.getDUp().isJustPressed()) {
-//            Currentpos = Currentpos.UP;
-//        }
-//
-//        //set intake to lowest position
-//        if (controller2.getDDown().isJustPressed()) {
-//            Currentpos = Currentpos.STACK1;
-//        }
-//
-////        //arm safety pause going up
-////        if (controller2.getA().isJustPressed()){
-////            CurrentApos = CurrentApos.SAFTEYUP;
-////            DownQuestion = DownQuestion.NO;
-////        }
-//
-//        if (controller2.getA().isJustPressed()) {
-//            macroStartTime = currentTime;
-//            CurrentSpos = CurrentSpos.TAPE1;
-//            if (macroStartTime + 1000 <= currentTime) {
-//                CurrentApos = CurrentApos.SCORE;
-//            }
-//        }
-//
-//        //arm safety pause going down
-//        if (controller2.getX().isJustPressed()) {
-//            CurrentApos = CurrentApos.SAFTEYDOWN;
-//            DownQuestion = DownQuestion.NO;
-//        }
-//
-//        //arm all the way up
-//        if (controller2.getB().isJustPressed()) {
-//            CurrentApos = CurrentApos.SCORE;
-//            DownQuestion = DownQuestion.NO;
-//        }
-//
-//        //arm all the way down
-//        if (controller2.getY().isJustPressed()) {
-//            CurrentApos = CurrentApos.SDOWN;
-//            DownQuestion = DownQuestion.YES;
-//        }
-//
-//        //set slides to max when left joystick is up
-//        if (sY >= 0.5) {
-//            CurrentSpos = CurrentSpos.MAX;
-//        }
-//
-//        //set slides to tape 1 level when left joystick is down
-//        if (sY <= -0.5) {
-//            CurrentSpos = CurrentSpos.TAPE1;
-//        }
-//
-//        //set slides to tape 3 when left joystick is right
-//        if (sX >= 0.2) {
-//            CurrentSpos = CurrentSpos.TAPE3;
-//        }
-//
-//        //set slides to tape 2 when left joystick is left
-//        if (sX <= -0.2) {
-//            CurrentSpos = CurrentSpos.TAPE2;
-//        }
-//
-//        //set slides all the way down when left bumper gets pressed
-//        if (controller2.getLeftBumper().isJustPressed()) {
-//            CurrentSpos = CurrentSpos.DOWN;
-//        }
-//
-//        //set intake all teh way up when right bumper is pressed
-//        if (controller2.getRightBumper().isJustPressed()) {
-//            Currentpos = Currentpos.UP;
-//        }
-//
-//        // update the runtime on the telemetry
-//        telemetry.addData("Runtime", currentTime);
-//        telemetry.update();
-//    }
-//}

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

@@ -1,7 +1,7 @@
 package org.firstinspires.ftc.teamcode.opmode.teleop;
 
-import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
-import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
+import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
@@ -11,14 +11,15 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import org.firstinspires.ftc.teamcode.controller.Controller;
 import org.firstinspires.ftc.teamcode.hardware.Intake;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
-import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
+import org.firstinspires.ftc.teamcode.hardware.Slides;
 
 @Config
 @TeleOp(name = "Main TeleOp", group = "OpModes")
-public class NewTeleop extends OpMode {
+public class MainTeleOp extends OpMode {
 
     public static double normal = 0.5;
     public static double turbo = 1;
+    public static double intakeMax = 0.65;
 
     private Robot robot;
     private Controller controller1;
@@ -30,9 +31,14 @@ public class NewTeleop extends OpMode {
         controller2 = new Controller(gamepad2);
 
         this.robot = new Robot(hardwareMap);
-        robot.intake.setpos(Intake.Position.STACK1);
+//        robot.intake.setpos(Intake.Position.STACK1);
 
-        telemetry.addLine("Started");
+        while (robot.camera.getFrameCount() < 1) {
+            telemetry.addLine("Initializing...");
+            telemetry.update();
+        }
+
+        telemetry.addLine("Initialized");
         telemetry.update();
     }
 
@@ -55,7 +61,7 @@ public class NewTeleop extends OpMode {
         robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
 
         // Driver 2
-        robot.intake.setDcMotor(gamepad2.right_trigger);
+        robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
         if (controller2.getRightBumper().isJustPressed()) {
             robot.intake.incrementPos();
         }
@@ -64,7 +70,7 @@ public class NewTeleop extends OpMode {
         }
 
         // Drone launcher
-        if (controller1.getA().isPressed()) {
+        if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
             this.robot.droneLauncher.launch();
         } else {
             this.robot.droneLauncher.reset();
@@ -74,20 +80,21 @@ public class NewTeleop extends OpMode {
         switch (robot.runningMacro) {
             case (0): // manual mode
                 robot.slides.increaseTarget(controller2.getLeftStick().getY());
-                if (controller2.getX().isJustPressed()) {
-                    robot.runningMacro = 1;
-                } else if (controller2.getY().isJustPressed()) {
-                    robot.runningMacro = 2;
-                } else if (controller2.getB().isJustPressed()) {
-                    robot.runningMacro = 3;
-                } else if (controller2.getA().isJustPressed()) {
-                    robot.runningMacro = 4;
-                }
                 if (robot.intake.getPower() >= 0.01) {
                     robot.arm.setDoor(OPEN);
                 } else {
                     robot.arm.setDoor(CLOSE);
                 }
+
+                if (controller2.getX().isJustPressed()) {
+                    robot.runningMacro = 1;
+                } else if (controller2.getY().isJustPressed()) {
+                    robot.runningMacro = 2;
+                } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 3;
+                } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+                    robot.runningMacro = 4;
+                }
                 break;
             case (1):
                 robot.extendMacro(Slides.Position.TIER1, getRuntime());

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

@@ -1,260 +0,0 @@
-//package org.firstinspires.ftc.teamcode.opmode.teleop;
-//import com.acmerobotics.dashboard.config.Config;
-//import com.acmerobotics.roadrunner.geometry.Pose2d;
-//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-//import com.qualcomm.robotcore.hardware.Servo;
-//
-//import org.firstinspires.ftc.teamcode.controller.Controller;
-//import org.firstinspires.ftc.teamcode.hardware.Arm;
-//import org.firstinspires.ftc.teamcode.hardware.DoorPos;
-//import org.firstinspires.ftc.teamcode.hardware.Down;
-//import org.firstinspires.ftc.teamcode.hardware.Hieght;
-//import org.firstinspires.ftc.teamcode.hardware.HopperPos;
-//import org.firstinspires.ftc.teamcode.hardware.Intake;
-//import org.firstinspires.ftc.teamcode.hardware.Robot;
-//import org.firstinspires.ftc.teamcode.hardware.SlidePos;
-//import com.qualcomm.robotcore.util.ElapsedTime;
-//
-//@Config
-//@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
-//public class Meet1TeleOp extends OpMode {
-//
-//    //turbo mode
-//    public static double normal = 0.5;
-//    public static double turbo = 1;
-//
-//    //keep track of runtime for advanced macros
-//    private ElapsedTime runTime = new ElapsedTime();
-//
-//    //create and set start position of intake
-//    private Intake.Position Currentpos = Intake.Position.UP;
-//    //inform if slides are in lowest position or not
-//    private Down.DownArm DownQuestion = Down.DownArm.YES;
-//    //create and set start position of slides
-//    private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
-//    //create and set start position of arm
-//    private Arm.Position CurrentApos = Arm.Position.SDOWN;
-//    //create and set start position of door, default close
-//    private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
-//    //create and set start position of hopper
-//    private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
-//    //create manual slide height varable and set it to hold position as start
-//    private Hieght.height CurrentHeight = Hieght.height.HOLD;
-//    //create robot instance
-//    private Robot robot;
-//    //create servo for plane
-//    private Servo planeServo;
-//    //create controller 1 (driver)
-//    private Controller controller1;
-//    //create controller 2 (macro user and one with hard life)
-//    private Controller controller2;
-//    //set starting slide height to 0
-//    private double sHeight = 0;
-//
-//    @Override
-//    public void init() {
-//
-//        //reset runtime when opmode is initialized
-//        runTime.reset();
-//
-//        //make each servo, motor, and controller and configure them
-//        planeServo = hardwareMap.get(Servo.class, "Plane Servo");
-//        this.robot = new Robot(hardwareMap);
-//        controller1 = new Controller(gamepad1);
-//        controller2 = new Controller(gamepad2);
-//        //feedback to driver hub to know if init was successful
-//        telemetry.addLine("Started");
-//        //update to make sure we receive last line of code
-//        telemetry.update();
-//    }
-//
-//    @Override
-//    public void loop() {
-//
-//        // Calculate the runtime in seconds
-//        double currentTime = runTime.seconds();
-//
-//        //set start of macro runtime
-//        double macroStartTime = 0;
-//
-//        //create and set X, Y, and Z for drive inputs
-//            double x = -gamepad1.left_stick_y;
-//            double y = -gamepad1.left_stick_x;
-//            double z = -gamepad1.right_stick_x;
-//            robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
-//
-//        //turbo activation
-//        if (controller1.getA().isJustPressed()){
-//            x *= turbo;
-//            y *= turbo;
-//            z *= turbo;
-//        } else if (controller1.getA().isJustReleased()){
-//            x *= normal;
-//            y *= normal;
-//            z *= normal;
-//        }
-//
-//        //set intake to be pressure reactant to right trigger
-//        robot.intake.setDcMotor(gamepad2.right_trigger);
-//        //manual slide height control
-//        double sHeight = gamepad2.right_stick_y;
-//        //activate intake or not
-//        double intakeON = gamepad2.right_trigger;
-//        //graph input of Y joystick to make macros for slides
-//        double sY = -gamepad2.left_stick_y;
-//        //graph of X
-//        double sX = gamepad2.left_stick_x;
-//        double manualSY = gamepad2.right_stick_y;
-//
-//        //reset value to set slides to starting value
-//        double reset = gamepad2.left_trigger;
-//
-//        //variable to determine shoot drone or not
-//        double shoot = gamepad1.right_trigger;
-//
-//        //update variables to constantly feed position each component should be in
-//            //intake
-//        robot.intake.setpos(Currentpos);
-//            //slide macro
-//        robot.slides.setSPos(CurrentSpos);
-//            //slide manual
-////        robot.slides.setHeight(CurrentHeight);
-//            //arm position
-//        robot.arm.setArmPos(CurrentApos);
-//            //door open or not
-//        robot.arm.openDoor(CurrentDpos);
-//            //update
-//        controller2.update();
-//
-//        //manual slide input reader
-////        if (sHeight >= 0.2) {
-////            CurrentHeight = Hieght.height.ASCEND;
-////        } else if (sHeight <= -0.2) {
-////            CurrentHeight = Hieght.height.DESCEND;
-////        } else {
-////            CurrentHeight = Hieght.height.HOLD;
-////        }
-//
-//        //read values to determine if plane should shoot or not
-//        if (shoot >= 0.9) {
-//            planeServo.setPosition(0.2);
-//        } else {
-//            planeServo.setPosition(0);
-//        }
-//
-//        //make door rise as intake goes on
-//        if (intakeON >= 0.01) {
-//            CurrentDpos = DoorPos.DoorPosition.OPEN;
-//        } else {
-//            CurrentDpos = DoorPos.DoorPosition.CLOSE;
-//        }
-//
-//        //make intake go to stack 1 when pressed down
-//        if (intakeON >= 0.25) {
-//            Currentpos = Intake.Position.STACK1;
-//        } else {
-//            Currentpos = Intake.Position.UP;
-//        }
-//
-//        //reset slide position
-//        if (reset >= 0.2) {
-//            CurrentSpos = CurrentSpos.DOWN;
-//        }
-//
-////        //control position of hopper
-////        if (controller2.getLeftBumper().isJustPressed()) {
-////            hopperpos = HopperPos.hopperPos.UP;
-////        } else if (controller2.getLeftBumper().isJustReleased()) {
-////            hopperpos = HopperPos.hopperPos.DOWN;
-////        }
-//
-//        //shift intake up one pixel
-//        if (controller2.getDLeft().isJustPressed()) {
-//            //prevent from going higher than highest legal value
-//            if (Currentpos != Intake.Position.UP) {
-//                Currentpos = Currentpos.nextPosition();
-//            }
-//        }
-//
-//        //shift intake down one pixel
-//        if (controller2.getDRight().isJustPressed()) {
-//            //prevent from going lower than lowest value
-//            if (Currentpos != Intake.Position.STACK1) {
-//                Currentpos = Currentpos.previousPosition();
-//            }
-//        }
-//
-//        //set intake to max position
-//        if (controller2.getDUp().isJustPressed()) {
-//            Currentpos = Currentpos.UP;
-//        }
-//
-//        //set intake to lowest position
-//        if (controller2.getDDown().isJustPressed()) {
-//            Currentpos = Currentpos.STACK1;
-//        }
-//
-//        //arm safety pause going up
-//        if (controller2.getA().isJustPressed()){
-//            CurrentApos = CurrentApos.SAFTEYUP;
-//            DownQuestion = DownQuestion.NO;
-//        }
-//
-//        //arm safety pause going down
-//        if (controller2.getX().isJustPressed()) {
-//            CurrentApos = CurrentApos.SAFTEYDOWN;
-//            DownQuestion = DownQuestion.NO;
-//        }
-//
-//        //arm all the way up
-//        if (controller2.getB().isJustPressed()) {
-//            CurrentApos = CurrentApos.SCORE;
-//            DownQuestion = DownQuestion.NO;
-//        }
-//
-//        //arm all the way down
-//        if (controller2.getY().isJustPressed()) {
-//            CurrentApos = CurrentApos.SDOWN;
-//            DownQuestion = DownQuestion.YES;
-//        }
-//
-//        //set slides to max when left joystick is up
-//        if (sY >= 0.5) {
-//            CurrentSpos = CurrentSpos.MAX;
-//        }
-//
-//        //set slides to tape 1 level when left joystick is down
-//        if (sY <= -0.5) {
-//            CurrentSpos = CurrentSpos.TAPE1;
-//        }
-//
-//        //set slides to tape 3 when left joystick is right
-//        if (sX >= 0.2) {
-//            CurrentSpos = CurrentSpos.TAPE3;
-//        }
-//
-//        //set slides to tape 2 when left joystick is left
-//        if (sX <= -0.2) {
-//            CurrentSpos = CurrentSpos.TAPE2;
-//        }
-//
-//        if (manualSY >=0.1) {
-//            CurrentSpos = CurrentSpos.MANUAL;
-//        }
-//
-////        //set slides all the way down when left bumper gets pressed
-////        if (controller2.getLeftBumper().isJustPressed()) {
-////            CurrentSpos = CurrentSpos.DOWN;
-////        }
-////
-////        //set intake all teh way up when right bumper is pressed
-////        if (controller2.getRightBumper().isJustPressed()) {
-////            Currentpos = Currentpos.UP;
-////        }
-//
-//        // update the runtime on the telemetry
-//        telemetry.addData("Runtime", currentTime);
-//        telemetry.update();
-//    }
-//}

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

@@ -1,11 +0,0 @@
-//package org.firstinspires.ftc.teamcode.opmode.teleop;
-//
-//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-//
-//@TeleOp(name = "Opmode For Team Red", group = "Competition")
-//public class RedTeam extends AbstractTeleOp {
-//
-//    public RedTeam() {
-//        team = Team.RED;
-//    }
-//}

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

@@ -1,6 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.teleop;
-
-    public enum Team {
-        RED, BLUE
-    }
-

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java

@@ -66,14 +66,11 @@ public class DriveConstants {
      * small and gradually increase them later after everything is working. All distance units are
      * inches.
      */
-    // old is 35 35 120 120
+    public static double MAX_VEL = 50;
-    // new is 70 45 90 90
+    public static double MAX_ACCEL = 50;
-    public static double MAX_VEL = 60;
-    public static double MAX_ACCEL = 60;
     public static double MAX_ANG_VEL = Math.toRadians(120);
     public static double MAX_ANG_ACCEL = Math.toRadians(120);
 
-
     public static double encoderTicksToInches(double ticks) {
         return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java

@@ -63,7 +63,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
 
         parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backLeft"));
 //        parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
-        perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "Intake Motor"));
+        perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
 
         // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
         perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);