fixed teleop things, started new auto and varun is bad

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

@@ -24,8 +24,8 @@ public class Arm {
     private double wristPos;
     public static double ARM_KP = 0.08;
 
-    public static double doorOpenPos = 0.09;
+    public static double doorOpenPos = 0.3;
-    public static double doorClosePos = 0.26;
+    public static double doorClosePos = 0.61;
 
     public static double armStart = 0.24;
     public static double armScore = 0.95;

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

@@ -1,25 +0,0 @@
-package org.firstinspires.ftc.teamcode.hardware;
-
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.util.Constants;
-
-public class DroneLauncher {
-    private Servo servo;
-    public static double initPos = 0;
-    public static double launchPos = 0.5;
-
-    public DroneLauncher(HardwareMap hardwareMap) {
-        this.servo = hardwareMap.get(Servo.class, "Drone Launcher");
-        this.servo.setPosition(initPos);
-    }
-
-    public void launch() {
-        this.servo.setPosition(launchPos);
-    }
-
-    public void reset() {
-        this.servo.setPosition(initPos);
-    }
-}

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

@@ -18,7 +18,7 @@ public class Robot {
     public Intake intake;
     public Slides slides;
     public Arm arm;
-    public DroneLauncher droneLauncher;
+    public endGame_Mechs endGameMechs;
 
     public double macroStartTime = 0;
     public int macroState = 0;
@@ -28,7 +28,7 @@ public class Robot {
     private boolean camEnabled = true;
 
     public static double slideWait = 1.5;
-    public static double scoreWait = 1.25;
+    public static double scoreWait = 0.65;
     public static double armWait = 2.0;
 
     //Name the objects
@@ -39,7 +39,7 @@ public class Robot {
         intake = new Intake(hardwareMap, UP);
         slides = new Slides(hardwareMap);
         arm = new Arm(hardwareMap);
-        droneLauncher = new DroneLauncher(hardwareMap);
+        endGameMechs = new endGame_Mechs(hardwareMap);
         camEnabled = true;
     }
 
@@ -71,6 +71,7 @@ public class Robot {
             case(0):
                 macroStartTime = runTime;
                 arm.setDoor(OPEN);
+                slides.setTarget(slides.getTarget()+70);
                 macroState++;
                 break;
             case(1):
@@ -104,6 +105,6 @@ public class Robot {
     }
 
     public String getTelemetry() {
-        return arm.getTelemetry();
+        return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s", arm.getTelemetry(), slides.getTelemetry());
     }
 }

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

@@ -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 = 555;
+    public static int targetMax = 830;
 
     public static int down = 0;
     public static int tier1 = 350;
-    public static int tier2 = 450;
+    public static int tier2 = 500;
-    public static int tier3 = 550;
+    public static int tier3 = 760;
 
     private int target = 0;
 
@@ -71,6 +71,8 @@ public class Slides {
         return target;
     }
 
+
+
     public boolean atTarget() {
         return controller.atSetPoint();
     }
@@ -114,6 +116,6 @@ public class Slides {
     }
 
     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());
+        return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
     }
 }

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

@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Config
+public class endGame_Mechs {
+    private Servo servo;
+    private Servo hang1;
+    private Servo hang2;
+    public static double initPos = 0;
+    public static double launchPos = 0.5;
+    public static double hold = 0;
+    public static double release = 0.5;
+    public static double hold2 = 0;
+    public static double release2 = 0.5;
+
+    public endGame_Mechs(HardwareMap hardwareMap) {
+        this.servo = hardwareMap.get(Servo.class, "Drone Launcher");
+        this.servo.setPosition(initPos);
+        this.hang1 = hardwareMap.get(Servo.class, "Hanger 1");
+        this.hang1.setPosition(hold);
+        this.hang2 = hardwareMap.get(Servo.class, "Hanger 2");
+        this.hang2.setPosition(hold);
+
+    }
+
+    public void launch() {
+        this.servo.setPosition(launchPos);
+    }
+
+    public void reset() {
+        this.servo.setPosition(initPos);
+    }
+
+
+    public void release() {
+        this.servo.setPosition(release);
+        this.servo.setPosition(release2);
+    }
+
+    public void hold() {
+        this.servo.setPosition(hold);
+        this.servo.setPosition(hold2);
+
+    }
+}
+
+

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

