From 766d7593fd966a045fd46acef268b8e7bb43ffb0 Mon Sep 17 00:00:00 2001 From: Eoghanmc22 Date: Fri, 30 Aug 2024 23:43:03 -0400 Subject: [PATCH] Minor cleanup --- src/main/java/frc/robot/Autonomous.java | 1118 ++++++++--------- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 15 +- .../robot/commands/DefaultDriveCommand.java | 28 +- .../java/frc/robot/commands/DriveCommand.java | 5 +- .../frc/robot/utilities/SwerveModule.java | 5 +- 6 files changed, 593 insertions(+), 580 deletions(-) diff --git a/src/main/java/frc/robot/Autonomous.java b/src/main/java/frc/robot/Autonomous.java index b92ee6c..bfacb5a 100644 --- a/src/main/java/frc/robot/Autonomous.java +++ b/src/main/java/frc/robot/Autonomous.java @@ -18,20 +18,20 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, this.drivetrainSubsystem = drivetrain; // RobotContainer.autonomousChooserFirstStep.getSelected(); RobotContainer.autonomousAim.setDefaultOption("False", false); - RobotContainer.autonomousChooserFirtWait.setDefaultOption("0", Integer.valueOf(0)); + RobotContainer.autonomousChooserFirstWait.setDefaultOption("0", Integer.valueOf(0)); RobotContainer.autonomousChooserFirstStep.setDefaultOption("0", Integer.valueOf(-1)); RobotContainer.autonomousChooserLastStep.setDefaultOption("0", Integer.valueOf(-1)); RobotContainer.autonomousAim.addOption("True", true); for (int i = 0; i < 6; ++i) { - RobotContainer.autonomousChooserFirtWait.addOption("" + (i), Integer.valueOf(i)); + RobotContainer.autonomousChooserFirstWait.addOption("" + (i), Integer.valueOf(i)); } for (int i = 0; i < 16; ++i) { RobotContainer.autonomousChooserFirstStep.addOption("" + (i + 1), Integer.valueOf(i)); RobotContainer.autonomousChooserLastStep.addOption("" + (i + 1), Integer.valueOf(i)); } - SmartDashboard.putData("Auto Start Wait", RobotContainer.autonomousChooserFirtWait); + SmartDashboard.putData("Auto Start Wait", RobotContainer.autonomousChooserFirstWait); SmartDashboard.putData("Auto Aim", RobotContainer.autonomousAim); SmartDashboard.putData("Autonomous Start", RobotContainer.autonomousChooserFirstStep); SmartDashboard.putData("Autonomous End", RobotContainer.autonomousChooserLastStep); @@ -97,7 +97,6 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, { 8.71, 0.80 } // 16 }; - public static final double accendingNoteInitialPosition[][] = new double[][] { { 1.30, 4.16, 180 }, // 1 { 1.30, 5.54, 180 }, // 2 -- good @@ -117,10 +116,9 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, { 8.71, 0.80, 0 } // 16 }; - public static final double descendingNoteInitialPosition[][] = new double[][] { { 2.15, 3.16, 130 }, // 1 - { 1.30, 5.54, 180 }, // 2 -- good + { 1.30, 5.54, 180 }, // 2 -- good { 1.30, 7.04, 180 }, // 3 { 7.71, 7.49, 180 }, // 4 { 7.71, 5.82, 180 }, // 5 @@ -137,7 +135,6 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, { 8.71, 0.80, 0 } // 16 }; - public static final double shootAccendingPosition[][] = new double[][] { { 1.30, 4.16 }, // 1 { 1.30, 5.54 }, // 2 -- good @@ -158,7 +155,7 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, }; public static final double shootDescendingPosition[][] = new double[][] { - { 1.30, 4.16 }, // 1 + { 1.30, 4.16 }, // 1 { 1.30, 5.54 }, // 2 -- good { 1.30, 7.04 }, // 3 { 1.30, 7.49 }, // 4 @@ -176,7 +173,7 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, { 14.02, 0.80 } // 16 }; - public static final double chaosPosition[][] = new double[][] { + public static final double chaosPosition[][] = new double[][] { { 7.71, 7.49, 45 }, // 4 { 7.71, 5.82, 45 }, // 5 { 7.71, 4.14, 45 }, // 6 @@ -197,559 +194,562 @@ public Autonomous(RobotContainer robotContainer, DrivetrainSubsystem drivetrain, public static final double allowedShootYBlueDown = 1.58; public static final double allowedShootYRedDown = 1.58; - public static Command getAutonomousCommand(RobotContainer robotContainer, int from, int until, - boolean aimFirst, int delay) { - //double robotAngle = (from >= 8) ? 0 : 180; - - //double shootFromPosition[][] = (from <= until) ? shootAccendingPosition : shootDescendingPosition; + public static Command getAutonomousCommand(RobotContainer robotContainer, int from, int until, + boolean aimFirst, int delay) { + // double robotAngle = (from >= 8) ? 0 : 180; + + // double shootFromPosition[][] = (from <= until) ? shootAccendingPosition : + // shootDescendingPosition; Command command = null; return command; - // if (aimFirst) { - // command = new ShootToSpeakerCommand(robotContainer.shooterSubsystem, - // robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.limeLightPoseSubsystem, - // robotContainer.drivetrainSubsystem) - // .alongWith(new SpeakerAlligningCommand( - // robotContainer.limeLightPoseSubsystem, - // robotContainer.drivetrainSubsystem)); - // } else { - // command = - // new Command() { - // @Override - // public void initialize() { - // robotContainer.shooterSubsystem.setTiltAngle(-4); - // } - // @Override - // public boolean isFinished() { - // return true; - // } - // }; - // command = command.andThen( - // new WaitUntilShooterReadyCommand(robotContainer.shooterSubsystem) - // .andThen(new GrabberOutCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.3))); - // } - // if (delay > 0) { - // command = new WaitCommand(delay).andThen(command); - // } - // if (from == -1) { - // return command; - // } - // /** - // * from can be higher than until. - // * In that case, we go in reverse order - // * look at the end of the loop to see how - // * i gets incremented, or decremented. - // */ - // for (int i = from; i != until;) { - // //command = command - // // .andThen(new IntakeNoteCommand(robotContainer.intakeSubsystem)); - // // IntakeCommand.State.OUT, 4000)); - // if (shootFromPosition[i][0] == noteInitialPosition[i][0] && - // shootFromPosition[i][1] == noteInitialPosition[i][1]) { - // //command = command - // // .andThen(new WaitUntilIntakeOutCommand(robotContainer.intakeSubsystem)); - // } - // double pickupX = noteInitialPosition[i][0]; - // double pickupY = noteInitialPosition[i][1]; - // double pickupAlpha = robotAngle; - - // if (from < until) { - // pickupX = accendingNoteInitialPosition[i][0]; - // pickupY = accendingNoteInitialPosition[i][1]; - // pickupAlpha = accendingNoteInitialPosition[i][2]; - // } else if (until < from) { - // pickupX = descendingNoteInitialPosition[i][0]; - // pickupY = descendingNoteInitialPosition[i][1]; - // pickupAlpha = descendingNoteInitialPosition[i][2]; - // } - - // command = command - // .andThen( - // new StraightPathCommand(robotContainer.drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(pickupX, - // pickupY, - // Rotation2d.fromDegrees( - // pickupAlpha))) - // .alongWith(new GrabberInCommand( - // robotContainer.grabberSubsystem))) - // .andThen(grabNoteCommand(robotContainer)); - // //.andThen(new WaitCommand(0.1)); - - // if (shootFromPosition[i][0] != noteInitialPosition[i][0] || - // shootFromPosition[i][1] != noteInitialPosition[i][1]) { - // command = command - // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.shooterSubsystem) - // .alongWith( - // new StraightPathCommand( - // robotContainer.drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(shootFromPosition[i][0], - // shootFromPosition[i][1], - // Rotation2d.fromDegrees( - // robotAngle))))); - // } else { - // command = command - // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.shooterSubsystem) - // .alongWith(new SpeakerAlligningCommand( - // robotContainer.limeLightPoseSubsystem, - // robotContainer.drivetrainSubsystem))); - // } - // command = command - // .andThen(new ShootToSpeakerCommand(robotContainer.shooterSubsystem, - // robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.limeLightPoseSubsystem, - // robotContainer.drivetrainSubsystem)); - // if (from < until) { - // ++i; - // } else { - // --i; - // } - // } - // return command; - // } - - // public static Command getAutonomousCommand(RobotContainer robotContainer, int from, int until, boolean accending) { - // double robotAngle = (from >= 8) ? 0 : 180; - - // double shootFromPosition[][] = (accending) ? shootAccendingPosition : shootDescendingPosition; - - // Command command = - // // new ResetOdometryWithCameraCommand(robotContainer.limeLightPoseSubsystem) - // // .andThen( - // new WaitUntilShooterReadyCommand(robotContainer.shooterSubsystem) - // .andThen(new GrabberOutCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.3)); - // if (from == -1) { - // return command; - // } - // /** - // * from can be higher than until. - // * In that case, we go in reverse order - // * look at the end of the loop to see how - // * i gets incremented, or decremented. - // */ - // for (int i = from; i != until;) { - // // command = command - // // .andThen(new IntakeNoteCommand(robotContainer.intakeSubsystem)); - // //IntakeCommand.State.OUT, 4000)); - // if (shootFromPosition[i][0] == noteInitialPosition[i][0] && - // shootFromPosition[i][1] == noteInitialPosition[i][1]) { - // //command = command - // // .andThen(new WaitUntilIntakeOutCommand(robotContainer.intakeSubsystem)); - // } - // double pickupX = noteInitialPosition[i][0]; - // double pickupY = noteInitialPosition[i][1]; - // double pickupAlpha = robotAngle; - - // if (from != until) { - // if (accending) { - // pickupX = accendingNoteInitialPosition[i][0]; - // pickupY = accendingNoteInitialPosition[i][1]; - // pickupAlpha = accendingNoteInitialPosition[i][2]; - // } else { - // pickupX = descendingNoteInitialPosition[i][0]; - // pickupY = descendingNoteInitialPosition[i][1]; - // pickupAlpha = descendingNoteInitialPosition[i][2]; - // } - // } - - // command = command - // .andThen( - // new StraightPathCommand(robotContainer.drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(pickupX, - // pickupY, - // Rotation2d.fromDegrees( - // pickupAlpha))) - // .alongWith(new GrabberInCommand( - // robotContainer.grabberSubsystem))) - // .andThen(grabNoteCommand(robotContainer)); - - // if (shootFromPosition[i][0] != noteInitialPosition[i][0] || - // shootFromPosition[i][1] != noteInitialPosition[i][1]) { - // command = command - // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.shooterSubsystem) - // .alongWith( - // new StraightPathCommand( - // robotContainer.drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(shootFromPosition[i][0], - // shootFromPosition[i][1], - // Rotation2d.fromDegrees( - // robotAngle))))); - // } else { - // command = command - // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.shooterSubsystem) - // .alongWith(new SpeakerAlligningCommand( - // robotContainer.limeLightPoseSubsystem, - // robotContainer.drivetrainSubsystem))); - // } - // command = command - // .andThen(new ShootToSpeakerCommand(robotContainer.shooterSubsystem, - // robotContainer.intakeSubsystem, - // robotContainer.grabberSubsystem, - // robotContainer.limeLightPoseSubsystem, - // robotContainer.drivetrainSubsystem)); - // if (accending) { - // ++i; - // } else { - // --i; - // } - // if (from < 8) { - // if (i < 0) { - // i = 7; - // } else if (i > 7) { - // i = 0; - // } - // } else { - // if (i < 8) { - // i = 15; - // } else if (i > 15) { - // i = 8; - // } - // } - // } - // return command; - //} - - //public static Command grabNoteCommand(RobotContainer robotContainer) { - //return new GrabNoteCommandAutonomous(robotContainer, true); - // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.1)) - // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.1)) - // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.1)) - // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.1)) - // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) - // .andThen(new WaitCommand(0.1)); - //} - - // public Command getAutonomousCommandCase2() { - // Command command = new DisplayLogCommand("Case 2") - // .andThen(new WaitCommand(0.5)); - // return command; - // } - - // public Command caseCommand(String name, IntakeSubsystem intakeSubsystem, - // double deltaY) { - // return new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, (180)) - // // .andThen(new SetModeConeCube(RobotMode.Cube)) - // // .andThen(new PositionCommand(this, OperatorButtons.LOW)) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)) - // // .andThen(new PositionCommand(this, OperatorButtons.HOME)) - // .andThen( - // new StraightPathCommand(drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(4.49, 5.08, - // new Rotation2d(Math.toRadians(180))))) - // .andThen(new RotateCommand(drivetrainSubsystem)) - // .andThen(new StraightPathCommand(drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(4.79, 5.08, new Rotation2d(Math.toRadians(0))))) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 1000)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) - // .andThen(new RotateCommand(drivetrainSubsystem)) - // .andThen(new StraightPathCommand(drivetrainSubsystem, - // robotContainer.limeLightPoseSubsystem, - // new Pose2d(1.89, 4.88, new Rotation2d(Math.toRadians(180))))) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)); - // } - - // public Command testMovements(DrivetrainSubsystem drivetrainSubsystem, - // IntakeSubsystem intake, - // RobotContainer robotContainer) { - // return new DisplayLogCommand("Test Case") - // // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, .4, 0, 0, - // 500)) - // // .andThen(new SetModeConeCube(RobotMode.Cube)) - // // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, 0, 0, 0, 50)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.LOW)) - // .andThen(new IntakeCommand(intake, State.OUT, 1000)) - // .andThen(new WaitCommand(.5)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) - // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, -.3, 0, 0, 1000)) - // .andThen(new WaitCommand(.25)) - // .andThen(new RotateCommand(drivetrainSubsystem)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) - // .andThen(new IntakeCommand(intake, State.IN, 1000)) - // .andThen(new RobotOrientedDriveDeacceleratedCommand(drivetrainSubsystem, -.3, - // 0, 0, - // 300)); - // // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, 0, 0, 0, 50)); - // } - - // private Supplier poseEstimator; - - // public Command caseCommandOld(String name, IntakeSubsystem intakeSubsystem, - // double speedY, int durationY) { - // Command command = new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, - // (180)) - // // .andThen(new SetModeConeCube(RobotMode.Cube)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HIGH)) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 1000)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) - // .andThen(new RobotOrientedDriveDeacceleratedCommand(drivetrainSubsystem, 0, - // speedY, 0, - // durationY)) - // .andThen(new WaitCommand(0.1)) - // .andThen(new RobotOrientedDriveDeacceleratedCommand(drivetrainSubsystem, -.7, - // 0, 0, - // 1500)) - // .andThen(new RotateCommand(drivetrainSubsystem)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 1000)); - // command.setName(name); - // return command; - // } - - // public Command test2() { - // Command command = new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, - // (180)) - // .andThen(new StraightPathCommand(drivetrainSubsystem, poseEstimator, - // new Pose2d(new Translation2d(2.1, - // KeyPadPositionSupplier.FIELD_WIDTH - 5.24), - // new Rotation2d(Math.toRadians(180))))) - // .andThen(new DriveCommand(drivetrainSubsystem, -2, 0, 100)) - // .andThen(new WaitCommand(0.5)) - // .andThen(new DriveCommand(drivetrainSubsystem, -2, 0, 0)) - // .andThen(new WaitCommand(2.5)) - // .andThen(new DriveCommand(drivetrainSubsystem, -0.05, 0, 0)) - // .andThen(new WaitCommand(2)) - // .andThen(new DriveCommand(drivetrainSubsystem, 0, 0, 0)) - // .andThen(new BalanceCommand(drivetrainSubsystem)); - // command.setName("Test 2"); - // return command; - // } - - // public Command getOverAndBalanceCommand(DrivetrainSubsystem - // m_drivetrainSubsystem) { - // BalanceCommand balanceCommand = new BalanceCommand(m_drivetrainSubsystem); - - // return new Command() { - // @Override - // public void initialize() { - // DefaultDriveCommand.autonomous = true; - // SwerveModule.setPowerRatio(1.5); - // balanceCommand.zeroGyroscope(); - // } - - // @Override - // public boolean isFinished() { - // return true; - // } - - // } - // // .andThen(new SetModeConeCube(RobotMode.Cube)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HIGH)) - // .andThen(new IntakeCommand(robotContainer.intakeSubsystem, - // IntakeCommand.State.OUT, - // 300)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) - // .andThen(new RobotOrientedDriveCommand(m_drivetrainSubsystem, -0.03, 0, 0, - // 3500)) - // .andThen(new RobotOrientedDriveCommand(m_drivetrainSubsystem, 0.02, 0, 0, - // 800)) - // .andThen(balanceCommand); - // } - - // public static Command getPieceWithCoral(RobotContainer robotContainer, - // LimeLightPoseSubsystem limeLightPoseSubsystem, - // String splineGetCube, String splineDropCube, Pose2d cubePose) { - // DrivetrainSubsystem drivetrainSubsystem = robotContainer.drivetrainSubsystem; - // IntakeSubsystem intakeSubsystem = robotContainer.intakeSubsystem; - // return - // // new SetModeConeCube(RobotMode.Cube) - // // .andThen( - // new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)// ) - // .andThen(new TrajectoryUntilSeeingCube(splineGetCube, - // drivetrainSubsystem, limeLightPoseSubsystem)) - // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 2500) - // .alongWith(new DriveToObjectCommand(drivetrainSubsystem, "cube")))// - // .andThen(new - // // PositionCommand(robotContainer, - // // OperatorButtons.HOME)) - // .andThen(new TrajectoryCommand(splineDropCube, - // drivetrainSubsystem, limeLightPoseSubsystem)) - // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)); - // } - - // static Command grabCube( - // RobotContainer robotContainer, - // DrivetrainSubsystem drivetrainSubsystem, - // LimeLightPoseSubsystem limeLightPoseSubsystem, IntakeSubsystem - // intakeSubsystem, - // Pose2d cubePose) { - // // return new PositionCommand(robotContainer, OperatorButtons.GROUND) - // // .andThen( - // return new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 2500) - // .alongWith(new StraightPathCommand(drivetrainSubsystem, - // limeLightPoseSubsystem, cubePose));// ); - // } - - // autonomousChooser.setDefaultOption("Over and Balance", - - // AutonomousCommandFactory.getAutonomousSimpleLowCommand(m_drivetrainSubsystem, - // m_armSubsystem, grabberSubsystem) - // .andThen(AutonomousCommandFactory.getOverAndBalanceCommand(m_drivetrainSubsystem, - // poseEstimator))); - // // A chooser for autonomous commands - // autonomousChooser.setDefaultOption("Middle Balance", - // AutonomousCommandFactory.getAutonomousSimpleCommand(m_drivetrainSubsystem, - // m_armSubsystem, grabberSubsystem) - // .andThen(AutonomousCommandFactory.getSetPositionAndBalanceCommand(m_drivetrainSubsystem, - // poseEstimator))); - // autonomousChooser.addOption("Simple Case and Left out", - // AutonomousCommandFactory.getAutonomousAcceleratedAndLeftOutCommand(m_drivetrainSubsystem, - // m_armSubsystem, - // grabberSubsystem)); - // autonomousChooser.addOption("Simple Case and Right out", - // AutonomousCommandFactory.getAutonomousSimpleAndRightDeacceleratedOutCommand(m_drivetrainSubsystem, - // m_armSubsystem, - // grabberSubsystem)); - - // // Add commands to the autonomous command chooser - // autonomousChooser.addOption("Case 1 left", - // getAutonomousCommandCase1(0).andThen(new - // StraightPathCommand(m_drivetrainSubsystem, - // getPoseEstimatorForTarget(poseEstimator, 2), - // getFinalPoseForCase1(0)))); - - // autonomousChooser.addOption("Case 1 middle", getAutonomousCommandCase1(1)); - // autonomousChooser.addOption("CaSe 1 right", getAutonomousCommandCase1(2) - // .andThen(new StraightPathCommand(m_drivetrainSubsystem, - // getPoseEstimatorForTarget(poseEstimator, 0), - // getFinalPoseForCase1(2)))); - - // autonomousChooser.addOption("Case 2 left", getAutonomousCommandCase2(0)); - // autonomousChooser.addOption("Case 1 middle", getAutonomousCommandCase1(1)); - // autonomousChooser.addOption("Case 2 red", getAutonomousCommandCase2Red()); - // autonomousChooser.addOption("Case 2 blue", getAutonomousCommandCase2Blue()); - // autonomousChooser.addOption("Case 2 right", getAutonomousCommandCase2(2)); - // autonomousChooser.addOption("Case 3", getAutonomousCommandCase3()); - - // public Command getAutonomousCommandCase2Red() { - // Command command = new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, - // (180)) - - // // .andThen(new GrabberCommand(grabberSubsystem, false)) - // // .andThen(new KeyPadStateCommand(1)) - // // .andThen(getCommandFor(1)) - // // .andThen(new GrabberCommand(grabberSubsystem, true)) - // // .andThen(new WaitCommand(0.5)) - // // .andThen(new ZeroExtenderCommand(m_armSubsystem)) - // // .andThen(new StraightPathCommand(m_drivetrainSubsystem, poseEstimator, - // // new Pose2d(new Translation2d(2.1, 5.24), new - // // Rotation2d(Math.toRadians(180))))) - // // .andThen(new ZeroShoulderCommand(m_armSubsystem)) - // // .andThen(new ChangeTurboModeCommand()) - // // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) - // .andThen(new WaitCommand(0.5)); - // // .andThen(new ChangeNormalModeCommand()) - // // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) - // // .andThen(new WaitCommand(2.5)) - // // // .andThen(new DriveCommand(m_drivetrainSubsystem, -0.05,0,0)) - // // // .andThen(new WaitCommand(2)) - // // .andThen(new DriveCommand(m_drivetrainSubsystem, 0, 0, 0)); - // // // .andThen(new BalanceCommand(m_drivetrainSubsystem)) - // command.setName("Case 2 red"); - // return command; - // } - - // public Command getAutonomousCommandCase2Blue() { - // Command command = new ZeroGyroCommand(m_drivetrainSubsystem, - // balanceCommand, (180)) - // .andThen(new GrabberCommand(grabberSubsystem, false)) - // .andThen(new KeyPadStateCommand(1)) - // .andThen(getCommandFor(1)) - // .andThen(new GrabberCommand(grabberSubsystem, true)) - // .andThen(new WaitCommand(0.5)) - // .andThen(new ZeroExtenderCommand(m_armSubsystem)) - // .andThen(new StraightPathCommand(m_drivetrainSubsystem, poseEstimator, - // new Pose2d(new Translation2d(2.1, KeyPadPositionSupplier.FIELD_WIDTH - 5.24), - // new Rotation2d(Math.toRadians(180))))) - // .andThen(new ZeroShoulderCommand(m_armSubsystem)) - // .andThen(new ChangeTurboModeCommand()) - // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) - // .andThen(new WaitCommand(0.5)) - // .andThen(new ChangeNormalModeCommand()) - // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) - // .andThen(new WaitCommand(2.5)) - // // .andThen(new DriveCommand(m_drivetrainSubsystem, -0.05,0,0)) - // // .andThen(new WaitCommand(2)) - // .andThen(new DriveCommand(m_drivetrainSubsystem, 0, 0, 0)); - // // .andThen(new BalanceCommand(m_drivetrainSubsystem)) - // command.setName("Case 2 blue"); - // return command; - // } - - // public Command getAutonomousCommandCase3() { - // KeyPadPositionSupplier.state = 0; - // Command command = new Command() { - // @Override - // public void initialize() { - // KeyPadPositionSupplier.state = 0; - // } - - // @Override - // public boolean isFinished() { - // return true; - // } - - // }.andThen(getCommandFor(0) - // .andThen(new GrabberCommand(grabberSubsystem, true)) - // // .andThen(new WaitCommand(1)) - // .andThen(new ZeroExtenderCommand(m_armSubsystem)) - // // .andThen( - // // new StraightPathCommand(m_drivetrainSubsystem, poseEstimator, - // // new Pose2d(5.75, - // // 7.4, - // // new Rotation2d(Math.toRadians(180))))) - // .andThen( - // new StraightPathCommand(m_drivetrainSubsystem, - // getPoseEstimatorForTarget(poseEstimator, 0), - // new Pose2d(4.4, - // 5.3, - // new Rotation2d(Math.toRadians(0))))) - // .andThen( - // new StraightPathCommand(m_drivetrainSubsystem, - // getPoseEstimatorForTarget(poseEstimator, 0), - // new Pose2d(6.4, - // 5.3, - // new Rotation2d(Math.toRadians(0))))) - // .andThen(new ShoulderCommand(m_armSubsystem, 40352)) - // .andThen(new ExtenderCommand(m_armSubsystem, 183023 * 16 / 36)) - // .andThen(new GrabberCommand(grabberSubsystem, false)) - // .andThen(new GrabberCommand(grabberSubsystem, false)) - // .andThen(new WaitCommand(2)) - // .andThen(new ZeroExtenderCommand(m_armSubsystem)) - // .andThen(Commands.parallel( - // new ShoulderCommand(m_armSubsystem, 190432), - // new StraightPathCommand(m_drivetrainSubsystem, - // getPoseEstimatorForTarget(poseEstimator, 0), - // new Pose2d(3.2, 5.3, - // new Rotation2d(Math.toRadians(0)))))) - // .andThen(getCommandFor(4)) - // .andThen(new GrabberCommand(grabberSubsystem, true))); - // command.setName("case 3"); - // return command; - // } + // if (aimFirst) { + // command = new ShootToSpeakerCommand(robotContainer.shooterSubsystem, + // robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.limeLightPoseSubsystem, + // robotContainer.drivetrainSubsystem) + // .alongWith(new SpeakerAlligningCommand( + // robotContainer.limeLightPoseSubsystem, + // robotContainer.drivetrainSubsystem)); + // } else { + // command = + // new Command() { + // @Override + // public void initialize() { + // robotContainer.shooterSubsystem.setTiltAngle(-4); + // } + // @Override + // public boolean isFinished() { + // return true; + // } + // }; + // command = command.andThen( + // new WaitUntilShooterReadyCommand(robotContainer.shooterSubsystem) + // .andThen(new GrabberOutCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.3))); + // } + // if (delay > 0) { + // command = new WaitCommand(delay).andThen(command); + // } + // if (from == -1) { + // return command; + // } + // /** + // * from can be higher than until. + // * In that case, we go in reverse order + // * look at the end of the loop to see how + // * i gets incremented, or decremented. + // */ + // for (int i = from; i != until;) { + // //command = command + // // .andThen(new IntakeNoteCommand(robotContainer.intakeSubsystem)); + // // IntakeCommand.State.OUT, 4000)); + // if (shootFromPosition[i][0] == noteInitialPosition[i][0] && + // shootFromPosition[i][1] == noteInitialPosition[i][1]) { + // //command = command + // // .andThen(new WaitUntilIntakeOutCommand(robotContainer.intakeSubsystem)); + // } + // double pickupX = noteInitialPosition[i][0]; + // double pickupY = noteInitialPosition[i][1]; + // double pickupAlpha = robotAngle; + + // if (from < until) { + // pickupX = accendingNoteInitialPosition[i][0]; + // pickupY = accendingNoteInitialPosition[i][1]; + // pickupAlpha = accendingNoteInitialPosition[i][2]; + // } else if (until < from) { + // pickupX = descendingNoteInitialPosition[i][0]; + // pickupY = descendingNoteInitialPosition[i][1]; + // pickupAlpha = descendingNoteInitialPosition[i][2]; + // } + + // command = command + // .andThen( + // new StraightPathCommand(robotContainer.drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(pickupX, + // pickupY, + // Rotation2d.fromDegrees( + // pickupAlpha))) + // .alongWith(new GrabberInCommand( + // robotContainer.grabberSubsystem))) + // .andThen(grabNoteCommand(robotContainer)); + // //.andThen(new WaitCommand(0.1)); + + // if (shootFromPosition[i][0] != noteInitialPosition[i][0] || + // shootFromPosition[i][1] != noteInitialPosition[i][1]) { + // command = command + // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.shooterSubsystem) + // .alongWith( + // new StraightPathCommand( + // robotContainer.drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(shootFromPosition[i][0], + // shootFromPosition[i][1], + // Rotation2d.fromDegrees( + // robotAngle))))); + // } else { + // command = command + // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.shooterSubsystem) + // .alongWith(new SpeakerAlligningCommand( + // robotContainer.limeLightPoseSubsystem, + // robotContainer.drivetrainSubsystem))); + // } + // command = command + // .andThen(new ShootToSpeakerCommand(robotContainer.shooterSubsystem, + // robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.limeLightPoseSubsystem, + // robotContainer.drivetrainSubsystem)); + // if (from < until) { + // ++i; + // } else { + // --i; + // } + // } + // return command; + // } + + // public static Command getAutonomousCommand(RobotContainer robotContainer, int + // from, int until, boolean accending) { + // double robotAngle = (from >= 8) ? 0 : 180; + + // double shootFromPosition[][] = (accending) ? shootAccendingPosition : + // shootDescendingPosition; + + // Command command = + // // new ResetOdometryWithCameraCommand(robotContainer.limeLightPoseSubsystem) + // // .andThen( + // new WaitUntilShooterReadyCommand(robotContainer.shooterSubsystem) + // .andThen(new GrabberOutCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.3)); + // if (from == -1) { + // return command; + // } + // /** + // * from can be higher than until. + // * In that case, we go in reverse order + // * look at the end of the loop to see how + // * i gets incremented, or decremented. + // */ + // for (int i = from; i != until;) { + // // command = command + // // .andThen(new IntakeNoteCommand(robotContainer.intakeSubsystem)); + // //IntakeCommand.State.OUT, 4000)); + // if (shootFromPosition[i][0] == noteInitialPosition[i][0] && + // shootFromPosition[i][1] == noteInitialPosition[i][1]) { + // //command = command + // // .andThen(new WaitUntilIntakeOutCommand(robotContainer.intakeSubsystem)); + // } + // double pickupX = noteInitialPosition[i][0]; + // double pickupY = noteInitialPosition[i][1]; + // double pickupAlpha = robotAngle; + + // if (from != until) { + // if (accending) { + // pickupX = accendingNoteInitialPosition[i][0]; + // pickupY = accendingNoteInitialPosition[i][1]; + // pickupAlpha = accendingNoteInitialPosition[i][2]; + // } else { + // pickupX = descendingNoteInitialPosition[i][0]; + // pickupY = descendingNoteInitialPosition[i][1]; + // pickupAlpha = descendingNoteInitialPosition[i][2]; + // } + // } + + // command = command + // .andThen( + // new StraightPathCommand(robotContainer.drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(pickupX, + // pickupY, + // Rotation2d.fromDegrees( + // pickupAlpha))) + // .alongWith(new GrabberInCommand( + // robotContainer.grabberSubsystem))) + // .andThen(grabNoteCommand(robotContainer)); + + // if (shootFromPosition[i][0] != noteInitialPosition[i][0] || + // shootFromPosition[i][1] != noteInitialPosition[i][1]) { + // command = command + // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.shooterSubsystem) + // .alongWith( + // new StraightPathCommand( + // robotContainer.drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(shootFromPosition[i][0], + // shootFromPosition[i][1], + // Rotation2d.fromDegrees( + // robotAngle))))); + // } else { + // command = command + // .andThen(new LoadNoteCommand(robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.shooterSubsystem) + // .alongWith(new SpeakerAlligningCommand( + // robotContainer.limeLightPoseSubsystem, + // robotContainer.drivetrainSubsystem))); + // } + // command = command + // .andThen(new ShootToSpeakerCommand(robotContainer.shooterSubsystem, + // robotContainer.intakeSubsystem, + // robotContainer.grabberSubsystem, + // robotContainer.limeLightPoseSubsystem, + // robotContainer.drivetrainSubsystem)); + // if (accending) { + // ++i; + // } else { + // --i; + // } + // if (from < 8) { + // if (i < 0) { + // i = 7; + // } else if (i > 7) { + // i = 0; + // } + // } else { + // if (i < 8) { + // i = 15; + // } else if (i > 15) { + // i = 8; + // } + // } + // } + // return command; + // } + + // public static Command grabNoteCommand(RobotContainer robotContainer) { + // return new GrabNoteCommandAutonomous(robotContainer, true); + // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new GrabberInCommand(robotContainer.grabberSubsystem)) + // .andThen(new WaitCommand(0.1)); + // } + + // public Command getAutonomousCommandCase2() { + // Command command = new DisplayLogCommand("Case 2") + // .andThen(new WaitCommand(0.5)); + // return command; + // } + + // public Command caseCommand(String name, IntakeSubsystem intakeSubsystem, + // double deltaY) { + // return new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, (180)) + // // .andThen(new SetModeConeCube(RobotMode.Cube)) + // // .andThen(new PositionCommand(this, OperatorButtons.LOW)) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)) + // // .andThen(new PositionCommand(this, OperatorButtons.HOME)) + // .andThen( + // new StraightPathCommand(drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(4.49, 5.08, + // new Rotation2d(Math.toRadians(180))))) + // .andThen(new RotateCommand(drivetrainSubsystem)) + // .andThen(new StraightPathCommand(drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(4.79, 5.08, new Rotation2d(Math.toRadians(0))))) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 1000)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) + // .andThen(new RotateCommand(drivetrainSubsystem)) + // .andThen(new StraightPathCommand(drivetrainSubsystem, + // robotContainer.limeLightPoseSubsystem, + // new Pose2d(1.89, 4.88, new Rotation2d(Math.toRadians(180))))) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)); + // } + + // public Command testMovements(DrivetrainSubsystem drivetrainSubsystem, + // IntakeSubsystem intake, + // RobotContainer robotContainer) { + // return new DisplayLogCommand("Test Case") + // // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, .4, 0, 0, + // 500)) + // // .andThen(new SetModeConeCube(RobotMode.Cube)) + // // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, 0, 0, 0, 50)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.LOW)) + // .andThen(new IntakeCommand(intake, State.OUT, 1000)) + // .andThen(new WaitCommand(.5)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) + // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, -.3, 0, 0, 1000)) + // .andThen(new WaitCommand(.25)) + // .andThen(new RotateCommand(drivetrainSubsystem)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) + // .andThen(new IntakeCommand(intake, State.IN, 1000)) + // .andThen(new RobotOrientedDriveDeacceleratedCommand(drivetrainSubsystem, -.3, + // 0, 0, + // 300)); + // // .andThen(new RobotOrientedDriveCommand(drivetrainSubsystem, 0, 0, 0, 50)); + // } + + // private Supplier poseEstimator; + + // public Command caseCommandOld(String name, IntakeSubsystem intakeSubsystem, + // double speedY, int durationY) { + // Command command = new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, + // (180)) + // // .andThen(new SetModeConeCube(RobotMode.Cube)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HIGH)) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 1000)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) + // .andThen(new RobotOrientedDriveDeacceleratedCommand(drivetrainSubsystem, 0, + // speedY, 0, + // durationY)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new RobotOrientedDriveDeacceleratedCommand(drivetrainSubsystem, -.7, + // 0, 0, + // 1500)) + // .andThen(new RotateCommand(drivetrainSubsystem)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 1000)); + // command.setName(name); + // return command; + // } + + // public Command test2() { + // Command command = new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, + // (180)) + // .andThen(new StraightPathCommand(drivetrainSubsystem, poseEstimator, + // new Pose2d(new Translation2d(2.1, + // KeyPadPositionSupplier.FIELD_WIDTH - 5.24), + // new Rotation2d(Math.toRadians(180))))) + // .andThen(new DriveCommand(drivetrainSubsystem, -2, 0, 100)) + // .andThen(new WaitCommand(0.5)) + // .andThen(new DriveCommand(drivetrainSubsystem, -2, 0, 0)) + // .andThen(new WaitCommand(2.5)) + // .andThen(new DriveCommand(drivetrainSubsystem, -0.05, 0, 0)) + // .andThen(new WaitCommand(2)) + // .andThen(new DriveCommand(drivetrainSubsystem, 0, 0, 0)) + // .andThen(new BalanceCommand(drivetrainSubsystem)); + // command.setName("Test 2"); + // return command; + // } + + // public Command getOverAndBalanceCommand(DrivetrainSubsystem + // m_drivetrainSubsystem) { + // BalanceCommand balanceCommand = new BalanceCommand(m_drivetrainSubsystem); + + // return new Command() { + // @Override + // public void initialize() { + // DefaultDriveCommand.autonomous = true; + // SwerveModule.setPowerRatio(1.5); + // balanceCommand.zeroGyroscope(); + // } + + // @Override + // public boolean isFinished() { + // return true; + // } + + // } + // // .andThen(new SetModeConeCube(RobotMode.Cube)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HIGH)) + // .andThen(new IntakeCommand(robotContainer.intakeSubsystem, + // IntakeCommand.State.OUT, + // 300)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.HOME)) + // .andThen(new RobotOrientedDriveCommand(m_drivetrainSubsystem, -0.03, 0, 0, + // 3500)) + // .andThen(new RobotOrientedDriveCommand(m_drivetrainSubsystem, 0.02, 0, 0, + // 800)) + // .andThen(balanceCommand); + // } + + // public static Command getPieceWithCoral(RobotContainer robotContainer, + // LimeLightPoseSubsystem limeLightPoseSubsystem, + // String splineGetCube, String splineDropCube, Pose2d cubePose) { + // DrivetrainSubsystem drivetrainSubsystem = robotContainer.drivetrainSubsystem; + // IntakeSubsystem intakeSubsystem = robotContainer.intakeSubsystem; + // return + // // new SetModeConeCube(RobotMode.Cube) + // // .andThen( + // new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)// ) + // .andThen(new TrajectoryUntilSeeingCube(splineGetCube, + // drivetrainSubsystem, limeLightPoseSubsystem)) + // // .andThen(new PositionCommand(robotContainer, OperatorButtons.GROUND)) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 2500) + // .alongWith(new DriveToObjectCommand(drivetrainSubsystem, "cube")))// + // .andThen(new + // // PositionCommand(robotContainer, + // // OperatorButtons.HOME)) + // .andThen(new TrajectoryCommand(splineDropCube, + // drivetrainSubsystem, limeLightPoseSubsystem)) + // .andThen(new IntakeCommand(intakeSubsystem, IntakeCommand.State.OUT, 300)); + // } + + // static Command grabCube( + // RobotContainer robotContainer, + // DrivetrainSubsystem drivetrainSubsystem, + // LimeLightPoseSubsystem limeLightPoseSubsystem, IntakeSubsystem + // intakeSubsystem, + // Pose2d cubePose) { + // // return new PositionCommand(robotContainer, OperatorButtons.GROUND) + // // .andThen( + // return new IntakeCommand(intakeSubsystem, IntakeCommand.State.IN, 2500) + // .alongWith(new StraightPathCommand(drivetrainSubsystem, + // limeLightPoseSubsystem, cubePose));// ); + // } + + // autonomousChooser.setDefaultOption("Over and Balance", + + // AutonomousCommandFactory.getAutonomousSimpleLowCommand(m_drivetrainSubsystem, + // m_armSubsystem, grabberSubsystem) + // .andThen(AutonomousCommandFactory.getOverAndBalanceCommand(m_drivetrainSubsystem, + // poseEstimator))); + // // A chooser for autonomous commands + // autonomousChooser.setDefaultOption("Middle Balance", + // AutonomousCommandFactory.getAutonomousSimpleCommand(m_drivetrainSubsystem, + // m_armSubsystem, grabberSubsystem) + // .andThen(AutonomousCommandFactory.getSetPositionAndBalanceCommand(m_drivetrainSubsystem, + // poseEstimator))); + // autonomousChooser.addOption("Simple Case and Left out", + // AutonomousCommandFactory.getAutonomousAcceleratedAndLeftOutCommand(m_drivetrainSubsystem, + // m_armSubsystem, + // grabberSubsystem)); + // autonomousChooser.addOption("Simple Case and Right out", + // AutonomousCommandFactory.getAutonomousSimpleAndRightDeacceleratedOutCommand(m_drivetrainSubsystem, + // m_armSubsystem, + // grabberSubsystem)); + + // // Add commands to the autonomous command chooser + // autonomousChooser.addOption("Case 1 left", + // getAutonomousCommandCase1(0).andThen(new + // StraightPathCommand(m_drivetrainSubsystem, + // getPoseEstimatorForTarget(poseEstimator, 2), + // getFinalPoseForCase1(0)))); + + // autonomousChooser.addOption("Case 1 middle", getAutonomousCommandCase1(1)); + // autonomousChooser.addOption("CaSe 1 right", getAutonomousCommandCase1(2) + // .andThen(new StraightPathCommand(m_drivetrainSubsystem, + // getPoseEstimatorForTarget(poseEstimator, 0), + // getFinalPoseForCase1(2)))); + + // autonomousChooser.addOption("Case 2 left", getAutonomousCommandCase2(0)); + // autonomousChooser.addOption("Case 1 middle", getAutonomousCommandCase1(1)); + // autonomousChooser.addOption("Case 2 red", getAutonomousCommandCase2Red()); + // autonomousChooser.addOption("Case 2 blue", getAutonomousCommandCase2Blue()); + // autonomousChooser.addOption("Case 2 right", getAutonomousCommandCase2(2)); + // autonomousChooser.addOption("Case 3", getAutonomousCommandCase3()); + + // public Command getAutonomousCommandCase2Red() { + // Command command = new ZeroGyroCommand(drivetrainSubsystem, balanceCommand, + // (180)) + + // // .andThen(new GrabberCommand(grabberSubsystem, false)) + // // .andThen(new KeyPadStateCommand(1)) + // // .andThen(getCommandFor(1)) + // // .andThen(new GrabberCommand(grabberSubsystem, true)) + // // .andThen(new WaitCommand(0.5)) + // // .andThen(new ZeroExtenderCommand(m_armSubsystem)) + // // .andThen(new StraightPathCommand(m_drivetrainSubsystem, poseEstimator, + // // new Pose2d(new Translation2d(2.1, 5.24), new + // // Rotation2d(Math.toRadians(180))))) + // // .andThen(new ZeroShoulderCommand(m_armSubsystem)) + // // .andThen(new ChangeTurboModeCommand()) + // // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) + // .andThen(new WaitCommand(0.5)); + // // .andThen(new ChangeNormalModeCommand()) + // // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) + // // .andThen(new WaitCommand(2.5)) + // // // .andThen(new DriveCommand(m_drivetrainSubsystem, -0.05,0,0)) + // // // .andThen(new WaitCommand(2)) + // // .andThen(new DriveCommand(m_drivetrainSubsystem, 0, 0, 0)); + // // // .andThen(new BalanceCommand(m_drivetrainSubsystem)) + // command.setName("Case 2 red"); + // return command; + // } + + // public Command getAutonomousCommandCase2Blue() { + // Command command = new ZeroGyroCommand(m_drivetrainSubsystem, + // balanceCommand, (180)) + // .andThen(new GrabberCommand(grabberSubsystem, false)) + // .andThen(new KeyPadStateCommand(1)) + // .andThen(getCommandFor(1)) + // .andThen(new GrabberCommand(grabberSubsystem, true)) + // .andThen(new WaitCommand(0.5)) + // .andThen(new ZeroExtenderCommand(m_armSubsystem)) + // .andThen(new StraightPathCommand(m_drivetrainSubsystem, poseEstimator, + // new Pose2d(new Translation2d(2.1, KeyPadPositionSupplier.FIELD_WIDTH - 5.24), + // new Rotation2d(Math.toRadians(180))))) + // .andThen(new ZeroShoulderCommand(m_armSubsystem)) + // .andThen(new ChangeTurboModeCommand()) + // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) + // .andThen(new WaitCommand(0.5)) + // .andThen(new ChangeNormalModeCommand()) + // .andThen(new DriveCommand(m_drivetrainSubsystem, -1, 0, 0)) + // .andThen(new WaitCommand(2.5)) + // // .andThen(new DriveCommand(m_drivetrainSubsystem, -0.05,0,0)) + // // .andThen(new WaitCommand(2)) + // .andThen(new DriveCommand(m_drivetrainSubsystem, 0, 0, 0)); + // // .andThen(new BalanceCommand(m_drivetrainSubsystem)) + // command.setName("Case 2 blue"); + // return command; + // } + + // public Command getAutonomousCommandCase3() { + // KeyPadPositionSupplier.state = 0; + // Command command = new Command() { + // @Override + // public void initialize() { + // KeyPadPositionSupplier.state = 0; + // } + + // @Override + // public boolean isFinished() { + // return true; + // } + + // }.andThen(getCommandFor(0) + // .andThen(new GrabberCommand(grabberSubsystem, true)) + // // .andThen(new WaitCommand(1)) + // .andThen(new ZeroExtenderCommand(m_armSubsystem)) + // // .andThen( + // // new StraightPathCommand(m_drivetrainSubsystem, poseEstimator, + // // new Pose2d(5.75, + // // 7.4, + // // new Rotation2d(Math.toRadians(180))))) + // .andThen( + // new StraightPathCommand(m_drivetrainSubsystem, + // getPoseEstimatorForTarget(poseEstimator, 0), + // new Pose2d(4.4, + // 5.3, + // new Rotation2d(Math.toRadians(0))))) + // .andThen( + // new StraightPathCommand(m_drivetrainSubsystem, + // getPoseEstimatorForTarget(poseEstimator, 0), + // new Pose2d(6.4, + // 5.3, + // new Rotation2d(Math.toRadians(0))))) + // .andThen(new ShoulderCommand(m_armSubsystem, 40352)) + // .andThen(new ExtenderCommand(m_armSubsystem, 183023 * 16 / 36)) + // .andThen(new GrabberCommand(grabberSubsystem, false)) + // .andThen(new GrabberCommand(grabberSubsystem, false)) + // .andThen(new WaitCommand(2)) + // .andThen(new ZeroExtenderCommand(m_armSubsystem)) + // .andThen(Commands.parallel( + // new ShoulderCommand(m_armSubsystem, 190432), + // new StraightPathCommand(m_drivetrainSubsystem, + // getPoseEstimatorForTarget(poseEstimator, 0), + // new Pose2d(3.2, 5.3, + // new Rotation2d(Math.toRadians(0)))))) + // .andThen(getCommandFor(4)) + // .andThen(new GrabberCommand(grabberSubsystem, true))); + // command.setName("case 3"); + // return command; + // } } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 947fd73..599b6db 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -99,7 +99,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { // cmd = RobotContainer.autonomousChooser.getSelected(); - Integer firstStepWait = RobotContainer.autonomousChooserFirtWait.getSelected(); + Integer firstStepWait = RobotContainer.autonomousChooserFirstWait.getSelected(); boolean autoAim = RobotContainer.autonomousAim.getSelected(); Integer firstStep = RobotContainer.autonomousChooserFirstStep.getSelected(); Integer lastStep = RobotContainer.autonomousChooserLastStep.getSelected(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 520db61..20e5069 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,7 +39,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.ChangeNormalModeCommand; import frc.robot.commands.ChangeTurboModeCommand; -import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.IntakeNoteCommand; import frc.robot.commands.ShootCommand; import frc.robot.commands.TiltHomeCommand; @@ -86,16 +85,16 @@ public class RobotContainer { private SlewRateLimiter sRX = new SlewRateLimiter(DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND / 1.0); public static SendableChooser autonomousAim = new SendableChooser<>(); - public static SendableChooser autonomousChooserFirtWait = new SendableChooser<>(); + public static SendableChooser autonomousChooserFirstWait = new SendableChooser<>(); public static SendableChooser autonomousChooserFirstStep = new SendableChooser<>(); public static SendableChooser autonomousChooserLastStep = new SendableChooser<>(); public PoseSubsystem poseSubsystem; public static CoralSubsystem coralSubsystem = new CoralSubsystem(); public TiltSubsystem tilt = new TiltSubsystem(); + // TODO: Move these to a separate file public final static Pose2d BLUE_SPEAKER = new Pose2d(-0.0381, 5.54, new Rotation2d()); public final static Pose2d RED_SPEAKER = new Pose2d(16.57, 5.54, new Rotation2d(Math.toRadians(180))); - public final static Pose2d BLUE_AMP = new Pose2d(72.5, 323, new Rotation2d(Math.toRadians(270))); public final static Pose2d RED_AMP = new Pose2d(578.77, 323, new Rotation2d(Math.toRadians(270))); @@ -113,13 +112,17 @@ public RobotContainer() { drivetrainSubsystem.setDefaultCommand(new DefaultDriveCommand( drivetrainSubsystem, - () -> sLY.calculate(-modifyAxis(driveController.getLeftY()) + () -> sLY.calculate(modifyAxis(driveController.getLeftY()) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND), - () -> sLX.calculate(-modifyAxis(driveController.getLeftX()) + () -> sLX.calculate(modifyAxis(driveController.getLeftX()) * DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND), () -> sRX.calculate(-modifyAxis(driveController.getRightX()) * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND), - driveController.y()));// Set precision based upon left bumper + // Set precision based upon left bumper + // Not used, currently implemented with separate commands + driveController.y(), + // Set robot oriented control based upon left bumper + driveController.leftBumper())); poseSubsystem = new PoseSubsystem(drivetrainSubsystem, "limelight"); configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/DefaultDriveCommand.java index 422cdef..fb0346b 100644 --- a/src/main/java/frc/robot/commands/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/DefaultDriveCommand.java @@ -10,6 +10,7 @@ import frc.robot.Robot; import frc.robot.RobotContainer; import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.utilities.SwerveModule; public class DefaultDriveCommand extends Command { @@ -27,25 +28,32 @@ public class DefaultDriveCommand extends Command { private final DoubleSupplier m_translationYSupplier; private final DoubleSupplier m_rotationSupplier; private final BooleanSupplier precisionActivator; + private final BooleanSupplier robotOrientedActivator; public DefaultDriveCommand(DrivetrainSubsystem drivetrainSubsystem, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, DoubleSupplier rotationSupplier, - BooleanSupplier precisionActivator) { + BooleanSupplier precisionActivator, BooleanSupplier robotOrientedActivator) { this.m_drivetrainSubsystem = drivetrainSubsystem; this.m_translationXSupplier = translationXSupplier; this.m_translationYSupplier = translationYSupplier; this.m_rotationSupplier = rotationSupplier; this.precisionActivator = precisionActivator; + this.robotOrientedActivator = robotOrientedActivator; + logf("Default Drive Command started precision:%b\n", this.precisionActivator.getAsBoolean()); addRequirements(drivetrainSubsystem); } @Override public void execute() { - // You can use `new ChassisSpeeds(...)` for robot-oriented movement instead of - // field-oriented movement + // Currently implemented with separate commands + // if (precisionActivator.getAsBoolean()) { + // SwerveModule.setPowerRatio(SwerveModule.PRECISION); + // } else { + // SwerveModule.setPowerRatio(SwerveModule.TURBO); + // } if (Robot.count % 20 == 0) { if (m_translationXSupplier.getAsDouble() != 0 && @@ -54,28 +62,28 @@ public void execute() { m_translationYSupplier.getAsDouble(), m_rotationSupplier.getAsDouble()); } } + // this allows for Robot Oriented driving - if (RobotContainer.getLeftBumper()) { + if (robotOrientedActivator.getAsBoolean()) { m_drivetrainSubsystem.drive( new ChassisSpeeds( - -m_translationXSupplier.getAsDouble(), - -m_translationYSupplier.getAsDouble(), + m_translationXSupplier.getAsDouble(), + m_translationYSupplier.getAsDouble(), m_rotationSupplier.getAsDouble())); } else { m_drivetrainSubsystem.drive( ChassisSpeeds.fromFieldRelativeSpeeds( - -m_translationXSupplier.getAsDouble(), - -m_translationYSupplier.getAsDouble(), + m_translationXSupplier.getAsDouble(), + m_translationYSupplier.getAsDouble(), m_rotationSupplier.getAsDouble(), m_drivetrainSubsystem.getGyroscopeRotation())); } - - // FIXME: Precision mode isnt implemented here??? } @Override public void end(boolean interrupted) { m_drivetrainSubsystem.drive(new ChassisSpeeds(0.0, 0.0, 0.0)); + // SwerveModule.setPowerRatio(SwerveModule.DEFAULT); } } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index f63eff3..74b66e1 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -22,7 +22,6 @@ public DriveCommand(DrivetrainSubsystem drivetrainSubsystem, double xSpeed, doub @Override public void initialize() { - } @Override @@ -42,7 +41,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - + } - + } diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java index 52de314..d563b1d 100644 --- a/src/main/java/frc/robot/utilities/SwerveModule.java +++ b/src/main/java/frc/robot/utilities/SwerveModule.java @@ -20,8 +20,9 @@ public class SwerveModule { public static final double TURBO = 1; public static final double NORMAL = 5; public static final double PRECISION = 10; + public static final double DEFAULT = TURBO; - private static double powerRatio = SwerveModule.TURBO; + private static double powerRatio = DEFAULT; public static final double openLoopRamp = 0.25; public static final double closedLoopRamp = 0.0; @@ -108,10 +109,12 @@ public SwerveModule(int moduleNumber, SwerveModuleType swerve_type, SwerveModule // lastAngle = getState().angle; } + // FIXME: This should prob be in drivetrain subsystem public static double getPowerRatio() { return powerRatio; } + // FIXME: This should prob be in drivetrain subsystem public static void setPowerRatio(double powerRatio) { SwerveModule.powerRatio = powerRatio; }