Claw, Gantry, and Lift

TeamCode/src/main/java/opmodes/AutoBase.java

@@ -138,8 +138,15 @@ public abstract class AutoBase extends LinearOpMode {
 
     protected void moveToBackstage() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
-        builder.lineToLinearHeading(new Pose2d(36, 11, 0));
+
-        builder.lineToLinearHeading(new Pose2d(36, 38, 0));
+        if (this.alliance == CenterStageCommon.Alliance.Blue) {
+            builder.lineToLinearHeading(new Pose2d(36, 11, 0));
+            builder.lineToLinearHeading(new Pose2d(36, 38, 0));
+        } else {
+            builder.lineToLinearHeading(new Pose2d(36, -11, 0));
+            builder.lineToLinearHeading(new Pose2d(36, -38, 0));
+        }
+
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 

TeamCode/src/main/java/opmodes/BlueFrontStage.java

@@ -63,4 +63,4 @@ public class BlueFrontStage extends AutoBase {
         builder.lineToLinearHeading(this.rendezvous);
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
-}
+}

TeamCode/src/main/java/opmodes/MainTeleOp.java

@@ -49,6 +49,9 @@ public class MainTeleOp extends OpMode {
         boolean clawDownSafe = gamepad2.dpad_down; // dpad-down
         boolean clawDown = gamepad2.a || clawDownSafe; // A
 
+        // Robot Drone Launch
+        boolean robotDroneLaunch = gamepad1.right_bumper; // Change if Merck wants (RT)
+
         // Robot Lift
         boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT
         boolean robotLiftExtend = gamepad2.right_trigger > 0.5; // RT
@@ -114,6 +117,11 @@ public class MainTeleOp extends OpMode {
         gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
         this.robot.getGantry().setX(gantryXPosition);
 
+        // Robot Drone
+
+        if (robotDroneLaunch) {
+            this.robot.getDrone().raise();
+        }
         // Robot Lift
 
         if (robotLiftRotation || this.liftArmShouldBeUp) {

TeamCode/src/main/java/opmodes/RedFrontStage.java

@@ -20,6 +20,7 @@ public class RedFrontStage extends AutoBase {
                 new Pose2d(-12, 62));
     }
 
+    // propLeft will be a reverse of BlueFrontpropRight
     protected void propLeft() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-54, -17, Math.toRadians(123)));
@@ -50,6 +51,7 @@ public class RedFrontStage extends AutoBase {
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
+    // propRight will be the reverse of BlueFrontpropRight
     protected void propRight() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180)));
@@ -60,5 +62,9 @@ public class RedFrontStage extends AutoBase {
         this.robot.getDrive().followTrajectorySequence(builder.build());
 
         openAndLiftClaw();
+
+        builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(this.rendezvous);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 }

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

@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DRONE_ROTATION_UP_NAME;
+
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class Drone {
+
+    private Servo droneUpServo;
+
+    public Drone(HardwareMap hardwareMap) {
+        this.droneUpServo = hardwareMap.get(Servo.class, DRONE_ROTATION_UP_NAME);
+    }
+
+    public void raise() {
+        this.droneUpServo.setPosition(0.2);
+
+    }
+}

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

@@ -28,6 +28,9 @@ public class Robot {
     @Getter
     private Camera camera;
 
+    @Getter
+    private Drone drone;
+
     @Getter
     CenterStageCommon.Alliance alliance;
 
@@ -68,7 +71,6 @@ public class Robot {
         this.lift.update();
         this.drive.update();
         this.claw.update();
-
         this.telemetry.update();
     }
 }

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

@@ -16,6 +16,7 @@ public class RobotConfig {
     public static final String GANTRY_X_NAME = "gantry_x";
     public static final String GANTRY_ARM_NAME = "gantryArm";
     public static final String GANTRY_SCREW_NAME = "screw";
+    public static final String DRONE_ROTATION_UP_NAME = "droneUp";
     public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation";
     public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
     public static final String WEBCAM_NAME = "webcam";
@@ -38,8 +39,8 @@ public class RobotConfig {
     public static double CLAW_KP = 0.3;
 
     // Gantry
-    public static double GANTRY_ARM_MIN = 0.435;
+    public static double GANTRY_ARM_MIN = 0.42;
-    public static double GANTRY_ARM_MAX = 0.94;
+    public static double GANTRY_ARM_MAX = 0.96;
     public static int GANTRY_LIFT_DELTA = 50;
     public static double GANTRY_ARM_KP = 0.1;
     public static double X_KP = 0.1;
@@ -50,6 +51,8 @@ public class RobotConfig {
     public static int SLIDE_UP = 820;
     public static double SLIDE_POWER = 0.5;
 
+    // Robot Drone Launch
+    public static double DRONE_ROTATION_UP = 0.2;
 
     // Robot Lift
     public static double LIFT_ROTATION_UP = 0.4;