@@ -84,10 +84,6 @@ public abstract class AbstractAuto extends LinearOpMode {
                 step.start();
             }
 
-//            // update turret and slides position
-//            PoseStorage.slidesPosition = robot.actuators.getSlides();
-//            PoseStorage.turretPosition = robot.actuators.getTurret();
-
             // while the step is running display telemetry
             step.whileRunning();
             robot.update(currentRuntime);
@@ -262,6 +258,7 @@ public abstract class AbstractAuto extends LinearOpMode {
             @Override
             public void end() {
                 robot.intake.setDcMotor(0);
+                robot.intake.setpos(Intake.Position.UP);
                 robot.arm.setDoor(Arm.DoorPosition.CLOSE);
             }
 

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

@@ -15,6 +15,8 @@ public class RedRightAuto extends AbstractAuto {
     public Trajectory scoreYellow;
     public Trajectory load1;
     public Trajectory score1;
+    public Trajectory load2;
+    public Trajectory score2;
     public Trajectory parkRobot;
 
     @Override
@@ -31,6 +33,12 @@ public class RedRightAuto extends AbstractAuto {
         Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180));
         Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180));
 
+        Pose2d lineup2 = new Pose2d(12, -14, Math.toRadians(180));
+        Pose2d pickup2 = new Pose2d(-58, -14, Math.toRadians(180));
+
+        Pose2d driveup2 = new Pose2d(12-6, -14, Math.toRadians(180));
+        Pose2d deposit2 = new Pose2d(48-6, -36-4, Math.toRadians(180));
+
         Pose2d park = new Pose2d(48, -12, Math.toRadians(180));
 
         scorePurple = robot.drive.trajectoryBuilder(start)
