diff --git a/MeepMeepTesting/.gitignore b/MeepMeepTesting/.gitignore new file mode 100644 index 0000000..42afabf --- /dev/null +++ b/MeepMeepTesting/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/MeepMeepTesting/build.gradle b/MeepMeepTesting/build.gradle new file mode 100644 index 0000000..49feefc --- /dev/null +++ b/MeepMeepTesting/build.gradle @@ -0,0 +1,17 @@ +plugins { + id 'java-library' +} + +java { + sourceCompatibility = JavaVersion.VERSION_1_8 + targetCompatibility = JavaVersion.VERSION_1_8 +} + +repositories { + maven { url = 'https://jitpack.io' } + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'com.github.NoahBres:MeepMeep:2.0.3' +} \ No newline at end of file diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java new file mode 100644 index 0000000..4fcc4b9 --- /dev/null +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -0,0 +1,32 @@ +package com.example.meepmeeptesting; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.noahbres.meepmeep.MeepMeep; +import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder; +import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity; + +public class MeepMeepTesting { + public static void main(String[] args) { + MeepMeep meepMeep = new MeepMeep(800); + + RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(-51.5, 33.6,Math.toRadians(180))) +// .lineToConstantHeading(new Vector2d(-45,59.5)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Pose2d(-48,59.5).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33,59.5).vec()) + .splineToConstantHeading(new Pose2d(52.5, 36).vec(),Math.toRadians(0)) + .build() + ); + + meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start(); + } +} \ No newline at end of file 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 a850233..3b82485 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 @@ -34,7 +34,7 @@ public class Slides { public static int manualSpeed = 50; - public enum Position { DOWN, TIER1, TIER2, TIER3 } + public enum Position { DOWN, mini_tier1, TIER1, TIER2, TIER3 } public Slides(HardwareMap hardwareMap) { slide = hardwareMap.get(DcMotor.class, "Rightslide"); @@ -57,6 +57,8 @@ public class Slides { public void setTarget(Position pos) { if (pos == Position.DOWN) { target = Math.min(Math.max(down, targetMin), targetMax); + } else if (pos == Position.mini_tier1) { + target = Math.min(Math.max(mini_tier1, targetMin), targetMax); } else if (pos == Position.TIER1) { target = Math.min(Math.max(tier1, targetMin), targetMax); } else if (pos == Position.TIER2) { 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 a4fdcfd..5073180 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 @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.hardware; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -9,17 +11,34 @@ public class endGame_Mechs { private Servo servo; private Servo hang1; private Servo hang2; - public static double initPos = 0.8; - public static double launchPos = 0.4; + private DcMotor hang; + public static double initPos = 0.42; + public static double launchPos = 0.8; + public static int initHang = -1000; public static double hold = 0.8; public static double release = 0.5; public static double hold2 = 0.8; public static double release2 = 0.8; + public static int HangUp = -860; + public static int Hanged = -30; + private int target = 0; + public static int down = 0; + + public static int manualSpeed = 50; + + + public endGame_Mechs(HardwareMap hardwareMap) { this.servo = hardwareMap.get(Servo.class, "Drone"); this.servo.setPosition(initPos); -// this.hang1 = hardwareMap.get(Servo.class, "Hanger 1"); + this.hang = hardwareMap.get(DcMotor.class, "Hang"); + this.hang.setTargetPosition(0); + this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.hang.setMode(DcMotor.RunMode.RUN_TO_POSITION); + this.hang.setDirection(DcMotorSimple.Direction.REVERSE); + this.hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // this.hang1 = hardwareMap.get(Servo.class, "Hanger 1"); // this.hang1.setPosition(hold); // this.hang2 = hardwareMap.get(Servo.class, "Hanger 2"); // this.hang2.setPosition(hold); @@ -35,16 +54,19 @@ public class endGame_Mechs { } - public void release() { - this.servo.setPosition(release); - this.servo.setPosition(release2); + public void hang_init_pos(){ + this.hang.setTargetPosition(0); + this.hang.setPower(1); } - public void hold() { - this.servo.setPosition(hold); - this.servo.setPosition(hold2); + public void hang_final_pos(){ + this.hang.setTargetPosition(2250); + this.hang.setPower(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 52e7ef7..f786cc1 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 @@ -25,7 +25,7 @@ public class BlueBackStageAuto extends AutoBase { public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180)); - public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90)); + public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); @@ -222,8 +222,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2))); break; case 3: - builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 - ))); + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 ))); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStagePark.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStagePark.java new file mode 100644 index 0000000..4b6987c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStagePark.java @@ -0,0 +1,313 @@ +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; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.Arm; +import org.firstinspires.ftc.teamcode.hardware.Slides; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Config +@Autonomous(name = "Blue Backstage Auto(2 and Park)", group = "Competition", preselectTeleOp = "Main TeleOp") +public class BlueBackStagePark extends AutoBase { + public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180)); + + public static final Pose2d DROP_3M = new Pose2d(13.8, 32, 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(24.5, 43, Math.toRadians(-90)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, 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.1 , Math.toRadians(-180));//-36 + + public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180)); + + public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180)); + + public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180)); + + public static final Pose2d PARK_1 = new Pose2d(-53,58,Math.toRadians(-180)); + + public static final Pose2d PARK_2 = new Pose2d(-58,58,Math.toRadians(-180)); + + + @Override + public void createTrajectories() { + // set start position + Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90)); + robot.drive.setPoseEstimate(start); + robot.camera.setAlliance(CenterStageCommon.Alliance.Blue); + } + + @Override + public void followTrajectories() { + TrajectorySequenceBuilder builder = null; + switch (macroState) { + case 0: + 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); + builder.lineToLinearHeading(DROP_3M); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + break; + // DRIVE TO TAPE + case 1: + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + macroState++; + } + break; + // RUN INTAKE + case 2: + // intake + if (getRuntime() < macroTime + 0.1) { + robot.intake.setDcMotor(-0.24); + } + // if intake is done move on + else { + robot.intake.setDcMotor(0); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); + robot.extendMacro(Slides.mini_tier1, getRuntime()); + builder = this.robot.getTrajectorySequenceBuilder(); + //Scores yellow pixel + 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; + // EXTEND AND MOVE TO BACKBOARD + case 3: + // extend macro + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + // if macro and drive are done, move on + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + macroTime = getRuntime(); + } + break; + // STACK RUN 1 ------------------------- + case 4: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { +// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); +// builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(PARK_1); + builder.lineToLinearHeading(PARK_2); + +// switch (teamPropLocation) { +// case 1: +// builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0))); +// break; +// case 2: +// builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0))); +// break; +// case 3: +// builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5))); +// break; +// } +// this.robot.drive.followTrajectorySequenceAsync(builder.build()); +// macroState++; + } + break; +// //waits for the robot to fin the trajectory +// case 5: +// robot.resetMacro(0, getRuntime()); +// robot.intake.setDcMotor(0.68); +// robot.intake.setpos(STACK5); +// if (!robot.drive.isBusy()) { +// macroState++; +// } +// break; +// //First 2 pixels off the stack are intaken by this +// case 6: +// robot.intake.setDcMotor(0.68); +// robot.intake.setpos(STACK5); +// macroTime = getRuntime(); +// macroState++; +// break; +// //goes back to the easel +// case 7: +// if (getRuntime() > macroTime + 0.03) { +// //robot.intake.setDcMotor(-0.0); +// robot.intake.setDcMotor(-0.35); +// builder = this.robot.getTrajectorySequenceBuilder(); +// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); +// builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); +// switch (teamPropLocation) { +// case 1: +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); +// break; +// case 2: +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); +// break; +// case 3: +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); +// break; +// } +// this.robot.drive.followTrajectorySequenceAsync(builder.build()); +// macroState++; +// macroTime = getRuntime(); +// } +// break; +// case 8: +// // if drive is done move on +// if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { +// macroTime = getRuntime(); +// robot.macroState = 0; +// 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 + 80, getRuntime()); +// } +// if (robot.macroState == 0 && !robot.drive.isBusy()) { +// robot.resetMacro(0, getRuntime()); +// macroState++; +// macroTime = getRuntime(); +// } +// break; +// // STACK RUN 2 +// case 10: +// robot.resetMacro(0, getRuntime()); +// if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { +// builder = this.robot.getTrajectorySequenceBuilder(); +// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); +// builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); +// switch (teamPropLocation) { +// case 1: +// builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2))); +// break; +// case 2: +// builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2))); +// break; +// case 3: +// builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 +// ))); +// break; +// } +// this.robot.drive.followTrajectorySequenceAsync(builder.build()); +// macroState++; +// } +// break; +// //waits for the robot to fin the trajectory +// case 11: +// robot.resetMacro(0, getRuntime()); +// robot.intake.setDcMotor(0.68); +// robot.intake.setpos(STACK3); +// if (!robot.drive.isBusy()) { +// macroState++; +// } +// break; +// //Third and 4th pixels off the stack are intaken by this +// case 12: +// robot.intake.setDcMotor(0.68); +// robot.intake.setpos(STACK2); +// macroTime = getRuntime(); +// macroState++; +// break; +// //goes back to the easel +// case 13: +// if (getRuntime() > macroTime + 0.03) { +// robot.intake.setDcMotor(-0.35); +// builder = this.robot.getTrajectorySequenceBuilder(); +// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); +// builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); +// switch (teamPropLocation) { +// case 1: +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); +// break; +// case 2: +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); +// break; +// case 3: +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); +// break; +// } +// this.robot.drive.followTrajectorySequenceAsync(builder.build()); +// macroState++; +// macroTime = getRuntime(); +// } +// break; +// case 14: +// // if drive is done move on +// if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { +// macroTime = getRuntime(); +// robot.macroState = 0; +// robot.extendMacro(Slides.mini_tier1 + 140, getRuntime()); +// macroState++; +// } +// break; +// //extending the macro and about to score +// case 15: +// if (robot.macroState != 0) { +// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); +// } +// if (robot.macroState == 0 && !robot.drive.isBusy()) { +// robot.resetMacro(0, getRuntime()); +// macroState++; +// } +// break; +// +// // PARK ROBOT + case 5: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; +// builder.lineToLinearHeading(PARK_1); +// builder.lineToLinearHeading(PARK_2); +// builder = this.robot.getTrajectorySequenceBuilder(); + } +// +// // PARK ROBOT +//// + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java index 9baa7f6..0a81e60 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java @@ -21,15 +21,20 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config @Autonomous(name = "Blue FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueFrontStageAuto extends AutoBase { - public static final Pose2d DROP_1 = new Pose2d(-50, 45.5, Math.toRadians(-90)); + 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_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90)); - public static final Pose2d DROP_3 = new Pose2d(-33.5, 40.5, Math.toRadians(180)); + 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 DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, 35.3, Math.toRadians(8));//187 + public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.5, 36, Math.toRadians(8));//187 + public static final Pose2d DROP_3M = new Pose2d(-48.5, 30, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, 32, Math.toRadians(8));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 32, Math.toRadians(8));//817 public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180)); @@ -37,7 +42,7 @@ public class BlueFrontStageAuto extends AutoBase { public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180)); - public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 60.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 59.5, Math.toRadians(180)); public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180)); @@ -81,20 +86,19 @@ public class BlueFrontStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.22) { + if (getRuntime() < macroTime + 0.32) { robot.intake.setDcMotor(-0.18); } else{ builder = this.robot.getTrajectorySequenceBuilder(); robot.intake.setDcMotor(0); robot.arm.setDoor(OPEN); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.8,1.5))).waitSeconds(.01); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01); robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK6); macroTime = getRuntime(); - builder.lineToLinearHeading(POST_DROP_POS); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -112,7 +116,7 @@ public class BlueFrontStageAuto extends AutoBase { case 4: if (getRuntime() > macroTime + 0.06) { robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.5); + robot.intake.setDcMotor(-0.8); builder = this.robot.getTrajectorySequenceBuilder(); // //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); // builder.lineToConstantHeading(POST_DROP_POS.vec()); @@ -120,17 +124,41 @@ public class BlueFrontStageAuto extends AutoBase { switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; +// case 1: +// builder.setTangent(Math.toRadians(90)) +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); +// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); +// break; +// case 2: +// builder.lineToLinearHeading(POST_DROP_POS); +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); +// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); +// break; +// case 3: +// builder.lineToLinearHeading(POST_DROP_POS); +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); +// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); +// break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -142,7 +170,7 @@ public class BlueFrontStageAuto extends AutoBase { if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1-70, getRuntime()); macroState++; } break; @@ -164,7 +192,7 @@ public class BlueFrontStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,-2)).vec(), Math.toRadians(180)); builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,-2)).vec()); - builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.7, -3.5)).vec(), Math.toRadians(180)); + builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180)); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -194,22 +222,28 @@ public class BlueFrontStageAuto extends AutoBase { robot.intake.setDcMotor(-0.45); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); - builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); + //builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2))); switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToLinearHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,-2))); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -222,7 +256,7 @@ public class BlueFrontStageAuto extends AutoBase { if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 20, getRuntime()); macroState++; } break; 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 97a74e4..f414679 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 @@ -29,7 +29,7 @@ public class RedBackStageAuto extends AutoBase { 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, -26.3, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -28.3, Math.toRadians(180)); public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -34.5, Math.toRadians(190)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(190)); @@ -41,13 +41,13 @@ 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(27, -10.2, Math.toRadians(190));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11.2, Math.toRadians(185));//-36 - public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11.2, Math.toRadians(185)); - public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11.2, Math.toRadians(185)); - public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185)); + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11.2, Math.toRadians(185)); @Override public void createTrajectories() { @@ -101,7 +101,7 @@ public class RedBackStageAuto extends AutoBase { //Scores yellow pixel switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); + builder.lineToLinearHeading(DEPOSIT_PRELOAD_1 ); break; case 2: robot.extendMacro(Slides.mini_tier1 -20, getRuntime()); @@ -140,7 +140,7 @@ public class RedBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3))); + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-5))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3))); 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 924242b..8e33845 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 @@ -19,27 +19,32 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config -@Autonomous(name = "Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "JANK Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedFrontStageAuto extends AutoBase { - public static final Pose2d DROP_1 = new Pose2d(-50, -45.5, Math.toRadians(90)); + public static final Pose2d DROP_1 = new Pose2d(-35, -32.5, Math.toRadians(180)); //THIS ANGLE NEEDS TO BE CHANGED public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90)); - public static final Pose2d DROP_3 = new Pose2d(-33.5, -40.5, Math.toRadians(0)); + 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 DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187 + public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50.5, -33, Math.toRadians(188));//187 + public static final Pose2d DROP_3M = new Pose2d(-48.5, -30, Math.toRadians(90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187 - public static final Pose2d STACK_LOCATION = new Pose2d(-54.5, -33.6, Math.toRadians(0)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -32, Math.toRadians(188));//817 + + public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, -33.6, Math.toRadians(0)); 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(-45, -57.5, Math.toRadians(0)); + public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0)); - public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(0)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(0)); + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0)); @Override public void createTrajectories() { @@ -81,14 +86,16 @@ public class RedFrontStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.22) { + if (getRuntime() < macroTime + 0.32) { robot.intake.setDcMotor(-0.18); } else{ builder = this.robot.getTrajectorySequenceBuilder(); robot.intake.setDcMotor(0); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-2,-1.5))); robot.arm.setDoor(OPEN); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,1.5))).waitSeconds(.01); + + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK6); macroTime = getRuntime(); @@ -109,25 +116,49 @@ public class RedFrontStageAuto extends AutoBase { case 4: if (getRuntime() > macroTime + 0.06) { robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.44); + robot.intake.setDcMotor(-0.8); 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)); +// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); +// builder.lineToConstantHeading(POST_DROP_POS.vec()); +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0)); switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180)); break; case 2: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180)); break; case 3: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180)); break; +// case 1: +// builder.setTangent(Math.toRadians(90)) +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); +// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); +// break; +// case 2: +// builder.lineToLinearHeading(POST_DROP_POS); +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); +// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); +// break; +// case 3: +// builder.lineToLinearHeading(POST_DROP_POS); +// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); +// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); +// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); +// break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -139,7 +170,7 @@ public class RedFrontStageAuto extends AutoBase { if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1-70, getRuntime()); macroState++; } break; @@ -157,11 +188,11 @@ public class RedFrontStageAuto extends AutoBase { //Stack run 2 case 7: robot.resetMacro(0, getRuntime()); - if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) { + if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(0)); - builder.lineToLinearHeading(POST_DROP_POS); - builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-0.5)).vec(), Math.toRadians(0)); + builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,2)).vec(), Math.toRadians(0)); + builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,2)).vec()); + builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, 3.7)).vec(), Math.toRadians(0)); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -187,25 +218,32 @@ public class RedFrontStageAuto extends AutoBase { case 10: if (getRuntime() > macroTime + 0.6) { + robot.arm.setDoor(CLOSE); robot.intake.setDcMotor(-0.45); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); - builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); + //builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2))); switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToLinearHeading(PRE_DEPOSIT_POS); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -218,7 +256,7 @@ public class RedFrontStageAuto extends AutoBase { if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 20, getRuntime()); macroState++; } break; @@ -232,8 +270,13 @@ public class RedFrontStageAuto extends AutoBase { macroState++; } break; + case 13: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; + } - //stack run 3 + //stack run 3 // case 13: // robot.resetMacro(0, getRuntime()); // if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) { @@ -319,3 +362,4 @@ public class RedFrontStageAuto extends AutoBase { } } + 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 32fbc5b..80913f9 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 @@ -27,6 +27,8 @@ public class MainTeleOp extends OpMode { private Controller controller1; private Controller controller2; + public boolean hang_counter = false; + @Override public void init() { controller1 = new Controller(gamepad1); @@ -34,7 +36,7 @@ public class MainTeleOp extends OpMode { this.robot = new Robot(hardwareMap); // robot.intake.setpos(Intake.Position.STACK1); - this.robot.endGameMechs.hold(); + // while (robot.camera.getFrameCount() < 1) { // telemetry.addLine("Initializing..."); // telemetry.update(); @@ -68,6 +70,31 @@ public class MainTeleOp extends OpMode { z *= normal; } robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + // Drone launcher + if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + this.robot.endGameMechs.launch(); + } else { + this.robot.endGameMechs.reset(); + } + + //Hang Motor + +// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){ +// this.robot.endGameMechs.hang_init_pos(); +// hang_counter = true; +// } +// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){ +// this.robot.endGameMechs.hang_final_pos(); +// hang_counter = false; +// } + + if (controller1.getB().isPressed()) { + this.robot.endGameMechs.hang_final_pos(); + } else { + this.robot.endGameMechs.hang_init_pos(); + } + + // Driver 2 if (controller2.getRightTrigger().getValue()>=0.1){ @@ -87,16 +114,8 @@ public class MainTeleOp extends OpMode { robot.intake.decrementPos(); } - // Drone launcher - if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { - this.robot.endGameMechs.launch(); - } else { - this.robot.endGameMechs.reset(); - } - //Hang Servos - if (controller1.getX().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { - this.robot.endGameMechs.release(); - } + + // macros diff --git a/settings.gradle b/settings.gradle index 9e2cfb3..a7f210f 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,2 +1,3 @@ include ':FtcRobotController' include ':TeamCode' +include ':MeepMeepTesting'