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 a4cc4ba..a61f7e4 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 @@ -31,7 +31,7 @@ public class Intake { //Position public static double stack1 = 0.015; - public static double stack2 = 0.026; + public static double stack2 = 0.029; public static double stack3 = 0.065; public static double stack4 = 0.09; public static double stack5 = 0.1; 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 0bdda44..e406779 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 @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2; 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 static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; @@ -43,7 +44,7 @@ public class BlueBackStageAuto extends AutoBase { public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 12.1, Math.toRadians(-187)); - public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 12.1, Math.toRadians(-187)); + public static final Pose2d STACK_LOCATION2 = new Pose2d(-55, 12.1, Math.toRadians(-187)); public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 12.1, Math.toRadians(-187)); @@ -88,7 +89,7 @@ public class BlueBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.1) { + if (getRuntime() < macroTime + 0.25) { robot.intake.setDcMotor(-0.24); } // if intake is done move on @@ -151,23 +152,23 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - //robot.intake.setDcMotor(0.5); - robot.intake.setpos(STACK5); - if (!robot.drive.isBusy()) { - robot.intake.setDcMotor(0.5); + robot.intake.setDcMotor(0.58); + robot.intake.setpos(STACK6); + if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) { + //robot.intake.setDcMotor(0.5); macroState++; } break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.5); - robot.intake.setpos(STACK4); + robot.intake.setDcMotor(0.48); + robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 0.5) { + if (getRuntime() > macroTime + 0.6) { //robot.intake.setDcMotor(-0.0); robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); @@ -236,22 +237,23 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.58); robot.intake.setpos(STACK3); - if (!robot.drive.isBusy()) { - robot.intake.setDcMotor(0.68); + if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) { + //robot.intake.setDcMotor(0.68); macroState++; } break; //Third and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.68); - robot.intake.setpos(STACK1); + robot.intake.setDcMotor(0.48); + robot.intake.setpos(STACK2); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: - if (getRuntime() > macroTime + 0.5) { + if (getRuntime() > macroTime + 0.7) { robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); 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 0441aff..4e9b215 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 @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2; 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 static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; @@ -30,12 +31,12 @@ public class RedBackStageAuto extends AutoBase { public static final Pose2d DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -32.5, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52, -40, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, -32.5, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.3, -39, Math.toRadians(180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(180));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(52, -35.3, Math.toRadians(180));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -29.5, Math.toRadians(180));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -30.5, Math.toRadians(180));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(52.4, -34.5, Math.toRadians(180));//187 @@ -142,7 +143,7 @@ public class RedBackStageAuto extends AutoBase { robot.intake.setpos(STACK5); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-1))); + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-2))); robot.intake.setpos(STACK5); break; case 3: @@ -152,22 +153,24 @@ public class RedBackStageAuto extends AutoBase { } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; + macroTime = getRuntime(); } break; //waits for the robot to fin the trajectory case 5: - robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.48); - robot.intake.setpos(STACK5); - if (!robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.58); + robot.intake.setpos(STACK6); + + if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) { macroState++; } break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.38); - robot.intake.setpos(STACK4); + robot.intake.setDcMotor(0.48); + robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; break; @@ -224,34 +227,37 @@ public class RedBackStageAuto extends AutoBase { builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2))); + builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.75))); robot.intake.setpos(STACK3); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3))); + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3.3))); robot.intake.setpos(STACK3); break; case 3: - builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-1.8))); + builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8))); robot.intake.setpos(STACK3); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; + macroTime = getRuntime(); } break; //waits for the robot to fin the trajectory case 11: - robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.48); - robot.intake.setpos(STACK3); - if (!robot.drive.isBusy()) { + + robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.58); + robot.intake.setpos(STACK3); + + if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) { macroState++; } break; //3rd and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.38); + robot.intake.setDcMotor(0.48); robot.intake.setpos(STACK2); macroTime = getRuntime(); macroState++; @@ -285,14 +291,14 @@ public class RedBackStageAuto extends AutoBase { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 100, getRuntime()); macroState++; } break; //extending the macro and about to score case 15: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 100, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime());