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 f6b1f1d..293dc4c 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 @@ -30,11 +30,11 @@ public class Intake { } //Position - public static double stack1 = 0.02; + public static double stack1 = 0.03; public static double stack2 = 0.03; - public static double stack3 = 0.06; - public static double stack4 = 0.085; - public static double stack5 = 0.09; + public static double stack3 = 0.065; + public static double stack4 = 0.09; + public static double stack5 = 0.1; public static double up = 0.30; public static double motorPower = 0; 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 f784ccb..5a3037a 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 @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; 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 com.acmerobotics.dashboard.config.Config; @@ -17,27 +18,31 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config @Autonomous(name = "Blue Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueBackStageAuto extends AutoBase { - public static final Pose2d DROP_3 = new Pose2d(16, 32, Math.toRadians(-180)); - public static final Pose2d DROP_2 = new Pose2d(14, 35, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180)); + + public static final Pose2d DROP_3M = new Pose2d(14.7, 30, Math.toRadians(-180)); + public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90)); public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180)); public static final Pose2d DROP_1 = new Pose2d(25, 43, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(54.8, 27.5, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, 32.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(54.8, 28.7, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.6, 34.5, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.2, 39.3, Math.toRadians(-180)); public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.4, 35.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.6, 30.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.5, 30.6, Math.toRadians(-187)); public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.7, 32, Math.toRadians(-187)); //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 - public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.8, Math.toRadians(-180));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-54.7, 11.8, Math.toRadians(-180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-54.8, 11.1, Math.toRadians(-180)); + + public static final Pose2d STACK_LOCATION2 = new Pose2d(-55.3, 11.1, Math.toRadians(-180)); @Override public void createTrajectories() { @@ -62,6 +67,7 @@ public class BlueBackStageAuto extends AutoBase { break; case 3: builder.lineToLinearHeading(DROP_3); + builder.lineToLinearHeading(DROP_3M); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -78,8 +84,8 @@ public class BlueBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.2) { - robot.intake.setDcMotor(-0.22); + if (getRuntime() < macroTime + 0.1) { + robot.intake.setDcMotor(-0.18); } // if intake is done move on else { @@ -123,7 +129,7 @@ public class BlueBackStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); - builder.lineToConstantHeading(STACK_LOCATION.vec()); + builder.lineToConstantHeading(STACK_LOCATION.vec().plus(new Vector2d(0,0))); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -131,7 +137,7 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.44); + robot.intake.setDcMotor(0.38); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; @@ -139,15 +145,16 @@ public class BlueBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.44); + robot.intake.setDcMotor(0.38); robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 0.3) { - robot.intake.setDcMotor(0); + if (getRuntime() > macroTime + 0.03) { + //robot.intake.setDcMotor(-0.0); + robot.intake.setDcMotor(-0.65); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); @@ -194,7 +201,7 @@ public class BlueBackStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); - builder.lineToConstantHeading(STACK_LOCATION.vec()); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0))); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -202,23 +209,23 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.44); - robot.intake.setpos(STACK3); + robot.intake.setDcMotor(0.38); + robot.intake.setpos(STACK4); if (!robot.drive.isBusy()) { macroState++; } break; - //First 2 pixels off the stack are intaken by this + //Third and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.44); + robot.intake.setDcMotor(0.38); robot.intake.setpos(STACK3); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: - if (getRuntime() > macroTime + 0.3) { - robot.intake.setDcMotor(0); + if (getRuntime() > macroTime + 0.03) { + robot.intake.setDcMotor(0.65); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); 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 f9c9344..babfc0b 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1; 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; @@ -19,18 +20,18 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "Red Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedBackStageAuto extends AutoBase { public static final Pose2d DROP_1 = new Pose2d(12, -32.5, Math.toRadians(180)); - public static final Pose2d DROP_2 = new Pose2d(13.7, -33.5, Math.toRadians(90)); + public static final Pose2d DROP_2 = new Pose2d(13.3, -33.5, Math.toRadians(90)); public static final Pose2d ALINE = new Pose2d(51,-32.5, Math.toRadians(180)); public static final Pose2d DROP_3 = new Pose2d(25, -45.5, Math.toRadians(90)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51, -31.5, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(50, -31.5, Math.toRadians(180)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -42, Math.toRadians(180)); public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50.5, -30, Math.toRadians(188));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -30, Math.toRadians(188));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 @@ -38,9 +39,9 @@ public class RedBackStageAuto extends AutoBase { //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-57.9, -12.4, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION = new Pose2d(-53, -12.9, Math.toRadians(190)); - public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-58.5, -12.4, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-54, -12.9, Math.toRadians(190)); @Override public void createTrajectories() { @@ -81,8 +82,8 @@ public class RedBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.28) { - robot.intake.setDcMotor(-0.22); + if (getRuntime() < macroTime + 0.23) { + robot.intake.setDcMotor(-0.18); } // if intake is done move on else { @@ -122,7 +123,7 @@ public class RedBackStageAuto extends AutoBase { // STACK RUN 1 ------------------------- case 4: robot.resetMacro(0, getRuntime()); - if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { + if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); @@ -150,7 +151,7 @@ public class RedBackStageAuto extends AutoBase { //goes back to the easel case 7: if (getRuntime() > macroTime + 0.5) { - robot.intake.setDcMotor(0); + robot.intake.setDcMotor(0.65); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); @@ -193,7 +194,7 @@ public class RedBackStageAuto extends AutoBase { // STACK RUN 2 case 10: robot.resetMacro(0, getRuntime()); - if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { + if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); builder.lineToConstantHeading(STACK_LOCATION_TWO.vec()); @@ -205,7 +206,7 @@ public class RedBackStageAuto extends AutoBase { case 11: robot.resetMacro(0, getRuntime()); robot.intake.setDcMotor(0.54); - robot.intake.setpos(STACK3); + robot.intake.setpos(STACK2); if (!robot.drive.isBusy()) { macroState++; } @@ -213,14 +214,14 @@ public class RedBackStageAuto extends AutoBase { //3rd and 4th pixels off the stack are intaken by this case 12: robot.intake.setDcMotor(0.54); - robot.intake.setpos(STACK2); + robot.intake.setpos(STACK1); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: if (getRuntime() > macroTime + 0.6) { - robot.intake.setDcMotor(0); + robot.intake.setDcMotor(0.65); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java index 20c19f1..c73c565 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java @@ -29,13 +29,15 @@ public class RedFrontStageAuto extends AutoBase { public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-55, -34.4, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-52, -34.4, Math.toRadians(180)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36 - public static final Pose2d POST_DROP_POS = new Pose2d(-22, -63.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS = new Pose2d(-45, -57.5, Math.toRadians(180)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(34, -65.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(180)); + + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(180)); @Override public void createTrajectories() { @@ -77,13 +79,13 @@ public class RedFrontStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.32) { - robot.intake.setDcMotor(-0.18); + if (getRuntime() < macroTime + 0.22) { + robot.intake.setDcMotor(-0.15); } else{ builder = this.robot.getTrajectorySequenceBuilder(); robot.intake.setDcMotor(0); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(0))); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(0,-1.5))); robot.arm.setDoor(OPEN); robot.intake.setDcMotor(0.54); robot.intake.setpos(STACK5); @@ -103,11 +105,12 @@ public class RedFrontStageAuto extends AutoBase { case 4: - if (getRuntime() > macroTime + 0.3) { + if (getRuntime() > macroTime + 0.2) { robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToLinearHeading(POST_DROP_POS); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0)); switch (teamPropLocation) { case 1: @@ -130,7 +133,7 @@ public class RedFrontStageAuto extends AutoBase { break; case 5: // if drive is done move on - if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) { + if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); @@ -185,6 +188,8 @@ public class RedFrontStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToSplineHeading(POST_DROP_POS, Math.toRadians(0)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0)); + switch (teamPropLocation) { case 1: @@ -207,7 +212,7 @@ public class RedFrontStageAuto extends AutoBase { break; case 11: // if drive is done move on - if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { + if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); @@ -285,7 +290,7 @@ public class RedFrontStageAuto extends AutoBase { break; case 17: // if drive is done move on - if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { + if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); 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 f35876c..32fbc5b 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 @@ -20,7 +20,7 @@ public class MainTeleOp extends OpMode { public static double normal = 0.7; public static double turbo = 1; public static double slow_mode = 0.35; - public static double intakeMax = 0.5; + public static double intakeMax = 0.65; public static double intakeMax2 = -0.65; private Robot robot; diff --git a/build.dependencies.gradle b/build.dependencies.gradle index e73a9ca..3eda355 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -5,15 +5,15 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:9.0.0' - implementation 'org.firstinspires.ftc:Blocks:9.0.0' - implementation 'org.firstinspires.ftc:Tfod:9.0.0' - implementation 'org.firstinspires.ftc:RobotCore:9.0.0' - implementation 'org.firstinspires.ftc:RobotServer:9.0.0' - implementation 'org.firstinspires.ftc:OnBotJava:9.0.0' - implementation 'org.firstinspires.ftc:Hardware:9.0.0' - implementation 'org.firstinspires.ftc:FtcCommon:9.0.0' - implementation 'org.firstinspires.ftc:Vision:9.0.0' + implementation 'org.firstinspires.ftc:Inspection:9.0.1' + implementation 'org.firstinspires.ftc:Blocks:9.0.1' + implementation 'org.firstinspires.ftc:Tfod:9.0.1' + implementation 'org.firstinspires.ftc:RobotCore:9.0.1' + implementation 'org.firstinspires.ftc:RobotServer:9.0.1' + implementation 'org.firstinspires.ftc:OnBotJava:9.0.1' + implementation 'org.firstinspires.ftc:Hardware:9.0.1' + implementation 'org.firstinspires.ftc:FtcCommon:9.0.1' + implementation 'org.firstinspires.ftc:Vision:9.0.1' implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0'