diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 49e0671..2dfa830 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/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()); } diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index 02c6f86..b49ff00 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/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()); } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 66a82fa..9eeb5b9 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/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) { diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java index d3c4371..73e38a1 100644 --- a/TeamCode/src/main/java/opmodes/RedFrontStage.java +++ b/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()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drone.java new file mode 100644 index 0000000..93c46af --- /dev/null +++ b/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); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index c7ae108..8f0f645 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/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(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index 9efe412..ad51b64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java +++ b/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_MAX = 0.94; + public static double GANTRY_ARM_MIN = 0.42; + 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;