diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index d550491..0c5d294 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -22,7 +22,7 @@ public class Arm { private double armTempPos; private double doorPos; private double wristPos; - public static double ARM_KP = 0.08; + public static double ARM_KP = 0.12; public static double doorOpenPos = 0.3; public static double doorClosePos = 0.61; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java index ea77fbe..0567c0b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java @@ -43,6 +43,7 @@ public class Intake { lServo.setDirection(Servo.Direction.REVERSE); rServo = hardwareMap.get(Servo.class, "Right Intake Servo"); dcMotor = hardwareMap.get(DcMotor.class, "Intakemotor"); + } public void setpos(Position stack) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index 928544b..a0530d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -24,7 +24,7 @@ public class Slides { public static int targetMax = 830; public static int down = 0; - public static int mini_tier1 = 155; + public static int mini_tier1 = 165; public static int tier1 = 350; public static int tier2 = 500; public static int tier3 = 720; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java index b0e4a2c..89155bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java @@ -1,105 +1,57 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; + +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.hardware.Slides; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +@Config @Autonomous(name = "Blue Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueBackStageAuto extends AutoBase { - public Trajectory scorePurple1; - public Trajectory scorePurple2; - public Trajectory scorePurple3; + public static final Pose2d DROP_1 = new Pose2d(12, 37.5, Math.toRadians(-90)); + public static final Pose2d DROP_2 = new Pose2d(12, 34.5, Math.toRadians(-90)); - public Trajectory scoreYellow1; - public Trajectory scoreYellow2; - public Trajectory scoreYellow3; + public static final Pose2d ALINE = new Pose2d(12,37.5, Math.toRadians(-90)); - public Trajectory parkRobot1; - public Trajectory parkRobot2; - public Trajectory parkRobot3; - - public Trajectory stackrun1b1; - public Trajectory stackrun1b2; - public Trajectory stackrun1b3; + public static final Pose2d DROP_3 = new Pose2d(12, 37.5, Math.toRadians(-90)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.5, 29, Math.toRadians(0)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.5, 32, Math.toRadians(0)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.5, 35, Math.toRadians(0)); + public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, 9.5);//-36 + public static final Pose2d STACK_LOCATION = new Pose2d(-58, 12, Math.toRadians(7)); @Override public void createTrajectories() { // set start position Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90)); robot.drive.setPoseEstimate(start); - - // create pose2d variables - // you might not need 3 instances of the deposit position, for example, however based on localization accuracy - // you might need them for each one to be slightly different - Pose2d drop1 = new Pose2d(12, 39.5, Math.toRadians(-90)); - Pose2d drop2 = new Pose2d(12, 39.5, Math.toRadians(-90)); - Pose2d drop3 = new Pose2d(12, 39.5, Math.toRadians(-90)); - - Pose2d depositPreload1 = new Pose2d(50.5, 32, Math.toRadians(-187)); - Pose2d depositPreload2 = new Pose2d(50.5, 32, Math.toRadians(-187)); - Pose2d depositPreload3 = new Pose2d(50.5, 32, Math.toRadians(-187)); - - Pose2d park1 = new Pose2d(48, 12, Math.toRadians(0)); - Pose2d park2 = new Pose2d(48, 12, Math.toRadians(0)); - Pose2d park3 = new Pose2d(48, 12, Math.toRadians(0)); - - Pose2d stack_1x1 = new Pose2d(-56, 12, Math.toRadians(0)); - Pose2d stack_2x1 = new Pose2d(-56, 12, Math.toRadians(0)); - Pose2d stack_3x1 = new Pose2d(-56, 12, Math.toRadians(0)); - - // create trajectories - scorePurple1 = robot.drive.trajectoryBuilder(start) - .lineToLinearHeading(drop1) - .build(); - scorePurple2 = robot.drive.trajectoryBuilder(start) - .lineToLinearHeading(drop2) - .build(); - scorePurple3 = robot.drive.trajectoryBuilder(start) - .lineToLinearHeading(drop3) - .build(); - - scoreYellow1 = robot.drive.trajectoryBuilder(scorePurple1.end()) - .lineToLinearHeading(depositPreload1) - .build(); - scoreYellow2 = robot.drive.trajectoryBuilder(scorePurple2.end()) - .lineToLinearHeading(depositPreload2) - .build(); - scoreYellow3 = robot.drive.trajectoryBuilder(scorePurple3.end()) - .lineToLinearHeading(depositPreload3) - .build(); - - parkRobot1 = robot.drive.trajectoryBuilder(scoreYellow1.end()) - .lineToLinearHeading(park1) - .build(); - parkRobot2 = robot.drive.trajectoryBuilder(scoreYellow2.end()) - .lineToLinearHeading(park2) - .build(); - parkRobot3 = robot.drive.trajectoryBuilder(scoreYellow3.end()) - .lineToLinearHeading(park3) - .build(); - - stackrun1b1 = robot.drive.trajectoryBuilder(scoreYellow1.end()) - .splineToConstantHeading(new Vector2d(30, 12), Math.toRadians(0)) - .lineToLinearHeading(stack_1x1) - .build(); - stackrun1b2 = robot.drive.trajectoryBuilder(scoreYellow2.end()) - .splineToConstantHeading(new Vector2d(30, 12), Math.toRadians(0)) - .lineToLinearHeading(stack_2x1) - .build(); - stackrun1b3 = robot.drive.trajectoryBuilder(scoreYellow1.end()) - .splineToConstantHeading(new Vector2d(30, 12), Math.toRadians(0)) - .lineToLinearHeading(stack_1x1) - .build(); } @Override public void followTrajectories() { + TrajectorySequenceBuilder builder; switch (macroState) { case 0: - robot.drive.followTrajectoryAsync(teamPropLocation==1?scorePurple1:(teamPropLocation==2?scorePurple2:scorePurple3)); + builder = this.robot.getTrajectorySequenceBuilder(); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_1); + break; + case 2: + builder.lineToLinearHeading(DROP_2); + break; + case 3: + builder.lineToLinearHeading(DROP_3); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; break; // DRIVE TO TAPE @@ -114,13 +66,25 @@ public class BlueBackStageAuto extends AutoBase { case 2: // intake if (getRuntime() < macroTime + 0.5) { - robot.intake.setDcMotor(-0.26); + robot.intake.setDcMotor(-0.1); } // if intake is done move on else { robot.intake.setDcMotor(0); robot.extendMacro(Slides.mini_tier1, getRuntime()); - robot.drive.followTrajectoryAsync(teamPropLocation==1?scoreYellow1:(teamPropLocation==2?scoreYellow2:scoreYellow3)); + builder = this.robot.getTrajectorySequenceBuilder(); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); + break; + case 2: + builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); + break; + case 3: + builder.lineToLinearHeading(DEPOSIT_PRELOAD_3); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; @@ -138,21 +102,112 @@ public class BlueBackStageAuto extends AutoBase { break; case 4: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 2){ - // robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3)); - robot.drive.followTrajectoryAsync(stackrun1b2); + if (robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(STACK_LOCATION.vec()); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; - + //waits for the robot to fin the trajectory case 5: - if(!robot.drive.isBusy()){ - macroState =-1; + robot.resetMacro(0, getRuntime()); + if (!robot.drive.isBusy()) { + macroState++; } - - //macroState++; break; - // PARK ROBOT + //First 2 pixels off the stack are intaken by this + case 6: + robot.intake.setDcMotor(0.46); + robot.intake.setpos(STACK4); + macroTime = getRuntime(); + macroState++; + break; + //gose back to the esile + case 7: + if (getRuntime() > macroTime + 1.5) { + robot.intake.setDcMotor(0); + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + switch (teamPropLocation) { + case 1: + builder.lineToConstantHeading(DEPOSIT_PRELOAD_1.vec()); + break; + case 2: + builder.lineToConstantHeading(DEPOSIT_PRELOAD_2.vec()); + break; + case 3: + builder.lineToConstantHeading(DEPOSIT_PRELOAD_3.vec()); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + case 8: + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1, getRuntime()); + macroState++; + } + break; + case 9: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + case 10: + robot.resetMacro(0, getRuntime()); + if (robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(STACK_LOCATION.vec()); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + case 11: + robot.resetMacro(0, getRuntime()); + if (!robot.drive.isBusy()) { + macroState++; + } + break; + //Geting the 2nd and 3rd pixel + case 12: + robot.intake.setDcMotor(0.46); + robot.intake.setpos(STACK3); + macroTime = getRuntime(); + macroState++; + break; + case 13: + if (getRuntime() > macroTime + 1.5) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + switch (teamPropLocation) { + case 1: + builder.lineToConstantHeading(DEPOSIT_PRELOAD_1.vec()); + break; + case 2: + builder.lineToConstantHeading(DEPOSIT_PRELOAD_2.vec()); + break; + case 3: + builder.lineToConstantHeading(DEPOSIT_PRELOAD_3.vec()); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + case 14: + macroState = -1; + // PARK ROBOT // case 6: // // reset macro' // if (robot.macroState != 0) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index 9ab902e..73f66dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -14,14 +14,26 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe @Config @Autonomous(name = "Red Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedBackStageAuto extends AutoBase { - public static final Pose2d DROP_1 = new Pose2d(12, -37.5, Math.toRadians(90)); - public static final Pose2d DROP_2 = new Pose2d(12, -37.5, Math.toRadians(90)); - public static final Pose2d DROP_3 = new Pose2d(12, -37.5, Math.toRadians(90)); - public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(53, -29, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(53, -32, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53, -35, Math.toRadians(180)); - public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, -10.5);//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-56, -14, Math.toRadians(180)); + public static final Pose2d DROP_1 = new Pose2d(12, -33.3, Math.toRadians(180)); + public static final Pose2d DROP_2 = new Pose2d(12, -33.2, Math.toRadians(90)); + + public static final Pose2d ALINE = new Pose2d(12,-37.5, Math.toRadians(90)); + + public static final Pose2d DROP_3 = new Pose2d(12, -33.2, Math.toRadians(0)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.2, -27.5, Math.toRadians(181)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, -32.5, Math.toRadians(181)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.2, -35.5, Math.toRadians(181)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.2, -35, Math.toRadians(187)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.2, -29, Math.toRadians(187)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.2, -29, Math.toRadians(187)); + + + public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, -8.5);//-36 + + public static final Pose2d STACK_LOCATION = new Pose2d(-56.6, -12, Math.toRadians(180)); @Override public void createTrajectories() { @@ -52,7 +64,6 @@ public class RedBackStageAuto extends AutoBase { break; // DRIVE TO TAPE case 1: - case 8: // if drive is done move on if (!robot.drive.isBusy()) { macroTime = getRuntime(); @@ -62,8 +73,8 @@ public class RedBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.5) { - robot.intake.setDcMotor(-0.46); + if (getRuntime() < macroTime + 0.3) { + robot.intake.setDcMotor(-0.21); } // if intake is done move on else { @@ -101,7 +112,7 @@ public class RedBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); if (robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(POST_SCORING_SPLINE_END, 0); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -116,31 +127,41 @@ public class RedBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.46); + robot.intake.setDcMotor(0.49); robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; break; //gose back to the esile case 7: - if (getRuntime() > macroTime + 1.5) { + if (getRuntime() > macroTime + 1.2) { + robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(POST_SCORING_SPLINE_END); switch (teamPropLocation) { case 1: - builder.splineToConstantHeading(DEPOSIT_PRELOAD_1.vec(), 0); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); break; case 2: - builder.splineToConstantHeading(DEPOSIT_PRELOAD_2.vec(), 0); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); break; case 3: - builder.splineToConstantHeading(DEPOSIT_PRELOAD_3.vec(), 0); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; + case 8: + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1, getRuntime()); + macroState++; + } + break; case 9: if (robot.macroState != 0) { robot.extendMacro(Slides.mini_tier1, getRuntime()); @@ -154,7 +175,7 @@ public class RedBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); if (robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(POST_SCORING_SPLINE_END, 0); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -168,24 +189,24 @@ public class RedBackStageAuto extends AutoBase { break; //Geting the 2nd and 3rd pixel case 12: - robot.intake.setDcMotor(0.46); + robot.intake.setDcMotor(0.49); robot.intake.setpos(STACK3); macroTime = getRuntime(); macroState++; break; case 13: - if (getRuntime() > macroTime + 1.5) { + if (getRuntime() > macroTime + 1.2) { builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(POST_SCORING_SPLINE_END); switch (teamPropLocation) { case 1: - builder.splineToConstantHeading(DEPOSIT_PRELOAD_1.vec(), 0); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); break; case 2: - builder.splineToConstantHeading(DEPOSIT_PRELOAD_2.vec(), 0); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); break; case 3: - builder.splineToConstantHeading(DEPOSIT_PRELOAD_3.vec(), 0); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index 0fccaf6..74015e3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -9,6 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.controller.Controller; +import org.firstinspires.ftc.teamcode.hardware.Arm; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.Slides; @@ -101,9 +102,14 @@ public class MainTeleOp extends OpMode { // macros switch (robot.runningMacro) { case (0): // manual mode + if (controller2.getLeftBumper().isPressed()){ + robot.arm.setDoor(Arm.DoorPosition.OPEN); + } robot.slides.increaseTarget(controller2.getLeftStick().getY()); if (robot.intake.getPower() >= 0.01) { robot.arm.setDoor(OPEN); + } else if (robot.intake.getPower() <= -0.01) { + robot.arm.setDoor(OPEN); } else { robot.arm.setDoor(CLOSE); } @@ -116,7 +122,10 @@ public class MainTeleOp extends OpMode { robot.runningMacro = 3; } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { robot.runningMacro = 4; + } else if (controller2.getDDown().isJustPressed() ) { + robot.runningMacro = 5; } + break; case (1): robot.extendMacro(Slides.tier1, getRuntime()); @@ -130,6 +139,9 @@ public class MainTeleOp extends OpMode { case (4): robot.resetMacro(0, getRuntime()); break; + case(5): + robot.extendMacro(Slides.mini_tier1, getRuntime()); + break; } // update and telemetry