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 36ee78c..371a31d 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 @@ -1,7 +1,8 @@ package org.firstinspires.ftc.teamcode.hardware; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMDOWN; -import static org.firstinspires.ftc.teamcode.util.Configurables.ARMUP; +import static org.firstinspires.ftc.teamcode.util.Configurables.ARMACCSCORE; +import static org.firstinspires.ftc.teamcode.util.Configurables.ARMSCORE; +import static org.firstinspires.ftc.teamcode.util.Configurables.ARMREST; import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN; import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE; import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK; @@ -30,13 +31,20 @@ import org.firstinspires.ftc.teamcode.vision.Camera; import lombok.Getter; public class Robot { - @Getter private MecanumDrive drive; - @Getter private Intake intake; - @Getter private Arm arm; - @Getter private Wrist wrist; - @Getter private Claw claw; - @Getter private Hang hang; - @Getter private Camera camera; + @Getter + private MecanumDrive drive; + @Getter + private Intake intake; + @Getter + private Arm arm; + @Getter + private Wrist wrist; + @Getter + private Claw claw; + @Getter + private Hang hang; + @Getter + private Camera camera; public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); @@ -49,7 +57,7 @@ public class Robot { return this; } - public static class Intake{ + public static class Intake { private DcMotor intake = null; public Intake init(HardwareMap hardwareMap) { @@ -59,11 +67,17 @@ public class Robot { return this; } - public void spinIn() {this.intake.setPower(0.7);} + public void spinIn() { + this.intake.setPower(0.7); + } - public void spinOut() {this.intake.setPower(-0.7);} + public void spinOut() { + this.intake.setPower(-0.7); + } - public void stop() {this.intake.setPower(0);} + public void stop() { + this.intake.setPower(0); + } } @@ -95,12 +109,12 @@ public class Robot { private Servo rightArm; - public Arm init(HardwareMap hardwareMap){ + public Arm init(HardwareMap hardwareMap) { this.leftArm = hardwareMap.get(Servo.class, LEFTARM); this.rightArm = hardwareMap.get(Servo.class, RIGHTARM); this.rightArm.setDirection(Servo.Direction.REVERSE); - this.rightArm.setPosition(ARMUP); - this.leftArm.setPosition(ARMUP); + this.rightArm.setPosition(ARMREST); + this.leftArm.setPosition(ARMREST); return this; } @@ -110,20 +124,25 @@ public class Robot { } public void armScore() { - this.leftArm.setPosition(ARMDOWN); - this.rightArm.setPosition(ARMDOWN); + this.leftArm.setPosition(ARMSCORE); + this.rightArm.setPosition(ARMSCORE); + } + + public void armAccurateScore() { + this.leftArm.setPosition(ARMACCSCORE); + this.rightArm.setPosition(ARMACCSCORE); } public void armRest() { - this.leftArm.setPosition(ARMUP); - this.rightArm.setPosition(ARMUP); + this.leftArm.setPosition(ARMREST); + this.rightArm.setPosition(ARMREST); } } public static class Wrist { private Servo wrist; - public Wrist init(HardwareMap hardwareMap){ + public Wrist init(HardwareMap hardwareMap) { this.wrist = hardwareMap.get(Servo.class, WRIST); this.wrist.setPosition(WRISTPICKUP); return this; @@ -141,8 +160,9 @@ public class Robot { public static class Claw { private Servo claw; - public Claw init (HardwareMap hardwareMap) { + public Claw init(HardwareMap hardwareMap) { this.claw = hardwareMap.get(Servo.class, CLAW); + this.claw.setPosition(CLOSE); return this; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java index a4a6c80..6e8e6e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java @@ -11,15 +11,32 @@ import org.firstinspires.ftc.teamcode.hardware.Robot; @Autonomous(name = "ThisIsTheLongestFlippingOpModeNameEverLolIamSeahorse!") public class Auto extends LinearOpMode { + protected Pose2d initialPosition; + + protected Trajectory preloadOne; + protected Trajectory scoreOne; + protected Trajectory boardOne; + protected Trajectory backOffOne; + + + protected Trajectory preloadTwo; + protected Trajectory scoreTwo; + protected Trajectory backOffTwo; + + + protected Trajectory preloadThree; + protected Trajectory scoreThree; + protected Trajectory boardThree; + protected Trajectory backOffThree; + + + protected Trajectory park1; + protected Trajectory park2; + + private Robot robot; private String randomization; private int random; - protected Pose2d initialPosition; - protected Trajectory preloadOne; - protected Trajectory scoreOne; - protected Trajectory preloadTwo; - protected Trajectory preloadThree; - @Override public void runOpMode() throws InterruptedException { @@ -27,27 +44,68 @@ public class Auto extends LinearOpMode { this.robot = new Robot().init(hardwareMap); this.robot.getCamera().initTargetingCamera(); - //Trajectories + //Trajectories this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); - //Preload One - this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270))) - .build(); + //Randomization One + this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270))) + .build(); this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end()) - .lineToLinearHeading(new Pose2d(33,-30, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(33, -30, Math.toRadians(360))) .build(); - //Preload Two + this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) + .lineToLinearHeading(new Pose2d(72, -26, Math.toRadians(360))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) + .lineToLinearHeading(new Pose2d(60, -26, Math.toRadians(360))) + .build(); + + + //Randomization Two this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(35,-27, Math.toRadians(270))) + .lineToLinearHeading(new Pose2d(35, -27, Math.toRadians(270))) .build(); - //Preload Three + this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) + .lineToLinearHeading(new Pose2d(72, -34, Math.toRadians(360))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) + .lineToLinearHeading(new Pose2d(60, -34, Math.toRadians(360))) + .build(); + + //Randomization Three this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(46,-35, Math.toRadians(270))) + .lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270))) + .build(); + + this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) + .lineToLinearHeading(new Pose2d(72, -42, Math.toRadians(360))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) + .lineToLinearHeading(new Pose2d(60, -40, Math.toRadians(360))) + .build(); + + //Park + this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) + .lineToLinearHeading(new Pose2d(65, -10, Math.toRadians(360))) + .addTemporalMarker(.3, robot.getArm()::armRest) + .addTemporalMarker(.3, robot.getWrist()::wristPickup) + .build(); + this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) + .lineToLinearHeading(new Pose2d(80, -10, Math.toRadians(360))) .build(); // Do super fancy chinese shit @@ -61,17 +119,37 @@ public class Auto extends LinearOpMode { case "LEFT": this.robot.getDrive().followTrajectory(preloadOne); this.robot.getDrive().followTrajectory(scoreOne); + this.robot.getDrive().followTrajectory(boardOne); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffOne); + sleep(500); + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); break; case "CENTER": this.robot.getDrive().followTrajectory(preloadTwo); + this.robot.getDrive().followTrajectory(scoreTwo); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffTwo); + sleep(500); + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); break; case "RIGHT": this.robot.getDrive().followTrajectory(preloadThree); + this.robot.getDrive().followTrajectory(scoreThree); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffThree); + sleep(500); + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); break; } - - } - } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index caa8e17..4d4fc06 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -21,6 +21,7 @@ public class MainTeleOp extends OpMode { boolean restArm = gamepad2.dpad_left || gamepad2.x; boolean pickupArm = gamepad2.dpad_down; boolean scoreArm = gamepad2.dpad_right || gamepad2.a; + boolean accurateScoreArm = gamepad2.y; boolean claw = gamepad2.b; boolean pickupWrist = gamepad2.left_bumper || gamepad2.x; boolean scoreWrist = gamepad2.right_bumper || gamepad2.a; @@ -37,6 +38,8 @@ public class MainTeleOp extends OpMode { this.robot.getArm().armRest(); } else if (scoreArm) { this.robot.getArm().armScore(); + } else if (accurateScoreArm) { + this.robot.getArm().armAccurateScore(); } //Claw if (claw) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index ce3ad05..613b5f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -18,8 +18,9 @@ public class Configurables { public static double LOCKSPEED = .25; public static double UNLOCK = .6; public static double LOCK = 0.4; - public static double ARMUP = .88; - public static double ARMDOWN = 0.15; + public static double ARMREST = .88; + public static double ARMSCORE = 0.15; + public static double ARMACCSCORE = 0.02; public static double PICKUP = .935; public static double WRISTPICKUP = 0.28; public static double WRISTSCORE = .96;