From 712fd3e0a578a966c42bbf9b47c96fa877896dbb Mon Sep 17 00:00:00 2001 From: sihan Date: Sat, 17 Feb 2024 10:48:33 -0600 Subject: [PATCH] final robot code for Ostrick O7 --- .../ftc/teamcode/hardware/Robot.java | 29 +- .../roadrunner/drive/DriveConstants.java | 6 +- .../ftc/teamcode/opmodes/AutoBlue.java | 54 ++-- .../ftc/teamcode/opmodes/AutoBlueFar.java | 69 ----- .../ftc/teamcode/opmodes/AutoBlueFarStem.java | 159 +++++++++++ .../teamcode/opmodes/AutoBlueTwoPlusTwo.java | 91 ++++--- .../ftc/teamcode/opmodes/AutoRed.java | 55 ++-- .../ftc/teamcode/opmodes/AutoRedFar.java | 131 +++++++-- .../opmodes/AutoRedFarTwoPlusTwo.java | 252 ++++++++++++++++++ .../opmodes/AutoRedFaronepluszero.java | 170 ++++++++++++ .../teamcode/opmodes/AutoRedTwoPlusTwo.java | 71 +++-- .../ftc/teamcode/opmodes/MainTeleOp.java | 4 +- .../ftc/teamcode/util/Configurables.java | 25 +- .../ftc/teamcode/util/Constants.java | 2 + 14 files changed, 914 insertions(+), 204 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java 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 56e1bcc..e48a811 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 @@ -15,6 +15,8 @@ import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELAUNCH; import static org.firstinspires.ftc.teamcode.util.Configurables.PLANELOCK; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEAUTOSTACKS; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERONE; +import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDELAYERTWO; +import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEPICKUPSTACKSTWO; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDEUP; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_DOWN; import static org.firstinspires.ftc.teamcode.util.Configurables.SLIDE_POWER_UP; @@ -24,6 +26,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.CLAW; import static org.firstinspires.ftc.teamcode.util.Constants.HANGLEFT; import static org.firstinspires.ftc.teamcode.util.Constants.HANGRIGHT; import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM; +import static org.firstinspires.ftc.teamcode.util.Constants.LIGHTS; import static org.firstinspires.ftc.teamcode.util.Constants.PLANE; import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM; import static org.firstinspires.ftc.teamcode.util.Constants.SLIDELEFT; @@ -31,6 +34,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.SLIDERIGHT; import static org.firstinspires.ftc.teamcode.util.Constants.WRIST; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -45,8 +49,8 @@ import lombok.Getter; public class Robot { @Getter private MecanumDrive drive; -// @Getter -// private Intake intake; + @Getter + private Led led; @Getter private Arm arm; @Getter @@ -71,6 +75,7 @@ public class Robot { this.camera = new Camera(hardwareMap); this.plane = new Plane().init(hardwareMap); this.slides= new Slides().init(hardwareMap); + this.led = new Led().init(hardwareMap); return this; } @@ -112,8 +117,12 @@ public class Robot { public void slideFirstLayer(){this.slideTo(SLIDELAYERONE, SLIDE_POWER_UP);} + public void slideScoreStack(){this.slideTo(SLIDELAYERTWO, SLIDE_POWER_UP);} + public void slideAutoStacks(){this.slideTo(SLIDEAUTOSTACKS, SLIDE_POWER_UP);} + public void slidePickupStackTwo(){this.slideTo(SLIDEPICKUPSTACKSTWO, SLIDE_POWER_UP);} + public void slideStop() {this.slideTo(slidesRight.getCurrentPosition(), 1.0);} } @@ -258,6 +267,22 @@ public class Robot { } + public static class Led { + private RevBlinkinLedDriver led; + + public Led init(HardwareMap hardwareMap) { + this.led = hardwareMap.get(RevBlinkinLedDriver.class, LIGHTS); + return this; + } + + public void gold() { + this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_VIOLET); + } + + public void white() { + this.led.setPattern(RevBlinkinLedDriver.BlinkinPattern.STROBE_WHITE); + } + } public static class Plane { private Servo plane; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index b811e28..c9b3922 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -57,7 +57,7 @@ public class DriveConstants { * motor encoders or have elected not to use them for velocity control, these values should be * empirically tuned. */ - public static double kV = 1.0 / rpmToVelocity(MAX_RPM); + public static double kV = 1 / rpmToVelocity(MAX_RPM); public static double kA = 0; public static double kStatic = 0; @@ -68,8 +68,8 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 40; - public static double MAX_ACCEL = 40; + public static double MAX_VEL = 50; + public static double MAX_ACCEL = 50; public static double MAX_ANG_VEL = Math.toRadians(60); public static double MAX_ANG_ACCEL = Math.toRadians(60); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java index c784ced..702b526 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java @@ -19,51 +19,38 @@ public class AutoBlue extends LinearOpMode { //Pose2ds //Preloads - final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180)); + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(220)); final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270)); //Board Scores - final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185)); - final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185)); - final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185)); - //Park - final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180)); - final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180)); + final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -26.5, Math.toRadians(180)); + final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(178)); + final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(180)); - protected void scorePreloadOne() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - switch (randomization) { - case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); - builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); - break; - case "CENTER": - builder.lineToLinearHeading(CENTER_PRELOAD); - break; - case "LEFT": - builder.lineToLinearHeading(LEFT_PRELOAD); - break; - } - this.robot.getDrive().followTrajectorySequence(builder.build()); - } + final static Pose2d BACK_OFF = new Pose2d(-68, -55,Math.toRadians(180)); + final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180)); protected void boardScore() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); builder.lineToLinearHeading(RIGHT_BOARD); break; case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); builder.lineToLinearHeading(CENTER_BOARD); break; case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); builder.lineToLinearHeading(LEFT_BOARD); break; } - builder.addTemporalMarker(.2, robot.getArm()::armScore); - builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); - builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + builder.addTemporalMarker(.75, robot.getArm()::armScore); + builder.addTemporalMarker(2, robot.getSlides()::slideFirstLayer); + builder.addTemporalMarker(2, robot.getWrist()::wristScore); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -88,23 +75,18 @@ public class AutoBlue extends LinearOpMode { this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); -// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) -// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) -// .build(); - // Do super fancy chinese shit while (!this.isStarted()) { this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); this.telemetry.update(); } - - scorePreloadOne(); boardScore(); - sleep(250); + sleep(100); this.robot.getClaw().open(); - sleep(250); + sleep(100); park(); + sleep(300); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java deleted file mode 100644 index 392734d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java +++ /dev/null @@ -1,69 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Config -@Autonomous(name = "autoBlueFar") -public class AutoBlueFar extends LinearOpMode { - protected Pose2d initialPosition; - private Robot robot; - private String randomization; - - //Pose2ds - //Preloads - final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360)); - final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); - - protected void scorePreloadOne() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(LEFT_PRELOAD_ONE); - builder.lineToLinearHeading(LEFT_PRELOAD_TWO); - break; - case "CENTER": - builder.lineToLinearHeading(CENTER_PRELOAD); - break; - case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD); - break; - } - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void goBackToWhereYouCameFrom() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(initialPosition); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - @Override - public void runOpMode() throws InterruptedException { - - this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - this.robot = new Robot().init(hardwareMap); - this.robot.getCamera().initTargetingCamera(); - this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); - this.robot.getDrive().setPoseEstimate(initialPosition); - - // Do super fancy chinese shit - while (!this.isStarted()) { - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); - randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); - this.telemetry.update(); - } - - scorePreloadOne(); - goBackToWhereYouCameFrom(); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java new file mode 100644 index 0000000..c282bb5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java @@ -0,0 +1,159 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; + +@Config +@Autonomous(name = "autoBlueFar") +public class AutoBlueFarStem extends LinearOpMode { + protected Pose2d initialPosition; + private Robot robot; + private String randomization; + private String parkLocation; + private int delay = 10000; + + //Pose2ds + //Preloads + final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360)); + final static Pose2d LEFT_PRELOAD_GETOUT = new Pose2d(43, -42, Math.toRadians(330)); + final static Pose2d CENTER_PRELOAD = new Pose2d(35, -27, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(44, -35, Math.toRadians(270)); + //Ready Truss + final static Pose2d READY_TRUSS = new Pose2d(43, -57, Math.toRadians(180)); + final static Pose2d READY_TRUSSTEMP = new Pose2d(35, -57, Math.toRadians(180)); + final static Pose2d TO_BOARD = new Pose2d(-35, -57, Math.toRadians(180)); + final static Pose2d SCORE_BOARD_LEFT = new Pose2d(-52, -38, Math.toRadians(180)); + final static Pose2d SCORE_BOARD_MID = new Pose2d(-52, -32, Math.toRadians(180)); + final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(-52, -27, Math.toRadians(180)); + final static Pose2d PARK = new Pose2d(-55, -56, Math.toRadians(180)); + final static Pose2d PARK2 = new Pose2d(-60, -59, Math.toRadians(180)); + final static Pose2d PARKLEFT = new Pose2d(-45, -10, Math.toRadians(180)); + final static Pose2d PARKLEFT2 = new Pose2d(-60, -10, Math.toRadians(180)); + + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD_ONE); + builder.lineToLinearHeading(LEFT_PRELOAD_TWO); + builder.lineToLinearHeading(LEFT_PRELOAD_GETOUT); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD); + break; + } + builder.addTemporalMarker(.3, this.robot.getArm()::armScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackToWhereYouCameFrom() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_TRUSSTEMP); +// builder.lineToLinearHeading(TO_BOARD, +// MecanumDrive.getVelocityConstraint(70, 70, DriveConstants.TRACK_WIDTH), +// MecanumDrive.getAccelerationConstraint(70)); + builder.addTemporalMarker(.3,robot.getArm()::armRest); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + +// protected void scoreBoard(){ +// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); +// switch (randomization) { +// case "LEFT": +// builder.lineToLinearHeading(SCORE_BOARD_LEFT, +// MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), +// MecanumDrive.getAccelerationConstraint(20)); +// break; +// case "CENTER": +// builder.lineToLinearHeading(SCORE_BOARD_MID, +// MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), +// MecanumDrive.getAccelerationConstraint(20)); +// break; +// case "RIGHT": +// builder.lineToLinearHeading(SCORE_BOARD_RIGHT, +// MecanumDrive.getVelocityConstraint(30, 30, DriveConstants.TRACK_WIDTH), +// MecanumDrive.getAccelerationConstraint(30)); +// break; +// } +// builder.addTemporalMarker(.2, robot.getArm()::armScore); +// builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); +// builder.addTemporalMarker(.2, robot.getWrist()::wristScore); +// this.robot.getDrive().followTrajectorySequence(builder.build()); +// } +// +// protected void park(){ +// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); +// switch(parkLocation) { +// case "RIGHT": +// builder.lineToLinearHeading(PARKLEFT); +// builder.lineToLinearHeading(PARKLEFT2); +// break; +// case "LEFT": +// builder.lineToLinearHeading(PARK); +// builder.lineToLinearHeading(PARK2); +// break; +// } +// builder.addTemporalMarker(.1, robot.getArm()::armRest); +// builder.addTemporalMarker(.1, robot.getClaw()::close); +// builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); +// builder.addTemporalMarker(.1, robot.getSlides()::slideDown); +// this.robot.getDrive().followTrajectorySequence(builder.build()); +// +// } + + protected void delaySet(){ + if (gamepad2.dpad_up && delay<13000) { + delay+=1000; + sleep(100); + } else if (gamepad2.dpad_down && delay>0) { + delay-=1000; + sleep(100); + } + } + + @Override + public void runOpMode() throws InterruptedException { + + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + // Do super fancy chinese shit + while (!this.isStarted()) { + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); + randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + if (gamepad2.dpad_left) { + parkLocation="LEFT"; + } else if (gamepad2.dpad_right) { + parkLocation = "RIGHT"; + } + delaySet(); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Delay", delay); + this.telemetry.update(); + } + sleep(delay); + scorePreloadOne(); + goBackToWhereYouCameFrom(); +// scoreBoard(); +// sleep(500); +// this.robot.getClaw().open(); +// sleep(500); +// park(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java index 4d95360..fea0bde 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java @@ -21,23 +21,25 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { //Pose2ds //Preloads - final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(180)); - final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(210)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-30.5, -31, Math.toRadians(210)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26.8, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270)); //Board Scores - final static Pose2d RIGHT_BOARD = new Pose2d(-75.7, -25.7, Math.toRadians(185)); - final static Pose2d CENTER_BOARD = new Pose2d(-75.7, -35, Math.toRadians(185)); - final static Pose2d LEFT_BOARD = new Pose2d(-75.7, -42, Math.toRadians(185)); + final static Pose2d RIGHT_BOARD = new Pose2d(-75.5, -26.5, Math.toRadians(182)); + final static Pose2d CENTER_BOARD = new Pose2d(-77, -33.5, Math.toRadians(185)); + final static Pose2d LEFT_BOARD = new Pose2d(-76, -41, Math.toRadians(185)); //Stack Cycle final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180)); - final static Pose2d TO_STACK = new Pose2d(40.75, -7.25, Math.toRadians(180)); + final static Pose2d TO_STACK = new Pose2d(35, -6.5, Math.toRadians(180)); + final static Pose2d TO_STACK_SLOW = new Pose2d(40, -7.5, Math.toRadians(180)); + final static Pose2d TO_STACK_SLOW2 = new Pose2d(40, -8.5, Math.toRadians(183)); final static Pose2d BACK_THROUGH_GATE = new Pose2d(-50, -10, Math.toRadians(180)); final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -28, Math.toRadians(180)); - final static Pose2d SCORE_STACK = new Pose2d(-73, -31, Math.toRadians(180)); + final static Pose2d SCORE_STACK = new Pose2d(-73.5, -29, Math.toRadians(180)); //Park - final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180)); - final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180)); + final static Pose2d BACK_OFF = new Pose2d(-68, -55,Math.toRadians(180)); + final static Pose2d PARK = new Pose2d(-80, -64, Math.toRadians(180)); protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); @@ -53,20 +55,27 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(LEFT_PRELOAD); break; } + builder.addTemporalMarker(.5, robot.getArm()::armScore); this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void boardScore() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { - case "RIGHT": - builder.lineToLinearHeading(RIGHT_BOARD); + case "LEFT": + builder.lineToLinearHeading(LEFT_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20));; break; case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD); + builder.lineToLinearHeading(CENTER_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; - case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD); + case "RIGHT": + builder.lineToLinearHeading(RIGHT_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; } builder.addTemporalMarker(.2, robot.getArm()::armScore); @@ -84,6 +93,24 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { builder.addTemporalMarker(1.5,robot.getClaw()::openStack); builder.addTemporalMarker(1.5, robot.getArm()::pickup); builder.lineToLinearHeading(TO_STACK); + builder.lineToLinearHeading(TO_STACK_SLOW, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void toStackNoDrift() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE_BOARD); + builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.addTemporalMarker(1.5, robot.getArm()::pickup); + builder.lineToLinearHeading(TO_STACK); + builder.lineToLinearHeading(TO_STACK_SLOW2, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -94,13 +121,13 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(SCORE_STACK); builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore); builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); - builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(2.5, robot.getSlides()::slideScoreStack); this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void scoreTest() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-77, -31, Math.toRadians(180)), + builder.lineToLinearHeading(new Pose2d(-77.5, -31, Math.toRadians(183)), MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(20)); builder.addTemporalMarker(.2,this::clawSlowOpen); @@ -108,14 +135,12 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { } protected void clawSlowOpen() { - double currentPos = .86; - double targetPos = .8; + double currentPos = .8; + double targetPos = .73; double delta = (targetPos - currentPos) / 100; for (int i = 0; i < 100; i++) { -// int Position = this.robot.getSlides().slidesLeft.getCurrentPosition(); this.robot.getClaw().setPos(currentPos + (delta * i)); -// this.robot.getSlides().slideTo(Position + 14,1); - sleep(35); + sleep(30); } } @@ -140,32 +165,38 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); -// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) -// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) -// .build(); - // Do super fancy chinese shit while (!this.isStarted()) { this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); this.telemetry.update(); } + scorePreloadOne(); boardScore(); sleep(100); this.robot.getClaw().open(); - toStack(); + switch (randomization) { + case "RIGHT": + toStackNoDrift(); + break; + case "CENTER": + toStack(); + break; + case "LEFT": + toStack(); + break; + } sleep(500); this.robot.getClaw().close(); sleep(250); this.robot.getArm().armRest(); scoreStack(); - this.robot.getClaw().setPos(.86); + this.robot.getClaw().setPos(.83); scoreTest(); park(); } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index 1b3d7e5..faad40f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java @@ -8,6 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @Config @@ -16,21 +18,24 @@ public class AutoRed extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; private String randomization; + private String parkLocation; //Pose2ds //Preloads final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360)); + final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360)); final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -35, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355)); - final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355)); - final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -40, Math.toRadians(355)); + final static Pose2d LEFT_BOARD = new Pose2d(75.8, -26.5, Math.toRadians(358)); + final static Pose2d CENTER_BOARD = new Pose2d(75.8, -36.3, Math.toRadians(358)); + final static Pose2d RIGHT_BOARD = new Pose2d(75.8, -40, Math.toRadians(358)); //Park - final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360)); - final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360)); + final static Pose2d PARK = new Pose2d(60,-58,Math.toRadians(360)); + final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360)); + final static Pose2d PARKLEFT = new Pose2d(60,-15,Math.toRadians(360)); + final static Pose2d PARKLEFT2 = new Pose2d(80, -10, Math.toRadians(360)); protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); @@ -53,13 +58,19 @@ public class AutoRed extends LinearOpMode { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD); + builder.lineToLinearHeading(LEFT_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD); + builder.lineToLinearHeading(CENTER_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; case "RIGHT": - builder.lineToLinearHeading(RIGHT_BOARD); + builder.lineToLinearHeading(RIGHT_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; } builder.addTemporalMarker(.2, robot.getArm()::armScore); @@ -70,14 +81,28 @@ public class AutoRed extends LinearOpMode { protected void park() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(BACK_OFF); - builder.lineToLinearHeading(PARK); + switch(parkLocation) { + case "LEFT": + builder.lineToLinearHeading(PARKLEFT); + builder.lineToLinearHeading(PARKLEFT2); + break; + case "RIGHT": + builder.lineToLinearHeading(PARK); + builder.lineToLinearHeading(PARK2); + break; + } builder.addTemporalMarker(.1, robot.getArm()::armRest); builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); builder.addTemporalMarker(.1, robot.getSlides()::slideDown); this.robot.getDrive().followTrajectorySequence(builder.build()); } + protected void parkLocation(){ + if (gamepad2.dpad_left) { + parkLocation="LEFT"; + } else if (gamepad2.dpad_right) { + parkLocation = "RIGHT"; + }} @Override public void runOpMode() throws InterruptedException { @@ -88,14 +113,12 @@ public class AutoRed extends LinearOpMode { this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); -// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) -// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) -// .build(); - // Do super fancy chinese shit while (!this.isStarted()) { this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + parkLocation(); + this.telemetry.addData("Park Position", parkLocation); this.telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java index 78ea576..e4a9276 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java @@ -8,67 +8,164 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @Config -@Autonomous(name = "autRedFar") +@Autonomous(name = "AutoRedFar") public class AutoRedFar extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; private String randomization; + private String parkLocation; + private int delay = 10000; //Pose2ds //Preloads - final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180)); - final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200)); + final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(230)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270)); + //Ready Truss + final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270)); + final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0)); + final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0)); + final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0)); + final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5)); + final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0)); + final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, Math.toRadians(0)); + final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0)); + final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0)); + final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0)); + final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0)); protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { - case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); - builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); break; case "CENTER": builder.lineToLinearHeading(CENTER_PRELOAD); break; - case "LEFT": - builder.lineToLinearHeading(LEFT_PRELOAD); + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT); break; } + builder.addTemporalMarker(.4,robot.getArm()::armScore); this.robot.getDrive().followTrajectorySequence(builder.build()); } + + protected void goBackToWhereYouCameFrom() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(initialPosition); + builder.lineToLinearHeading(LEAVE); + builder.lineToLinearHeading(READY_TRUSS); + builder.lineToLinearHeading(TO_BOARD); + builder.addTemporalMarker(.3,robot.getArm()::armRest); this.robot.getDrive().followTrajectorySequence(builder.build()); } + protected void scoreBoard(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_SCORE); + + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void score(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(SCORE_BOARD_LEFT, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "CENTER": + builder.lineToLinearHeading(SCORE_BOARD_MID, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "RIGHT": + builder.lineToLinearHeading(SCORE_BOARD_RIGHT, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + } + builder.addTemporalMarker(1.8, robot.getClaw()::open); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void park(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch(parkLocation) { + case "LEFT": + builder.lineToLinearHeading(PARKLEFT); + builder.lineToLinearHeading(PARKLEFT2); + break; + case "RIGHT": + builder.lineToLinearHeading(PARK); + builder.lineToLinearHeading(PARK2); + break; + } + builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + } + + protected void parkLocation(){ + if (gamepad2.dpad_left) { + parkLocation="LEFT"; + } else if (gamepad2.dpad_right) { + parkLocation = "RIGHT"; + }} + + protected void delaySet(){ + if (gamepad2.dpad_up && delay<13000) { + delay+=1000; + sleep(100); + } else if (gamepad2.dpad_down && delay>0) { + delay-=1000; + sleep(100); + } + } + @Override public void runOpMode() throws InterruptedException { - this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); this.robot = new Robot().init(hardwareMap); this.robot.getCamera().initTargetingCamera(); this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); -// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) -// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) -// .build(); - // Do super fancy chinese shit while (!this.isStarted()) { + parkLocation(); + delaySet(); this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Delay", delay); this.telemetry.update(); - } + } + sleep(delay); scorePreloadOne(); goBackToWhereYouCameFrom(); + scoreBoard(); + score(); + sleep(500); + park(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java new file mode 100644 index 0000000..72d165a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java @@ -0,0 +1,252 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; + +@Config +@Autonomous(name = "AutoRedFar2+2") +public class AutoRedFarTwoPlusTwo extends LinearOpMode { + protected Pose2d initialPosition; + private Robot robot; + private String randomization; + private String parkLocation; + private int delay = 10000; + + //Pose2ds + //Preloads + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200)); + final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(0)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270)); + //Ready Truss + final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270)); + final static Pose2d READY_TRUSS = new Pose2d(-43, -56.5, Math.toRadians(0)); + final static Pose2d TO_BOARD = new Pose2d(26, -56.5, Math.toRadians(0)); + final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0)); + final static Pose2d SCORE_BOARD_LEFT = new Pose2d(55, -27, Math.toRadians(5)); + final static Pose2d SCORE_BOARD_MID = new Pose2d(55.5, -32.2, Math.toRadians(5)); + final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(55, -39, Math.toRadians(5)); + final static Pose2d GET_STACK = new Pose2d(-47, -30, Math.toRadians(0)); + final static Pose2d PICKUP_STACK = new Pose2d(-61.5, -30.5, Math.toRadians(0)); + final static Pose2d PICKUP_STACK_MID = new Pose2d(-61.5, -32.5, Math.toRadians(0)); + final static Pose2d READY_SCORESTACK = new Pose2d(50, -41, Math.toRadians(0)); + final static Pose2d PICKUP_STACK_LEFT = new Pose2d(-60.5, -33, Math.toRadians(0)); + final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0)); + final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0)); + final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0)); + final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + break; + } + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackToWhereYouCameFrom() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE, + MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(90)); + builder.lineToLinearHeading(READY_TRUSS, + MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(90)); + builder.lineToLinearHeading(TO_BOARD, + MecanumDrive.getVelocityConstraint(90, Math.toRadians(90), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreBoard(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_SCORE); + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void score(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(SCORE_BOARD_LEFT, + MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + case "CENTER": + builder.lineToLinearHeading(SCORE_BOARD_MID, + MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "RIGHT": + builder.lineToLinearHeading(SCORE_BOARD_RIGHT, + MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + } + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void backTruss(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(TO_BOARD); + builder.lineToLinearHeading(READY_TRUSS, + MecanumDrive.getVelocityConstraint(80, Math.toRadians(80), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(80)); + builder.lineToLinearHeading(GET_STACK); + switch (randomization){ + case "MID": + builder.lineToLinearHeading(PICKUP_STACK_MID, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "RIGHT": + builder.lineToLinearHeading(PICKUP_STACK, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + default: + builder.lineToLinearHeading(PICKUP_STACK_LEFT, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + } + + builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.addTemporalMarker(1.5, robot.getArm()::armPickupStack); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackstage() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_TRUSS); + builder.lineToLinearHeading(TO_BOARD); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreBoardStack(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_SCORESTACK); + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreTest() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(55, -41, Math.toRadians(0)), + MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + builder.addTemporalMarker(.3,this::clawSlowOpen); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void clawSlowOpen() { + double currentPos = .81; + double targetPos = .73; + double delta = (targetPos - currentPos) / 100; + for (int i = 0; i < 100; i++) { + this.robot.getClaw().setPos(currentPos + (delta * i)); + sleep(30); + } + } + + + protected void park(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch(parkLocation) { + case "LEFT": + builder.lineToLinearHeading(PARKLEFT); + builder.lineToLinearHeading(PARKLEFT2); + break; + case "RIGHT": + builder.lineToLinearHeading(PARK); + builder.lineToLinearHeading(PARK2); + break; + } + builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + } + + protected void parkLocation(){ + if (gamepad2.dpad_left) { + parkLocation="LEFT"; + } else if (gamepad2.dpad_right) { + parkLocation = "RIGHT"; + }} + + protected void delaySet(){ + if (gamepad2.dpad_up && delay<13000) { + delay+=1000; + sleep(100); + } else if (gamepad2.dpad_down && delay>0) { + delay-=1000; + sleep(100); + } + } + + @Override + public void runOpMode() throws InterruptedException { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + // Do super fancy chinese shit + while (!this.isStarted()) { + parkLocation = "RIGHT"; + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Delay", delay); + this.telemetry.update(); + + } + scorePreloadOne(); + goBackToWhereYouCameFrom(); +// scoreBoard(); + score(); + this.robot.getClaw().open(); + backTruss(); + this.robot.getClaw().close(); + sleep(200); + goBackstage(); + scoreBoardStack(); + scoreTest(); + park(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java new file mode 100644 index 0000000..817a494 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java @@ -0,0 +1,170 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; + +@Config +@Autonomous(name = "AutoRedFar1+0") +public class AutoRedFaronepluszero extends LinearOpMode { + protected Pose2d initialPosition; + private Robot robot; + private String randomization; + private String parkLocation; + private int delay = 10000; + + //Pose2ds + //Preloads + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200)); + final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(230)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270)); + //Ready Truss + final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270)); + final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0)); + final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0)); + final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0)); + final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5)); + final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0)); + final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, Math.toRadians(0)); + final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0)); + final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0)); + final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0)); + final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT); + break; + } + builder.addTemporalMarker(.4,robot.getArm()::armScore); +// builder.addTemporalMarker(2,robot.getArm()::armRest); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackToWhereYouCameFrom() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE); +// builder.lineToLinearHeading(READY_TRUSS); +// builder.lineToLinearHeading(TO_BOARD); + builder.addTemporalMarker(.3,robot.getArm()::armRest); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreBoard(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(READY_SCORE); + + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void score(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(SCORE_BOARD_LEFT, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "CENTER": + builder.lineToLinearHeading(SCORE_BOARD_MID, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + case "RIGHT": + builder.lineToLinearHeading(SCORE_BOARD_RIGHT, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + break; + } + builder.addTemporalMarker(1.8, robot.getClaw()::open); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void park(){ + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch(parkLocation) { + case "LEFT": + builder.lineToLinearHeading(PARKLEFT); + builder.lineToLinearHeading(PARKLEFT2); + break; + case "RIGHT": + builder.lineToLinearHeading(PARK); + builder.lineToLinearHeading(PARK2); + break; + } + builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + } + + protected void parkLocation(){ + if (gamepad2.dpad_left) { + parkLocation="LEFT"; + } else if (gamepad2.dpad_right) { + parkLocation = "RIGHT"; + }} + + protected void delaySet(){ + if (gamepad2.dpad_up && delay<13000) { + delay+=1000; + sleep(100); + } else if (gamepad2.dpad_down && delay>0) { + delay-=1000; + sleep(100); + } + } + + @Override + public void runOpMode() throws InterruptedException { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + // Do super fancy chinese shit + while (!this.isStarted()) { + parkLocation(); + delaySet(); + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Delay", delay); + this.telemetry.update(); + + } +// sleep(delay); + scorePreloadOne(); + goBackToWhereYouCameFrom(); +// scoreBoard(); +// score(); +// sleep(500); +// park(); + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java index 5524adb..8983e34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java @@ -22,15 +22,17 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { //Preloads final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270)); final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360)); - final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270)); + final static Pose2d CENTER_PRELOAD = new Pose2d(34, -28, Math.toRadians(270)); final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(75.3, -26.5, Math.toRadians(360)); - final static Pose2d CENTER_BOARD = new Pose2d(75.3, -36.3, Math.toRadians(360)); - final static Pose2d RIGHT_BOARD = new Pose2d(75.3, -40, Math.toRadians(355)); + final static Pose2d LEFT_BOARD = new Pose2d(76.7, -27, Math.toRadians(360)); + final static Pose2d CENTER_BOARD = new Pose2d(76.5, -32, Math.toRadians(355)); + final static Pose2d RIGHT_BOARD = new Pose2d(76.5, -40, Math.toRadians(355)); //Stack Cycle final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360)); - final static Pose2d TO_STACK = new Pose2d(-40, -8.4, Math.toRadians(360)); + final static Pose2d TO_STACK = new Pose2d(-35, -8, Math.toRadians(360)); + final static Pose2d TO_STACK_SLOW = new Pose2d(-38.5, -8.5, Math.toRadians(360)); + final static Pose2d TO_STACK_SLOW2 = new Pose2d(-38.5, -8, Math.toRadians(360)); final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360)); final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360)); final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360)); @@ -52,6 +54,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(RIGHT_PRELOAD); break; } + builder.addTemporalMarker(.5, robot.getArm()::armScore); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -59,13 +62,19 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD); + builder.lineToLinearHeading(LEFT_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20));; break; case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD); + builder.lineToLinearHeading(CENTER_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; case "RIGHT": - builder.lineToLinearHeading(RIGHT_BOARD); + builder.lineToLinearHeading(RIGHT_BOARD, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); break; } builder.addTemporalMarker(.2, robot.getArm()::armScore); @@ -83,6 +92,24 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.addTemporalMarker(1.5,robot.getClaw()::openStack); builder.addTemporalMarker(1.5, robot.getArm()::pickup); builder.lineToLinearHeading(TO_STACK); + builder.lineToLinearHeading(TO_STACK_SLOW, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void toStackNoDrift() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE_BOARD); + builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.addTemporalMarker(1.5, robot.getArm()::pickup); + builder.lineToLinearHeading(TO_STACK); + builder.lineToLinearHeading(TO_STACK_SLOW2, + MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -93,7 +120,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(SCORE_STACK); builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore); builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); - builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(2.5, robot.getSlides()::slideScoreStack); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -107,8 +134,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { } protected void clawSlowOpen() { - double currentPos = .86; - double targetPos = .78; + double currentPos = .8; + double targetPos = .7; double delta = (targetPos - currentPos) / 100; for (int i = 0; i < 100; i++) { this.robot.getClaw().setPos(currentPos + (delta * i)); @@ -121,6 +148,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(BACK_OFF); builder.lineToLinearHeading(PARK); builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getClaw()::close); builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); builder.addTemporalMarker(.1, robot.getSlides()::slideDown); this.robot.getDrive().followTrajectorySequence(builder.build()); @@ -136,10 +164,6 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); -// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) -// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) -// .build(); - // Do super fancy chinese shit while (!this.isStarted()) { this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); @@ -150,17 +174,28 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { scorePreloadOne(); boardScore(); - sleep(100); + sleep(150); this.robot.getClaw().open(); + sleep(150); - toStack(); + switch (randomization) { + case "LEFT": + toStackNoDrift(); + break; + case "CENTER": + toStack(); + break; + case "RIGHT": + toStack(); + break; + } sleep(500); this.robot.getClaw().close(); sleep(250); this.robot.getArm().armRest(); scoreStack(); - this.robot.getClaw().setPos(.86); + this.robot.getClaw().setPos(.83); scoreTest(); park(); } 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 aa96aa7..1d32585 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 @@ -45,13 +45,15 @@ public class MainTeleOp extends OpMode { } else if (restArm) { this.robot.getArm().armRest(); } else if (scoreArm) { - this.robot.getArm().armScore(); + this.robot.getArm().armSecondaryScore(); } //Claw if (claw) { this.robot.getClaw().open(); + this.robot.getLed().white(); } else { this.robot.getClaw().close(); + this.robot.getLed().gold(); } //Wrist if (pickupWrist) { 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 066e999..6fb30a1 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 @@ -15,18 +15,17 @@ public class Configurables { //Servo Positions public static double ARMREST = 0.8; - public static double ARMSCORE = 0.39; - public static double ARMACCSCORE = .38; - public static double ARMPICKUPSTACK = 0.8415; - public static double PICKUP = 0.84; + public static double ARMSCORE = 0.4; + public static double ARMACCSCORE = .37; + public static double ARMPICKUPSTACK = 0.815; + public static double PICKUP = 0.835; public static double WRISTPICKUP = 0.28; public static double WRISTSCORE = .96; - public static double OPEN = 0.85; - public static double BIGOPEN = 0.67; - public static double CLOSE = 0.95; + public static double OPEN = 0.82; + public static double BIGOPEN = 0.65; + public static double CLOSE = 0.91; public static double PLANELOCK = 0.1; public static double PLANELAUNCH = 0.9; - public static double OPENSTAGEONE = .78; //Drive Speed public static double SPEED = 1; @@ -37,12 +36,14 @@ public class Configurables { //Motor Positions public static double SLIDE_POWER_UP = 1; public static double SLIDE_POWER_DOWN = .7; - public static int SLIDELAYERONE = 150; - public static int SLIDEAUTOSTACKS = 350; + public static int SLIDELAYERONE = 60; + public static int SLIDEAUTOSTACKS = 250; public static int SLIDEUP = 1150; public static int HANGRELEASE = 2500; public static int HANG = 0; - public static int HANGPLANE = 1900; - + public static int HANGPLANE = 1800; + public static int SLIDELAYERTWO = 350; + public static int SLIDESTACK = 80; + public static int SLIDEPICKUPSTACKSTWO = 30; } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java index 7e0d326..e7c10ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java @@ -61,4 +61,6 @@ public class Constants { public static final String SLIDELEFT = "slideleft"; public static final String HANGRIGHT = "hangright"; public static final String HANGLEFT = "hangleft"; + public static final String LIGHTS = "lights"; + } \ No newline at end of file