From 099ce699ef2780010f2a7cb7e8e011ce52dcf337 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Fri, 12 Jan 2024 16:56:05 -0600 Subject: [PATCH] mirror mirror on the wall --- .../ftc/teamcode/hardware/Arm.java | 2 +- .../ftc/teamcode/hardware/Slides.java | 5 +- .../opmode/autonomous/BlueBackStageAuto.java | 110 +++++++++++++----- .../opmode/autonomous/RedBackStageAuto.java | 86 ++++++++++---- 4 files changed, 150 insertions(+), 53 deletions(-) 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 0c5d294..4b677ee 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.12; + public static double ARM_KP = 0.11; public static double doorOpenPos = 0.3; public static double doorClosePos = 0.61; 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 a0530d7..8d6b340 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 @@ -16,7 +16,8 @@ public class Slides { // public static double i = 0.02; // public static double d = 0; // public static double f = 0.01; - public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0014,0.02,0,0.01); + //p was 0.0014 + public static PIDFCoefficients coefficients = new PIDFCoefficients(0.002,0.02,0,0.01); public static double pTolerance = 20; private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d); @@ -24,7 +25,7 @@ public class Slides { public static int targetMax = 830; public static int down = 0; - public static int mini_tier1 = 165; + public static int mini_tier1 = 170; 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 89155bd..569e1f3 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,7 +1,6 @@ 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; @@ -13,19 +12,28 @@ 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") +@Autonomous(name = "Blue Backstage Auto(2+2)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueBackStageAuto 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, 34.5, Math.toRadians(-90)); + public static final Pose2d DROP_1 = new Pose2d(12.3, 32.5, Math.toRadians(0)); + public static final Pose2d DROP_2 = new Pose2d(13.7, 32.5, 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, 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)); + public static final Pose2d DROP_3 = new Pose2d(25, 41.3, Math.toRadians(-90)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, 27.5, Math.toRadians(1)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, 32.5, Math.toRadians(1)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, 39.5, Math.toRadians(1)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.3, 35.3, Math.toRadians(7)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.3, 29, Math.toRadians(7)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, 32, Math.toRadians(7)); + + + public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, 8.5);//-36 + + public static final Pose2d STACK_LOCATION = new Pose2d(-57.4, 10.6, Math.toRadians(0)); @Override public void createTrajectories() { @@ -65,14 +73,15 @@ public class BlueBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.5) { - robot.intake.setDcMotor(-0.1); + if (getRuntime() < macroTime + 0.28) { + robot.intake.setDcMotor(-0.2); } // if intake is done move on else { robot.intake.setDcMotor(0); robot.extendMacro(Slides.mini_tier1, getRuntime()); builder = this.robot.getTrajectorySequenceBuilder(); + //Scores yellow pixel switch (teamPropLocation) { case 1: builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); @@ -119,26 +128,26 @@ public class BlueBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.46); - robot.intake.setpos(STACK4); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; break; - //gose back to the esile + //goes back to the easel case 7: - if (getRuntime() > macroTime + 1.5) { + if (getRuntime() > macroTime + 1.6) { robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(POST_SCORING_SPLINE_END); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(DEPOSIT_PRELOAD_1.vec()); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); break; case 2: - builder.lineToConstantHeading(DEPOSIT_PRELOAD_2.vec()); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); break; case 3: - builder.lineToConstantHeading(DEPOSIT_PRELOAD_3.vec()); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -150,55 +159,65 @@ public class BlueBackStageAuto extends AutoBase { if (!robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); macroState++; } break; + //extending the macro and about to score case 9: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; } break; + //scores pixel and goes back to the stack case 10: robot.resetMacro(0, getRuntime()); if (robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + + } + break; + /* builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; + //Ensures that the robot has scored the pixel and moves on case 11: robot.resetMacro(0, getRuntime()); if (!robot.drive.isBusy()) { macroState++; } break; - //Geting the 2nd and 3rd pixel + //Getting 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; + //Goes back to the easel 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.lineToConstantHeading(DEPOSIT_PRELOAD_1.vec()); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); break; case 2: - builder.lineToConstantHeading(DEPOSIT_PRELOAD_2.vec()); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); break; case 3: - builder.lineToConstantHeading(DEPOSIT_PRELOAD_3.vec()); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -206,7 +225,40 @@ public class BlueBackStageAuto extends AutoBase { } break; case 14: - macroState = -1; + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1 +20 , getRuntime()); + macroState++; + } + break; + //Scoring the pixels + case 15: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1 + 20 , getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + case 16: + robot.resetMacro(0, getRuntime()); + if (robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break;*/ + + case 11: + robot.resetMacro(0, getRuntime()); + if (robot.macroState >= 4) { + macroState = -1; + } + // PARK ROBOT // case 6: // // reset macro' 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 73f66dd..8aeeba2 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 @@ -12,28 +12,28 @@ import org.firstinspires.ftc.teamcode.hardware.Slides; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @Config -@Autonomous(name = "Red Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "Red Backstage Auto(2+2)", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedBackStageAuto extends AutoBase { - 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 DROP_1 = new Pose2d(12.3, -32.5, Math.toRadians(180)); + public static final Pose2d DROP_2 = new Pose2d(13.7, -32.5, 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 DROP_3 = new Pose2d(25, -41.3, Math.toRadians(90)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -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_PRELOAD_3 = new Pose2d(51.3, -39.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_1 = new Pose2d(51.3, -35.3, 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_2 = new Pose2d(51.3, -29, Math.toRadians(187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.2, -29, Math.toRadians(187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(187)); - public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, -8.5);//-36 + public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-56.6, -12, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-57.4, -10.6, Math.toRadians(180)); @Override public void createTrajectories() { @@ -73,14 +73,15 @@ public class RedBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.3) { - robot.intake.setDcMotor(-0.21); + if (getRuntime() < macroTime + 0.28) { + robot.intake.setDcMotor(-0.2); } // if intake is done move on else { robot.intake.setDcMotor(0); robot.extendMacro(Slides.mini_tier1, getRuntime()); builder = this.robot.getTrajectorySequenceBuilder(); + //Scores yellow pixel switch (teamPropLocation) { case 1: builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); @@ -127,14 +128,14 @@ public class RedBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.49); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; break; - //gose back to the esile + //goes back to the easel case 7: - if (getRuntime() > macroTime + 1.2) { + if (getRuntime() > macroTime + 1.6) { robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(POST_SCORING_SPLINE_END); @@ -158,42 +159,52 @@ public class RedBackStageAuto extends AutoBase { if (!robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); macroState++; } break; + //extending the macro and about to score case 9: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; } break; + //scores pixel and goes back to the stack case 10: robot.resetMacro(0, getRuntime()); if (robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + + } + break; + /* builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; + //Ensures that the robot has scored the pixel and moves on case 11: robot.resetMacro(0, getRuntime()); if (!robot.drive.isBusy()) { macroState++; } break; - //Geting the 2nd and 3rd pixel + //Getting the 2nd and 3rd pixel case 12: robot.intake.setDcMotor(0.49); robot.intake.setpos(STACK3); macroTime = getRuntime(); macroState++; break; + //Goes back to the easel case 13: if (getRuntime() > macroTime + 1.2) { builder = this.robot.getTrajectorySequenceBuilder(); @@ -214,7 +225,40 @@ public class RedBackStageAuto extends AutoBase { } break; case 14: - macroState = -1; + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1 +20 , getRuntime()); + macroState++; + } + break; + //Scoring the pixels + case 15: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1 + 20 , getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + case 16: + robot.resetMacro(0, getRuntime()); + if (robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break;*/ + + case 11: + robot.resetMacro(0, getRuntime()); + if (robot.macroState >= 4) { + macroState = -1; + } + // PARK ROBOT // case 6: // // reset macro'