diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java index 00c8ebf1b6c1..2ad41a7f1d44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainAuto.java @@ -1,117 +1,217 @@ -package org.firstinspires.ftc.teamcode; -import static dev.nextftc.extensions.pedro.PedroComponent.follower; +package org.firstinspires.ftc.teamcode; // make sure this aligns with class location +//HIIII pls don't delete +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierCurve; import com.pedropathing.geometry.BezierLine; import com.pedropathing.geometry.Pose; import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.pedropathing.util.Timer; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import org.firstinspires.ftc.teamcode.subsystems.Drive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Outtake; -import org.firstinspires.ftc.teamcode.subsystems.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Storage; -import org.firstinspires.ftc.teamcode.subsystems.Transitions; - -import dev.nextftc.core.commands.Command; -import dev.nextftc.core.commands.CommandManager; -import dev.nextftc.core.commands.delays.Delay; -import dev.nextftc.core.commands.groups.SequentialGroup; -import dev.nextftc.core.components.BindingsComponent; -import dev.nextftc.core.components.SubsystemComponent; -import dev.nextftc.extensions.pedro.FollowPath; -import dev.nextftc.ftc.NextFTCOpMode; -import dev.nextftc.ftc.components.BulkReadComponent; - -@Autonomous -public class MainAuto extends NextFTCOpMode { - { - addComponents( - BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop) - BindingsComponent.INSTANCE, - CommandManager.INSTANCE, - new SubsystemComponent( - Storage.INSTANCE, - Robot.INSTANCE, - Drive.INSTANCE, - Intake.INSTANCE, - Outtake.INSTANCE, - Transitions.INSTANCE - ) - ); - } - public static Pose endPose; +@Autonomous(name = "Main Auto") +public class MainAuto extends OpMode { + + private Follower follower; + private Timer pathTimer, actionTimer, opmodeTimer; + + private int pathState; + + + + /* + private final Pose poseA = new Pose(48, 12, Math.toRadians(180)); // Start Pose of our robot. + private final Pose poseB = new Pose(18, 84, Math.toRadians(135)); + private final Pose poseC = new Pose(74, 77, Math.toRadians(50)); + private final Pose poseD = new Pose(21, 60, Math.toRadians(50)); + private final Pose poseE = new Pose(73, 80, Math.toRadians(50)); + private final Pose poseF = new Pose(18, 36, Math.toRadians(50)); + private final Pose poseG = new Pose(75, 81, Math.toRadians(50)); + private Path startPosition; + private PathChain moveB, moveC, moveD, moveE, moveF; + */ + private final Pose startPose = new Pose(48, 12, Math.toRadians(90)); // Start Pose of our robot. + private final Pose pickUp1 = new Pose(18, 84, Math.toRadians(180)); - public static final Pose startPoseFarBlue = new Pose(56, 8, Math.toRadians(270)); // Start Pose of our robot. - public static final Pose scorePoseCloseBlue = new Pose(20, 123, Math.toRadians(323)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - public static final Pose scorePoseBlue = new Pose(68, 76, Math.toRadians(315)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + private final Pose pickUp1C1 = new Pose(57, 90); + private final Pose pickUp1C2 = new Pose(57, 84); - public static final Pose intakeAlign1Blue = new Pose(68, 84, Math.toRadians(180)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. - public static final Pose intake1Blue = new Pose(16, 84, Math.toRadians(180)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + private final Pose pickUp2 = new Pose(18, 60, Math.toRadians(180)); + private final Pose pickUp2C1 = new Pose(75, 57); + private final Pose pickUp2C2 = new Pose(66, 60); - public static final Pose startPoseFarRed = new Pose(87, 8, Math.toRadians(270)); // Start Pose of our robot. - public static final Pose startPoseCloseRed = new Pose(124, 123, Math.toRadians(37)); // Start Pose of our robot. - public static final Pose scorePoseRed = new Pose(76, 76, Math.toRadians(225)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + private final Pose pickUp3 = new Pose(18, 36, Math.toRadians(180)); + private final Pose pickUp3C1 = new Pose(72, 30); + private final Pose pickUp3C2 = new Pose(72, 36); - private final Pose startPose = startPoseFarBlue; - public static final Pose scorePose = scorePoseBlue; - //public static final Pose scorePosebutActually = new Pose(73, 70, Math.toRadians(135)); // Scoring Pose of our robot. It is facing the goal at a 135 degree angle. + private final Pose shootPose = new Pose(75, 87, Math.toRadians(330)); - Path scorePreload = new Path(new BezierLine(startPose, scorePose)); - Path intakeAlign1 = new Path(new BezierLine(scorePose, intakeAlign1Blue)); - Path intake1 = new Path(new BezierLine(intakeAlign1Blue, intake1Blue)); - Path intakeOut1 = new Path(new BezierLine(intake1Blue, intakeAlign1Blue)); - Path score1 = new Path(new BezierLine(intakeAlign1Blue, scorePose)); + private Path startPosition; + private PathChain shoot1, intake2, shoot2, intake3, shoot3, goToStart; + public void buildPaths() { + //startPosition = new Path(new BezierLine(startPose, pickUp1)); + startPosition = new Path(new BezierCurve(startPose, pickUp1C1, pickUp1C2, pickUp1)); + startPosition.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)); + shoot1 = follower.pathBuilder() + .addPath(new BezierLine(pickUp1, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + intake2 = follower.pathBuilder() + //.addPath(new BezierLine(shootPose, pickUp2)) + .addPath(new BezierCurve(shootPose, pickUp2C1, pickUp2C2 ,pickUp2)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .build(); + shoot2 = follower.pathBuilder() + .addPath(new BezierLine(pickUp2, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); - private Command autonomousRoutine() { - scorePreload.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading()); - intakeAlign1.setLinearHeadingInterpolation(scorePose.getHeading(), intakeAlign1Blue.getHeading()); - intake1.setLinearHeadingInterpolation(intakeAlign1Blue.getHeading(), intake1Blue.getHeading()); - score1.setLinearHeadingInterpolation(startPose.getHeading(), scorePose.getHeading()); + intake3 = follower.pathBuilder() + //.addPath(new BezierLine(shootPose, pickUp3)) + .addPath(new BezierCurve(shootPose, pickUp3C1, pickUp3C2 ,pickUp3)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180)) + .build(); + + shoot3 = follower.pathBuilder() + .addPath(new BezierLine(pickUp3, shootPose)) + .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(330)) + .build(); + + goToStart = follower.pathBuilder() + .addPath(new BezierLine(shootPose, startPose)) + .setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(90)) + .build(); + + + + } - int standardDelay = 1; - return new SequentialGroup( - new FollowPath(scorePreload), - new Delay(standardDelay), - Robot.outtakeAll, - new Delay(standardDelay), - new FollowPath(intakeAlign1), - new Delay(standardDelay), - // Start running intake procedure - new FollowPath(intake1), - new Delay(standardDelay), - new FollowPath(intakeOut1), - new Delay(standardDelay), - new FollowPath(score1), - Robot.outtakeAll - ); + public void autonomousPathUpdate() { + switch (pathState) { + case 0: + follower.followPath(startPosition); + setPathState(1); + break; + case 1: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot1, 0.7, true); + setPathState(2); + } + break; + case 2: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(intake2, .7,true); + setPathState(3); + } + break; + case 3: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot2, .7,true); + setPathState(4); + } + break; + case 4: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(intake3, .7,true); + setPathState(5); + } + break; + case 5: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(shoot3, .7,true); + setPathState(6); + + } + break; + case 6: + /* This case checks the robot's position and will wait until the robot position is close (1 inch away) from the scorePose's position */ + if(!follower.isBusy()) { + /* Score Preload */ + /* Since this is a pathChain, we can have Pedro hold the end point while we are grabbing the sample */ + follower.followPath(goToStart, .7,true); + setPathState(-1); + + } + } } + /** These change the states of the paths and actions. It will also reset the timers of the individual switches **/ + public void setPathState(int pState) { + pathState = pState; + pathTimer.resetTimer(); + } + @Override - public void onStartButtonPressed() { - follower().setStartingPose(startPose); - autonomousRoutine().schedule(); - follower().breakFollowing(); + public void loop() { + // These loop the movements of the robot, these must be called continuously in order to work + follower.update(); + autonomousPathUpdate(); + // Feedback to Driver Hub for debugging + telemetry.addData("path state", pathState); + telemetry.addData("x", follower.getPose().getX()); + telemetry.addData("y", follower.getPose().getY()); + telemetry.addData("heading", follower.getPose().getHeading()); + telemetry.update(); } - public void onUpdate(){ - Drive.telemetryM.update(); - follower().update(); + + + @Override + public void init() { + pathTimer = new Timer(); + opmodeTimer = new Timer(); + opmodeTimer.resetTimer(); + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setStartingPose(startPose); } - public void onStop(){ - endPose = follower().getPose(); + /** This method is called continuously after Init while waiting for "play". **/ + @Override + public void init_loop() {} + + + /** This method is called once at the start of the OpMode. + * It runs all the setup actions, including building paths and starting the path system **/ + @Override + public void start() { + opmodeTimer.resetTimer(); + setPathState(0); } -} \ No newline at end of file + /** We do not use this because everything should automatically disable **/ + @Override + public void stop() {} + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java new file mode 100644 index 000000000000..c0dca67cffaa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SecondaryTeleOp.java @@ -0,0 +1,173 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.subsystems.Drive; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; +import org.firstinspires.ftc.teamcode.subsystems.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Storage; +import org.firstinspires.ftc.teamcode.subsystems.Transitions; +import org.firstinspires.ftc.teamcode.utils.Logger; + +import dev.nextftc.core.commands.CommandManager; +import dev.nextftc.core.components.BindingsComponent; +import dev.nextftc.core.components.SubsystemComponent; +import dev.nextftc.ftc.ActiveOpMode; +import dev.nextftc.ftc.GamepadEx; +import dev.nextftc.ftc.Gamepads; +import dev.nextftc.ftc.NextFTCOpMode; +import dev.nextftc.ftc.components.BulkReadComponent; + +//PLEASE PUSHHH!H!! + +@TeleOp(name="SecondTeleOp", group="TeleOp") +public class SecondaryTeleOp extends NextFTCOpMode { + { + addComponents( + BulkReadComponent.INSTANCE, // TODO: make actual MANUAL mode bulkreading (we don't need to also read the expansion hub every loop) + BindingsComponent.INSTANCE, + CommandManager.INSTANCE, + new SubsystemComponent( + Storage.INSTANCE, + Robot.INSTANCE, + Drive.INSTANCE, + Intake.INSTANCE, + Outtake.INSTANCE, + Transitions.INSTANCE + ) + //new PedroComponent(Constants::createFollower) + ); + } + + @Override public void onInit() { + } + @Override public void onWaitForStart() { + ActiveOpMode.telemetry().update(); + } + @Override public void onStartButtonPressed() { + + GamepadEx caimo = Gamepads.gamepad1(); + GamepadEx jeff = Gamepads.gamepad2(); + //gamepad1. + // Debug stuff + jeff.back() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.resetEncoderAtOuttakeCommand().schedule(); + }); + + jeff.start() + .whenBecomesTrue(() -> { + Storage.setPIDMode(false).schedule(); + }); + +// + // Run Outake + jeff.rightBumper() + .whenBecomesTrue(() -> { + Outtake.setRunDownCommand(false).schedule(); + Outtake.on.schedule(); + }) + .whenBecomesFalse(() -> { + Outtake.setRunDownCommand(true).schedule(); + }); + + jeff.leftBumper() + .whenBecomesTrue(() -> { + Outtake.setRunDownCommand(true).schedule(); + Outtake.setOuttakePowerCommand(1).schedule(); + }) + .whenBecomesFalse(() -> { + Outtake.setRunDownCommand(true).schedule(); + Outtake.setOuttakePowerCommand(0).schedule(); + }); + + jeff.x() + .whenBecomesTrue(() -> Transitions.setOuttakePositionCommand(Transitions.UP_POS).schedule()) + .whenBecomesFalse(() -> Transitions.setOuttakePositionCommand(Transitions.DOWN_POS).schedule()); + + + // Drive Stuff + caimo.rightBumper() + .whenBecomesTrue(() -> Drive.setSlowModeCommand(true).schedule()) + .whenBecomesFalse(() -> Drive.setSlowModeCommand(false).schedule()); + + caimo.back() + .toggleOnBecomesTrue() + .whenBecomesTrue(() -> Drive.setHeadingLock(true)) + .whenBecomesFalse(() -> Drive.setHeadingLock(false)); + + caimo.back().and(caimo.start()) + .whenBecomesTrue(() -> Drive.resetDriveCommand().schedule()); + + caimo.rightTrigger().greaterThan(0.5) + .whenBecomesTrue(() -> Intake.setIntakePowerCommand(1).schedule()) + .whenBecomesFalse(() -> Intake.setIntakePowerCommand(0).schedule()); + caimo.leftTrigger().greaterThan(0.5) + .whenBecomesTrue(() -> Intake.setIntakePowerCommand(-1).schedule()) + .whenBecomesFalse(() -> Intake.setIntakePowerCommand(0).schedule()); + + jeff.dpadDown() + .whenBecomesTrue(() -> Storage.spinToNextIntakeIndex().schedule()); + jeff.dpadUp() + .whenBecomesTrue(() -> Storage.spinToNextOuttakeIndex().schedule()); + caimo.dpadDown() + .whenBecomesTrue(() -> Storage.spinToNextIntakeIndex().schedule()); + caimo.dpadUp() + .whenBecomesTrue(() -> Storage.spinToNextOuttakeIndex().schedule()); + + caimo.dpadLeft() + .whenBecomesTrue(() -> Robot.intakeAll.schedule()); + caimo.dpadRight() + .whenBecomesTrue(() -> Robot.outtakeAll.schedule()); + jeff.dpadLeft() + .whenBecomesTrue(() -> Robot.intakeAll.schedule()); + jeff.dpadRight() + .whenBecomesTrue(() -> Robot.outtakeAll.schedule()); + + jeff.a() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0.075).schedule(); + }) + .whenBecomesFalse(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0).schedule(); + }); + jeff.b() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0.5).schedule(); + }) + .whenBecomesFalse(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0).schedule(); + }); + jeff.y() + .whenBecomesTrue(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0.2).schedule(); + }) + .whenBecomesFalse(() -> { + Storage.setManualModeCommand(true).schedule(); + Storage.setManualPowerCommand(0).schedule(); + }); +// jeff.rightBumper() +// .whenBecomesTrue(() -> Outtake.setOuttakePowerCommand(1).schedule()) +// .whenBecomesFalse(() -> Outtake.setOuttakePowerCommand(0).schedule()); +// jeff.leftBumper() +// .whenBecomesTrue(() -> Outtake.setOuttakePowerCommand(0.8).schedule()) +// .whenBecomesFalse(() -> Outtake.setOuttakePowerCommand(0).schedule()); + + } + @Override public void onUpdate() { + for (String cname : CommandManager.INSTANCE.snapshot()) { + Logger.add("Commands", Logger.Level.DEBUG, cname); + } + Logger.update(Logger.Level.DEBUG); + } + + @Override public void onStop() { + } +}