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 5a92461..56e1bcc 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 @@ -76,7 +76,7 @@ public class Robot { public static class Slides { private DcMotor slidesRight = null; - private DcMotor slidesLeft = null; + public DcMotor slidesLeft = null; public Slides init(HardwareMap hardwareMap) { this.slidesLeft = hardwareMap.get(DcMotor.class, SLIDELEFT); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwoNew.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java similarity index 54% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwoNew.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java index 0834f39..13e5e30 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwoNew.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java @@ -4,50 +4,52 @@ 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.acmerobotics.roadrunner.trajectory.Trajectory; 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 = "autoRed2+2New") -public class AutoRedTwoPlusTwoNew extends LinearOpMode { +@Autonomous(name = "autoBLue2+2") +public class AutoBlueTwoPlusTwo extends LinearOpMode { protected Pose2d initialPosition; - - protected Trajectory park1; - protected Trajectory park2; - 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(46, -35, Math.toRadians(270)); + 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)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(76, -26.5, Math.toRadians(360)); - final static Pose2d CENTER_BOARD = new Pose2d(75.7, -36.3, Math.toRadians(360)); - final static Pose2d RIGHT_BOARD = new Pose2d(75, -42, Math.toRadians(360)); + 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)); //Stack Cycle - final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360)); - final static Pose2d TO_STACK = new Pose2d(-41, -8, Math.toRadians(360)); - final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360)); - final static Pose2d APPROACHING_BOARD = new Pose2d(73.5, -28, Math.toRadians(360)); - final static Pose2d SCORE_STACK = new Pose2d(72.5, -28, Math.toRadians(360)); + 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 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(-74.5, -28, 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)); protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(LEFT_PRELOAD_ONE); - builder.lineToLinearHeading(LEFT_PRELOAD_TWO); + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + break; case "CENTER": builder.lineToLinearHeading(CENTER_PRELOAD); - case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD); + break; + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; } this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -55,12 +57,15 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode { protected void boardScore() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD); - case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD); case "RIGHT": builder.lineToLinearHeading(RIGHT_BOARD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_BOARD); + break; + case "LEFT": + builder.lineToLinearHeading(LEFT_BOARD); + break; } builder.addTemporalMarker(.2, robot.getArm()::armScore); builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); @@ -71,9 +76,10 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode { protected void toStack() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(LEAVE_BOARD); - builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.addTemporalMarker(.3, robot.getArm()::armPickupStack); builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + builder.addTemporalMarker(1.5,robot.getClaw()::openStack); builder.lineToLinearHeading(TO_STACK); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -83,22 +89,35 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode { builder.lineToLinearHeading(BACK_THROUGH_GATE); builder.lineToLinearHeading(APPROACHING_BOARD); builder.lineToLinearHeading(SCORE_STACK); - builder.addTemporalMarker(2, robot.getArm()::armSecondaryScore); - builder.addTemporalMarker(2, robot.getWrist()::wristScore); - builder.addTemporalMarker(2, robot.getSlides()::slideAutoStacks); + builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); + builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks); this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void clawSlowOpen() { double currentPos = .86; - double targetPos = .78; + double targetPos = .8; 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)); - sleep(12); +// this.robot.getSlides().slideTo(Position + 14,1); + sleep(35); } } + protected void park() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + 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()); + } + @Override public void runOpMode() throws InterruptedException { @@ -106,17 +125,17 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode { 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.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(); +// 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()); + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); + randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); this.telemetry.update(); } @@ -130,13 +149,15 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode { toStack(); sleep(500); - this.robot.getClaw().setPos(.86); - sleep(250); + this.robot.getClaw().close(); + sleep(500); + this.robot.getArm().armRest(); scoreStack(); - - sleep(200); + this.robot.getClaw().setPos(.86); + sleep(150); clawSlowOpen(); sleep(300); + 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 8a0ac02..c100a8c 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 @@ -1,142 +1,132 @@ 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.acmerobotics.roadrunner.trajectory.Trajectory; 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 = "autoRed2+2") public class AutoRedTwoPlusTwo 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; - - protected Trajectory goGate; - protected Trajectory goStack; - protected Trajectory backGate; - protected Trajectory approachBoard; - protected Trajectory scoreStack; - - private Robot robot; private String randomization; - private int random; + + //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)); + //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)); + //Stack Cycle + final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360)); + final static Pose2d TO_STACK = new Pose2d(-40, -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, -28, Math.toRadians(360)); + final static Pose2d SCORE_STACK = new Pose2d(74.45, -28, Math.toRadians(360)); + //Park + final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360)); + final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360)); + + 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 boardScore() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_BOARD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_BOARD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_BOARD); + break; + } + builder.addTemporalMarker(.2, robot.getArm()::armScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void toStack() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(LEAVE_BOARD); + builder.addTemporalMarker(.3, robot.getArm()::armPickupStack); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + builder.addTemporalMarker(1.5,robot.getClaw()::openStack); + builder.lineToLinearHeading(TO_STACK); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void scoreStack() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(BACK_THROUGH_GATE); + builder.lineToLinearHeading(APPROACHING_BOARD); + 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); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void clawSlowOpen() { + double currentPos = .86; + double targetPos = .78; + 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(); + builder.lineToLinearHeading(BACK_OFF); + builder.lineToLinearHeading(PARK); + builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + 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(); - - //Trajectories this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); - //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(29.5, -29, Math.toRadians(360))) - .build(); - - this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) - .lineToLinearHeading(new Pose2d(76, -26.5, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armScore) - .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) - .lineToLinearHeading(new Pose2d(60, -15, Math.toRadians(360))) - .build(); - - - //Randomization Two - this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(33, -28, Math.toRadians(270))) - .build(); - - this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) - .lineToLinearHeading(new Pose2d(75.7, -36.3, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armScore) - .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) - .lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360))) - .build(); - - //Randomization Three - this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270))) - .build(); - - this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) - .lineToLinearHeading(new Pose2d(75, -42, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armScore) - .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) - .lineToLinearHeading(new Pose2d(60, -45, 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) - .addTemporalMarker(.1,robot.getSlides()::slideDown) - .build(); - this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) - .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) - .build(); - -// //Cycle - this.goGate = this.robot.getDrive().trajectoryBuilder(park1.end()) - .lineToLinearHeading(new Pose2d(-41,-8, Math.toRadians(360))) - .addTemporalMarker(.5,robot.getClaw()::openStack) - .addTemporalMarker(.2,robot.getArm()::armPickupStack) - .addTemporalMarker(.1,robot.getSlides()::slideDown) - .build(); - this.backGate = this.robot.getDrive().trajectoryBuilder(goGate.end()) - .lineToLinearHeading(new Pose2d(50,-10, Math.toRadians(360))) - .build(); - this.approachBoard = this.robot.getDrive().trajectoryBuilder(backGate.end()) - .lineToLinearHeading(new Pose2d(73.5, -28, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armSecondaryScore) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .addTemporalMarker(.2,robot.getSlides()::slideAutoStacks) - .build(); - this.scoreStack = this.robot.getDrive().trajectoryBuilder(approachBoard.end()) - .lineToLinearHeading(new Pose2d(72.5, -28, Math.toRadians(360))) - .build(); +// 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()) { @@ -145,60 +135,25 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { this.telemetry.update(); } - switch (randomization) { - case "LEFT": - this.robot.getDrive().followTrajectory(preloadOne); - this.robot.getDrive().followTrajectory(scoreOne); - this.robot.getDrive().followTrajectory(boardOne); - sleep(500); - this.robot.getClaw().open(); - sleep(500); - this.robot.getDrive().followTrajectory(backOffOne); - this.robot.getSlides().slideDown(); - sleep(300); - break; - case "CENTER": - this.robot.getDrive().followTrajectory(preloadTwo); - this.robot.getDrive().followTrajectory(scoreTwo); - sleep(500); - this.robot.getClaw().open(); - sleep(500); - this.robot.getDrive().followTrajectory(backOffTwo); - this.robot.getSlides().slideDown(); - sleep(300); - break; - case "RIGHT": - this.robot.getDrive().followTrajectory(preloadThree); - this.robot.getDrive().followTrajectory(scoreThree); - sleep(500); - this.robot.getClaw().open(); - sleep(500); - this.robot.getDrive().followTrajectory(backOffThree); - this.robot.getSlides().slideDown(); - sleep(300); - break; - } - //Cycle - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(goGate); + scorePreloadOne(); + boardScore(); + + sleep(250); + this.robot.getClaw().open(); + sleep(250); + + toStack(); + sleep(500); this.robot.getClaw().close(); - sleep(250); - this.robot.getDrive().followTrajectory(backGate); - this.robot.getDrive().followTrajectory(approachBoard); + sleep(500); + this.robot.getArm().armRest(); + scoreStack(); this.robot.getClaw().setPos(.86); - sleep(200); - - double currentPos = .86; - double targetPos = .78; - double delta = (targetPos - currentPos) / 100; - for (int i = 0 ; i < 100; i++) { - this.robot.getClaw().setPos(currentPos + (delta * i)); - sleep(12); - } - + sleep(500); + clawSlowOpen(); sleep(300); - this.robot.getDrive().followTrajectory(park1); + park(); } } 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 2c5773c..cc29aa8 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 @@ -16,13 +16,13 @@ public class Configurables { //Servo Positions public static double ARMREST = 0.8; public static double ARMSCORE = 0.39; - public static double ARMACCSCORE = .39; - public static double ARMPICKUPSTACK = 0.825; + public static double ARMACCSCORE = .38; + public static double ARMPICKUPSTACK = 0.843; public static double PICKUP = 0.84; public static double WRISTPICKUP = 0.28; public static double WRISTSCORE = .96; public static double OPEN = 0.85; - public static double BIGOPEN = 0.73; + public static double BIGOPEN = 0.67; public static double CLOSE = 0.95; public static double PLANELOCK = 0.1; public static double PLANELAUNCH = 0.9; @@ -38,7 +38,7 @@ public class Configurables { public static double SLIDE_POWER_UP = 1; public static double SLIDE_POWER_DOWN = .7; public static int SLIDELAYERONE = 150; - public static int SLIDEAUTOSTACKS = 300; + public static int SLIDEAUTOSTACKS = 225; public static int SLIDEUP = 1150; public static int HANGRELEASE = 2500; public static int HANG = 0;