diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java index 1c06d82..f106437 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -95,8 +95,8 @@ public class MeepMeepTesting { .setBackgroundAlpha(0.95f) //.addEntity(myBot) //.addEntity(BlueFrontStage) - //.addEntity(BlueFrontStage3) - .addEntity(BlueFrontStage1) + .addEntity(BlueFrontStage3) + //.addEntity(BlueFrontStage1) .start(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java index 1031f5c..25ca318 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java @@ -14,7 +14,7 @@ public class endGame_Mechs { private DcMotor hang; public static double initPos = 0.42; public static double launchPos = 0.8; - public static double finger_out = 0.5; + public static double finger_out = 0.48; public static double finger_in = 0.2; public static int initHang = -1000; public static double hold = 0.8; @@ -34,8 +34,8 @@ public class endGame_Mechs { public endGame_Mechs(HardwareMap hardwareMap) { this.servo = hardwareMap.get(Servo.class, "Drone"); this.servo.setPosition(initPos); - this.servo = hardwareMap.get(Servo.class, "Finger"); - this.servo.setPosition(finger_out); +// this.servo = hardwareMap.get(Servo.class, "Finger"); +// this.servo.setPosition(finger_out); this.hang = hardwareMap.get(DcMotor.class, "Hang"); this.hang.setTargetPosition(0); this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -58,9 +58,9 @@ public class endGame_Mechs { } - public void Finger_in() {this.servo.setPosition(finger_in);} - - public void Finger_out () {this.servo.setPosition(finger_out);} +// public void Finger_in() {this.servo.setPosition(finger_in);} +// +// public void Finger_out () {this.servo.setPosition(finger_out);} public void hang_init_pos(){ 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 8b6bb44..40945c8 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 @@ -39,13 +39,13 @@ public class BlueBackStageAuto 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(26, 11.1 , Math.toRadians(-187));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 12.1 , Math.toRadians(-187));//-36 - public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-187)); + 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, 11.1, Math.toRadians(-187)); + public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 12.1, Math.toRadians(-187)); - public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-187)); + public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 12.1, Math.toRadians(-187)); @Override @@ -141,7 +141,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5))); break; case 3: - builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5))); + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(-1.25))); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -151,7 +151,7 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.7); + robot.intake.setDcMotor(0.5); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; @@ -159,7 +159,7 @@ public class BlueBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.68); + robot.intake.setDcMotor(0.5); robot.intake.setpos(STACK4); macroTime = getRuntime(); macroState++; @@ -168,6 +168,7 @@ public class BlueBackStageAuto extends AutoBase { case 7: if (getRuntime() > macroTime + 0.5) { //robot.intake.setDcMotor(-0.0); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); @@ -223,7 +224,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3))); break; case 3: - builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 ))); + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(-1.25))); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -249,6 +250,7 @@ public class BlueBackStageAuto extends AutoBase { //goes back to the easel case 13: if (getRuntime() > macroTime + 0.5) { + robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java index f1dec5a..82f7481 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java @@ -21,15 +21,17 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config @Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueFrontPreload extends AutoBase { - public static final Pose2d DROP_1 = new Pose2d(-35, 32.5, Math.toRadians(0)); //THIS ANGLE NEEDS TO BE CHANGED + public static final Pose2d DROP_1 = new Pose2d(-36, 33.5, Math.toRadians(0)); + + public static final Pose2d DROP_1_PART_2 = new Pose2d(-32,33.5,Math.toRadians(0)); public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90)); public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90)); - public static final Pose2d DROP_3 = new Pose2d(-49, 33.5, Math.toRadians(-90)); - public static final Pose2d DROP_1M = new Pose2d(-48.5, 30, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90)); + public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90)); public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90)); - public static final Pose2d DROP_3M = new Pose2d(-48.5, 30, Math.toRadians(-90)); + public static final Pose2d DROP_3M = new Pose2d(-46.7, 39.5, Math.toRadians(-90)); public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187 @@ -74,7 +76,8 @@ public class BlueFrontPreload extends AutoBase { builder.lineToLinearHeading(DROP_2); break; case 3: - builder.lineToLinearHeading(DROP_3); + builder.lineToLinearHeading(DROP_3M); +// builder.lineToLinearHeading(DROP_3); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -93,20 +96,20 @@ public class BlueFrontPreload extends AutoBase { case 2: // intake if (getRuntime() < macroTime + 0.32) { - robot.intake.setDcMotor(-0.18); + robot.intake.setDcMotor(-0.23); } else{ builder = this.robot.getTrajectorySequenceBuilder(); robot.intake.setDcMotor(0); switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(DROP_2_PART_2); + builder.lineToLinearHeading(DROP_1_PART_2); break; case 2: builder.lineToLinearHeading(DROP_2_PART_2); break; case 3: - builder.lineToLinearHeading(DROP_2_PART_2); + builder.lineToLinearHeading(DROP_3); break; } robot.arm.setDoor(OPEN); @@ -188,7 +191,6 @@ case 5: if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(PARK_1); - builder.lineToLinearHeading(PARK_2); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } 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 2fc5903..a577b6c 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 @@ -77,11 +77,11 @@ public class MainTeleOp extends OpMode { this.robot.endGameMechs.reset(); } - if (controller1.getRightBumper().isPressed()) { - this.robot.endGameMechs.Finger_in(); - }else { - this.robot.endGameMechs.Finger_out(); - } +// if (controller1.getRightBumper().isPressed()) { +// this.robot.endGameMechs.Finger_in(); +// }else { +// this.robot.endGameMechs.Finger_out(); +// } //Hang Motor // if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){