@@ -41,29 +49,53 @@ public class RedRightAuto extends AbstractAuto {
                 .lineToLinearHeading(depositPreload)
                 .build();
 
+        int driveSpeed = 45;
+
         load1 = robot.drive.trajectoryBuilder(scoreYellow.end())
                 .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
-                        SampleMecanumDrive.getVelocityConstraint(45, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, 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.getVelocityConstraint(driveSpeed, 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.getVelocityConstraint(driveSpeed, 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.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                         SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
                 )
                 .build();
 
-        parkRobot = robot.drive.trajectoryBuilder(score1.end())
+        load2 = robot.drive.trajectoryBuilder(score1.end())
+                .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
+                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .lineToLinearHeading(pickup1,
+                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .build();
+
+        score2 = robot.drive.trajectoryBuilder(load2.end(), true)
+                .lineToLinearHeading(driveup1,
+                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
+                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+                )
+                .build();
+
+        parkRobot = robot.drive.trajectoryBuilder(score2.end())
                 .lineToLinearHeading(park)
                 .build();
 
@@ -72,14 +104,19 @@ public class RedRightAuto extends AbstractAuto {
 
     @Override
     public void initializeSteps(int location) {
+        // score preloads
         followTrajectory(scorePurple);
         runIntake(-0.4, 0.5);
-//        followAndExtend(scoreYellow, Slides.Position.TIER1);
+        followAndExtend(scoreYellow, Slides.Position.TIER1);
-//        followAndRetract(load1, Slides.Position.DOWN);
+
-        followTrajectory(scoreYellow);
+        // cycle
-        followTrajectory(load1);
+        followAndRetract(load1, Slides.Position.DOWN);
         intakeStack(Intake.Position.STACK5, Intake.Position.STACK4);
         followAndExtend(score1, Slides.Position.TIER1);
+        followAndRetract(load2, Slides.Position.DOWN);
+        intakeStack(Intake.Position.STACK3, Intake.Position.STACK2);
+        followAndExtend(score2, Slides.Position.TIER1);
+
         followAndRetract(parkRobot, Slides.Position.DOWN);
     }
 

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

@@ -0,0 +1,64 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+
+@Autonomous (name= "varun_is_Dumb")
+public class vannobAuto_RightRed extends LinearOpMode {
+    public Robot robot;
+    int teamElementLocation = 2;
+    int macrostate = 0;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        robot = new Robot(hardwareMap);
+
+        // make trajectories
+
+        while (!(isStarted() || isStopRequested())) {
+            double currentRuntime = getRuntime();
+            robot.update(currentRuntime);
+
+//            int newLocation = robot.camera.getMarkerId();
+//            if (newLocation != -1) {
+//                teamElementLocation = newLocation;
+//            }
+
+            telemetry.addLine("Initialized");
+
+            telemetry.addLine("Randomization of T.O.M: "+teamElementLocation);
+            telemetry.addLine(robot.getTelemetry());
+            telemetry.update();
+        }
+
+        while(macrostate < 6 || !isStopRequested()){
+            //update everything
+            robot.update(getRuntime());
+
+            switch (macrostate){
+                case (0):
+                    //follow trajectory
+                    macrostate++;
+                    break;
+                case (1):
+                    // run intake
+                    macrostate++;
+                    break;
+                case (2):
+                    macrostate++;
+                    break;
+                case (3):
+                    macrostate++;
+                    break;
+                case (4):
+                    macrostate++;
+                    break;
+                case (5):
+                    macrostate++;
+                    break;
+            }
+        }
+    }
+}

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

@@ -9,7 +9,6 @@ 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.Intake;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.Slides;
 
@@ -19,7 +18,9 @@ public class MainTeleOp extends OpMode {
 
     public static double normal = 0.5;
     public static double turbo = 1;
+    public static double slow_mode = 0.15;
     public static double intakeMax = 0.65;
+    public static double intakeMax2 = -0.65;
 
     private Robot robot;
     private Controller controller1;
@@ -32,7 +33,7 @@ public class MainTeleOp extends OpMode {
 
         this.robot = new Robot(hardwareMap);
 //        robot.intake.setpos(Intake.Position.STACK1);
-
+        this.robot.endGameMechs.hold();
         while (robot.camera.getFrameCount() < 1) {
             telemetry.addLine("Initializing...");
             telemetry.update();
@@ -40,6 +41,8 @@ public class MainTeleOp extends OpMode {
 
         telemetry.addLine("Initialized");
         telemetry.update();
+
+
     }
 
     @Override
@@ -53,6 +56,11 @@ public class MainTeleOp extends OpMode {
             x *= turbo;
             y *= turbo;
             z *= turbo;
+        }
+        else if (controller1.getLeftTrigger().getValue() > 0.1) {
+            x *= slow_mode;
+            y *= slow_mode;
+            z *= slow_mode;
         } else {
             x *= normal;
             y *= normal;
@@ -61,7 +69,16 @@ public class MainTeleOp extends OpMode {
         robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
 
         // Driver 2
-        robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
+        if (controller2.getRightTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
+        }
+        else if(controller2.getLeftTrigger().getValue()>=0.1){
+            robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
+        }
+        else {
+            robot.intake.setDcMotor(0);
+        }
+
         if (controller2.getRightBumper().isJustPressed()) {
             robot.intake.incrementPos();
         }
@@ -71,10 +88,15 @@ public class MainTeleOp extends OpMode {
 
         // Drone launcher
         if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
-            this.robot.droneLauncher.launch();
+            this.robot.endGameMechs.launch();
         } else {
-            this.robot.droneLauncher.reset();
+            this.robot.endGameMechs.reset();
         }
+        //Hang Servos
+        if (controller1.getX().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
+            this.robot.endGameMechs.release();
+        }
+
 
         // macros
         switch (robot.runningMacro) {