added opmode jsut for drivebase to driver pratice one at a time moved turbo to driver A button and works

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

@@ -5,6 +5,8 @@ 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 {
 
@@ -13,11 +15,16 @@ public class Arm {
     private Servo rAServo;
     private Servo lAServo;
     private Servo wristServo;
-    private SlidePos.SPosition pos = SlidePos.SPosition.DOWN;
+    private Slides.Position pos = Slides.Position.DOWN;
     private PController armController;
     private double armControllerTarget;
     private double ARM_KP = 0.001;
 
+
+    public enum APosition {
+        SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP;
+    }
+
     public Arm(HardwareMap hardwareMap) {
         wristServo = hardwareMap.get(Servo.class, "Wrist Servo");
         dServo = hardwareMap.get(Servo.class, "Door Servo");
@@ -47,22 +54,22 @@ public class Arm {
         }
     }
 
-    public void setPos(ArmPos.APosition tape) {
+    public void setPos(APosition tape) {
-        if (tape == ArmPos.APosition.SDOWN) {
+        if (tape == APosition.SDOWN) {
             this.armControllerTarget = startingArmPos;
             lAServo.setPosition(startingArmPos);
             rAServo.setPosition(startingArmPos);
             wristServo.setPosition(startingWristPos);
-        } else if (tape == ArmPos.APosition.SCORE) {
+        } else if (tape == APosition.SCORE) {
             this.armControllerTarget = armScorePos;
             lAServo.setPosition(armScorePos);
             rAServo.setPosition(armScorePos);
             wristServo.setPosition(wristScorePos);
-        } else if (tape == ArmPos.APosition.SAFTEYDOWN) {
+        } else if (tape == APosition.SAFTEYDOWN) {
             lAServo.setPosition(safteyDownPos);
             rAServo.setPosition(safteyDownPos);
             wristServo.setPosition(wristScorePos);
-        } else if (tape == ArmPos.APosition.SAFTEYUP) {
+        } else if (tape == APosition.SAFTEYUP) {
             lAServo.setPosition(safteyUpPos);
             rAServo.setPosition(safteyUpPos);
             wristServo.setPosition(wristScorePos);

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

@@ -82,9 +82,6 @@ public class Camera {
         return (targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION);
     }
 
-    public Detection getBlack() {
-        return (targetingCameraInitialized ? targetingPipeline.getBlack() : INVALID_DETECTION);
-    }
 
     //return frame rate
     public int getFrameCount() {

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

@@ -34,7 +34,7 @@ public class Intake {
     public static double up = 0.30;
     public static double motorPower = 0;
 
-    public Intake(HardwareMap hardwareMap) {
+    public Intake(HardwareMap hardwareMap, Position up) {
         lServo = hardwareMap.get(Servo.class, "Left Servo");
         lServo.setDirection(Servo.Direction.REVERSE);
         rServo = hardwareMap.get(Servo.class, "Right Servo");

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

@@ -6,10 +6,10 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 @Config
-public class Slides {
+public class KhangSlides {
 
     //Create and assign motors
-    public Slides(HardwareMap hardwareMap) {
+    public KhangSlides(HardwareMap hardwareMap) {
         rSMotor = hardwareMap.get(DcMotor.class, "Right Slide Motor");
 //        rSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
         rSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

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

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
@@ -13,18 +15,23 @@ public class Robot {
     public SampleMecanumDrive drive;
     public Camera camera;
     public Intake intake;
-    public Slides slides;
+    public KhangSlides slides;
     public Arm arm;
+    public double macroStartTime = 0;
+    public int macroState = 0;
+    public int runningMacro = 0; // 0 = no macro | 1 = low macro | 2 = mid macro | 3 = high macro | 4 = pickup macro
+    public int lastMacro = 0;
+
     private boolean camEnabled = true;
 
     //Name the objects
     public Robot(HardwareMap hardwareMap) {
         arm = new Arm(hardwareMap);
         drive = new SampleMecanumDrive(hardwareMap);
-        slides = new Slides(hardwareMap);
+        slides = new KhangSlides(hardwareMap);
         camera = new Camera(hardwareMap);
         camera.initTargetingCamera();
-        intake = new Intake(hardwareMap);
+        intake = new Intake(hardwareMap, UP);
         camEnabled = true;
         }
 
@@ -39,6 +46,66 @@ public class Robot {
         return String.format("position: %s", slide.getCurrentPosition());
     }
 
+//    public void resetMacroNoDunk(int pos, double runTime) {
+//        switch(macroState) {
+//            case(0):
+//                macroStartTime = runTime;
+//                macroState++;
+//                break;
+//            case(1):
+//                if (runTime > macroStartTime) {
+//                    macroState ++;
+//                }
+//                break;
+//            case(2):
+//                macroStartTime = runTime;
+//                slides.setTarget(pos);
+//                macroState = 0;
+//                runningMacro = 0;
+//                lastMacro = 0;
+//        }
+//    }
+//
+//    public void resetMacro(int pos, double runTime) {
+//        switch(macroState) {
+//            case(0):
+//                macroStartTime = runTime;
+//                macroState++;
+//                break;
+//            case(1):
+//                if (runTime > macroStartTime) {
+//                    macroState ++;
+//                }
+//                break;
+//            case(2):
+//                macroStartTime = runTime;
+//                slides.setTarget(pos);
+//                macroState = 0;
+//                runningMacro = 0;
+//                lastMacro = 0;
+//        }
+//    }
+//
+//    public void resetMacroEnd(int pos, double runTime) {
+//        switch(macroState) {
+//            case(0):
+//                macroStartTime = runTime;
+//                macroState++;
+//                break;
+//            case(1):
+//                if (runTime > macroStartTime) {
+//                    macroState ++;
+//                }
+//                break;
+//            case(2):
+//                macroStartTime = runTime;
+//                slides.setTarget(pos);
+//                macroState = 0;
+//                runningMacro = 0;
+//                lastMacro = 0;
+//        }
+//    }
+
     public Arm getArm() {
         return this.arm;
     }

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

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

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

@@ -0,0 +1,126 @@
+package org.firstinspires.ftc.teamcode.hardware.robby;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.controller.PIDController;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+@Config
+public class Slides {
+    private DcMotor slide;
+    private DcMotor slide2;
+
+    public static double p = 0.0014;
+    public static double i = 0.02;
+    public static double d = 0;
+    public static double f = 0.01;
+    public static double pTolerance = 20;
+    public static PIDController controller = new PIDController(p, i, d);
+
+    public static int[] heights = {0, (int)(190/4.0), 2*(int)(190/4.0), 3*(int)(190/4.0), 4*(int)(180/4)};
+
+    public static int heightOffset = 0;
+    public static int targetMin = -10;
+    public static int targetMax = 770;
+
+    public static int highPos = 720 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES
+    public static int midPos = 350 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES
+    public static int lowPos = heightOffset; // ALSO DEFINED IN UPDATE SLIDES
+    public static int pickupPos = 220 + heightOffset; // ALSO DEFINED IN UPDATE SLIDES
+    public static int downPos = heightOffset;
+
+    private int target = 0;
+
+    public static int manualSpeed = 20;
+    public static int zeroPower = 5;
+
+    public enum Position { HIGH, MEDIUM, LOW, PICKUP, DOWN }
+
+    public Slides(HardwareMap hardwareMap) {
+        slide = hardwareMap.get(DcMotor.class, "slide");
+        slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+//        slide.setDirection(DcMotorSimple.Direction.REVERSE);
+        slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        slide2 = hardwareMap.get(DcMotor.class, "slide2");
+        slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        slide2.setDirection(DcMotorSimple.Direction.REVERSE);
+        slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void setTarget(int pos) {
+        target = Math.min(Math.max(pos, targetMin), targetMax);
+    }
+
+    public void setTarget(Position pos) {
+        if (pos == Position.HIGH) {
+            target = Math.min(Math.max(highPos, targetMin), targetMax);
+        } else if (pos == Position.MEDIUM) {
+            target = Math.min(Math.max(midPos, targetMin), targetMax);
+        } else if (pos == Position.LOW) {
+            target = Math.min(Math.max(lowPos, targetMin), targetMax);
+        } else if (pos == Position.PICKUP) {
+            target = Math.min(Math.max(pickupPos, targetMin), targetMax);
+        } else if (pos == Position.DOWN) {
+            target = Math.min(Math.max(downPos, targetMin), targetMax);
+        }
+    }
+
+    public void increaseTarget(double increase) {
+        target += (int) (increase * manualSpeed);
+        target = Math.min(targetMax, Math.max(targetMin, target));
+    }
+
+    public int getTarget() {
+        return target;
+    }
+
+    public boolean atTarget() {
+        return controller.atSetPoint();
+    }
+
+    public void cancel() {
+        target = slide.getCurrentPosition();
+    }
+
+    public void targetReset() {
+        target = targetMin;
+    }
+
+    public void update(double runTime) {
+//        highPos = 720 + heightOffset;
+//        midPos = 350 + heightOffset;
+//        lowPos = heightOffset;
+//        pickupPos = 20 + heightOffset;
+//        downPos = heightOffset;// TODO add these back in
+
+//        if (target == 0) {
+//            slide.setPower(0);
+//            slide2.setPower(0);
+//        } else {
+//            if (target < 5) {
+//                slide.setPower(0);
+//                slide2.setPower(0);
+//            } else {
+        double pid, ff;
+        controller.setPID(p, i, d);
+        controller.setTolerance(pTolerance);
+
+        pid = controller.calculate(slide.getCurrentPosition(), target);
+        ff = f;
+        slide.setPower(pid + ff);
+
+        pid = controller.calculate(slide2.getCurrentPosition(), target);
+        ff = f;
+        slide2.setPower(pid + ff);
+//            }
+//        }
+    }
+
+    public String getTelemetry() {
+        return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower(), heightOffset);
+    }
+}

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

@@ -1,150 +1,304 @@
-package org.firstinspires.ftc.teamcode.opmode.teleop;
+//package org.firstinspires.ftc.teamcode.opmode.teleop;
-
-import static java.lang.Math.PI;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
-import org.firstinspires.ftc.teamcode.controller.Controller;
-import org.firstinspires.ftc.teamcode.hardware.Robot;
-
-@Config
-public abstract class AbstractTeleOp extends OpMode {
-    private Robot robot;
-    Controller driver1;
-    Controller driver2;
-
-    public static double drivebaseThrottle = 0.6;
-    public static double drivebaseTurbo = 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;
-
-    double strafePID;
-    public static double strafeP = 0.05;
-    public static double strafeI = 0;
-    public static double strafeD = 0.01;
-
-    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;
+//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.BLUE;
-//    public static double coneRadius = 4;
+//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.RED;
+//import static java.lang.Math.PI;
 //
-//    public static double armWait = 0.2;
+//import com.acmerobotics.dashboard.config.Config;
-
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
-//    private double timeSinceOpened = 0; // for claw
+//import com.arcrobotics.ftclib.controller.PIDController;
-//    private double timeSinceClosed = 0;
+//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-
+//
-//    private int delayState = 0; // for arm
+//import org.firstinspires.ftc.teamcode.controller.Controller;
-//    private double delayStart = 0;
+//import org.firstinspires.ftc.teamcode.hardware.Arm;
-//    private boolean doArmDelay = false; // for arm
+//import org.firstinspires.ftc.teamcode.hardware.Robot;
-    private boolean isAutoClose = true;
+//import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
-
+//
-    @Override
+//@Config
-    public void init() {
+//public abstract class AbstractTeleOp extends OpMode {
-        robot =  new Robot(hardwareMap);
+//    private Robot robot;
-        driver1 = new Controller(gamepad1);
+//    public Team team;
-        driver2 = new Controller(gamepad2);
+//    Controller driver1;
-    }
+//    Controller driver2;
-
+//
-    @Override
+//    public static double normal = 0.5;
-    public void init_loop() {
+//    public static double vazrooooming = 1.0;
-//        if (robot.camera.getFrameCount() < 1) {
+//    public static int heightIncrement = 20;
-//            telemetry.addLine("Initializing Robot...");
+//
-//        } else {
+//    Pose2d robot_pos;
-//            telemetry.addLine("Initialized");
+//    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);
 //        }
-        telemetry.addLine("Initialized");
+//
-        telemetry.addLine(robot.getTelemetry());
+//        PoseStorage.AutoJustEnded = false;
-        telemetry.update();
+//    }
-    }
+//
-
+//    @Override
-    private int getQuadrant(double angle) {
+//    public void init_loop() {
-        if (0 < angle && angle < PI/2.0) {
+//        ////init when camera is on and responding
-            return 1;
+////        if (robot.camera.getFrameCount() < 1) {
-        } else if (PI/2.0 < angle && angle < PI) {
+////            telemetry.addLine("Initializing Robot...");
-            return 2;
+////        } else {
-        } else if (PI < angle && angle < 3*PI/2.0) {
+////            telemetry.addLine("Initialized");
-            return 3;
+////        }
-        } else {
+//        telemetry.addLine("Initialized");
-            return 4;
+//        telemetry.addLine(robot.getTelemetry());
-        }
+//        telemetry.update();
-    }
+//    }
-
+//
-    @Override
+//    //find out what quarter you are in
-    public void loop() {
+//    private int getQuadrant(double angle) {
-        // robot position update
+//        if (0 < angle && angle < PI/2.0) {
-        //robot_pos = robot.drive.getPoseEstimate();
+//            return 1;
-        //robot_x = robot_pos.getX();
+//        } else if (PI/2.0 < angle && angle < PI) {
-        //robot_y = robot_pos.getY();
+//            return 2;
-        //robot_heading = robot_pos.getHeading(); // in radians
+//        } else if (PI < angle && angle < 3*PI/2.0) {
-
+//            return 3;
-        // driver 1 controls
+//        } else {
-        //driver1.update();
+//            return 4;
-        //driver2.update();
+//        }
-        double x = -driver1.getLeftStick().getY();
+//    }
-        double y = driver1.getLeftStick().getX();
+//
-        double z = -driver1.getRightStick().getX();
+//    @Override
-
+//    public void loop() {
-//         figure out if a toggle is present
+//        // robot position update
-        if (driver1.getRightBumper().isPressed()) {
+//        robot_pos = robot.drive.getPoseEstimate();
-            if (!fixed90Toggle) {
+//        robot_x = robot_pos.getX();
-                //robot_y_pos = robot.drive.getPoseEstimate().getX();
+//        robot_y = robot_pos.getY();
-            }
+//        robot_heading = robot_pos.getHeading(); // in radians
-            fixed90Toggle = true;
+//
-        } else {
+//        // driver 1 controls
-            fixed90Toggle = false;
+//        driver1.update();
-        }
+//        driver2.update();
-
+//
-        if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) {
+//        //create and set X, Y, and Z for drive inputs
-            if (!fixed0Toggle) {
+//        double x = -gamepad1.left_stick_y;
-                //robot_y_pos = robot.drive.getPoseEstimate().getY();
+//        double y = -gamepad1.left_stick_x;
-            }
+//        double z = -gamepad1.right_stick_x;
-            fixed0Toggle = true;
+//
-        } else {
+////         figure out if a toggle is present
-            fixed0Toggle = false;
+//        if (driver1.getRightBumper().isPressed()) {
-        }
+//            if (!fixed90Toggle) {
-
+//                robot_y_pos = robot.drive.getPoseEstimate().getX();
-        telemetry.addLine("Wanted Position: "+robot_y_pos);
+//            }
-        telemetry.addLine("Actual Position: "+robot_y);
+//            fixed90Toggle = true;
-        telemetry.addLine("PID: "+strafePID);
+//        } else {
-
+//            fixed90Toggle = false;
-        // turbo
+//        }
-        if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) {
+//
-            x *= drivebaseTurbo;
+//        if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) {
-            y *= drivebaseTurbo;
+//            if (!fixed0Toggle) {
-            z *= drivebaseTurbo;
+//                robot_y_pos = robot.drive.getPoseEstimate().getY();
-        } else {
+//            }
-            x *= drivebaseThrottle;
+//            fixed0Toggle = true;
-            y *= drivebaseThrottle;
+//        } else {
-            z *= drivebaseThrottle;
+//            fixed0Toggle = false;
-        }
+//        }
-
+//
-        // actually set power
+//        // heading pid
-        if (fixed0Toggle || fixed90Toggle) {
+//        headingController.setPID(headingP, headingI, headingD);
-            //robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID));
+//        int quadrant = getQuadrant(robot_heading);
-        } else {
+//        if (fixed0Toggle) {
-            //robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
+//            switch (quadrant) {
-        }
+//                case 1:
-
+//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 0);//- right
-        // update and telemetry
+//                    break;
-        //robot.update(getRuntime());
+//                case 2:
-
+//                    headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//+ left
-        telemetry.addLine(robot.getTelemetry());
+//                    break;
-        telemetry.update();
+//                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

@@ -0,0 +1,11 @@
+//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/Drivebase.java

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.opmode.teleop;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.controller.Controller;
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+
+@TeleOp(name = "Drivebase Only", group = "OpModes")
+public class Drivebase extends OpMode {
+    //turbo mode
+    public static double normal = 0.5;
+    public static double turbo = 1;
+    //create robot instance
+    private Robot robot;
+    //create controller 1 (driver)
+    private Controller controller1;
+    @Override
+    public void init() {
+        this.robot = new Robot(hardwareMap);
+        controller1 = new Controller(gamepad1);
+        telemetry.addLine("Started");
+        //update to make sure we receive last line of code
+        telemetry.update();
+    }
+
+    @Override
+    public void loop() {
+        //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;
+        }
+    }
+}

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

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

@@ -6,7 +6,7 @@ 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.ArmPos;
+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;
@@ -18,7 +18,7 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 
 @Config
 @TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
-public class KhangMain extends OpMode {
+public class Meet1TeleOp extends OpMode {
 
     //turbo mode
     public static double normal = 0.5;
@@ -34,7 +34,7 @@ public class KhangMain extends OpMode {
     //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;
+    private Arm.APosition CurrentApos = Arm.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
@@ -65,7 +65,7 @@ public class KhangMain extends OpMode {
         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
+        //update to make sure we receive last line of code
         telemetry.update();
     }
 
@@ -85,11 +85,11 @@ public class KhangMain extends OpMode {
             robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
 
         //turbo activation
-        if (controller1.getRightBumper().isJustPressed()){
+        if (controller1.getA().isJustPressed()){
             x *= turbo;
             y *= turbo;
             z *= turbo;
-        } else {
+        } else if (controller1.getA().isJustReleased()){
             x *= normal;
             y *= normal;
             z *= normal;
@@ -105,6 +105,7 @@ public class KhangMain extends OpMode {
         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;
@@ -145,9 +146,14 @@ public class KhangMain extends OpMode {
         //make door rise as intake goes on
         if (intakeON >= 0.01) {
             CurrentDpos = DoorPos.DoorPosition.OPEN;
-            Currentpos = Intake.Position.STACK1;
         } 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;
         }
 
@@ -233,15 +239,19 @@ public class KhangMain extends OpMode {
             CurrentSpos = CurrentSpos.TAPE2;
         }
 
-        //set slides all the way down when left bumper gets pressed
+        if (manualSY >=0.1) {
-        if (controller2.getLeftBumper().isJustPressed()) {
+            CurrentSpos = CurrentSpos.MANUAL;
-            CurrentSpos = CurrentSpos.DOWN;
         }
 
-        //set intake all teh way up when right bumper is pressed
+//        //set slides all the way down when left bumper gets pressed
-        if (controller2.getRightBumper().isJustPressed()) {
+//        if (controller2.getLeftBumper().isJustPressed()) {
-            Currentpos = Currentpos.UP;
+//            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);

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

@@ -0,0 +1,9 @@
+package org.firstinspires.ftc.teamcode.opmode.teleop;
+
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+public class PoseStorage {
+    public static Pose2d currentPose = new Pose2d();
+    public static boolean AutoJustEnded = false;
+}

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

@@ -0,0 +1,11 @@
+//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

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

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java

@@ -148,7 +148,4 @@ public class TargetingPipeline extends OpenCvPipeline {
     public Detection getBlue() {
         return this.blue;
     }
-    public Detection getBlack() {
-        return this.black;
-    }
 }