diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c6d9bf..afa8e21 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,24 +12,28 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.Balance; -import frc.robot.commands.DeployPlunger; import frc.robot.commands.DriveArcade; import frc.robot.commands.DriveForwards; import frc.robot.commands.DriveReverse; import frc.robot.commands.DriveTank; +import frc.robot.commands.ExtendArm; +import frc.robot.commands.ManualArm; import frc.robot.commands.ResetDrivePose; import frc.robot.commands.StartLeavingCommunity; import frc.robot.commands.StopLeavingCommunity; -import frc.robot.commands.TestDrivePID; import frc.robot.commands.TrackTrajectory; import frc.robot.positioning.Pose; import frc.robot.subsystems.Camera; import frc.robot.subsystems.Drive; +import frc.robot.subsystems.I2CManager; import frc.robot.subsystems.Limelight; import frc.robot.subsystems.LimelightManager; -import frc.robot.subsystems.Plunger; +import frc.robot.subsystems.arm.ArmBase; +import frc.robot.subsystems.arm.ArmClaw; +import frc.robot.subsystems.arm.ArmExtender; +import frc.robot.subsystems.arm.ArmShoulder; +import frc.robot.subsystems.arm.CompilationArm; -@SuppressWarnings("unused") /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -37,51 +41,58 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - /* Messy, ugly commands and subsystems section - * TODO: organize later + + /* =============================================================== + * SUBSYSTEMS + * =============================================================== */ - /* Autotasks are mostly commented out in here for now because I don't care that they exist */ + + /* LIMELIGHT */ private final Limelight limelightBack = new Limelight("limelight-back"); private final Limelight limelightFront = new Limelight("limelight-front"); - private final LimelightManager limelightManager = - new LimelightManager(limelightBack, limelightFront); - private final Drive driveSubsystem = new Drive(limelightManager); - /* BIG CHUNGUS ARM CODE */ - // private final I2CManager I2CManager = new I2CManager(); - // private final ArmBase armBase = new ArmBase(); - // private final ArmShoulder armShoulder = new ArmShoulder(); - // private final ArmExtender armExtender = new ArmExtender(I2CManager); - // private final ArmClaw armClaw = new ArmClaw(); + private final LimelightManager limelightManager = new LimelightManager(limelightBack, limelightFront); + /* CAMERA */ private final Camera aimCamera = new Camera(Constants.Camera.PORT_CAMERA_AIM); - // private final CompilationArm compilationArm = - // new CompilationArm(armBase, armClaw, armExtender, armShoulder); - // private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); - // private RetractArm retractArmCommand = new RetractArm(armExtender); - private final Plunger plunger = new Plunger(); - private final DeployPlunger deployPlunger = new DeployPlunger(plunger); - private final StartLeavingCommunity startLeavingCommunity = - new StartLeavingCommunity(driveSubsystem); - private final StopLeavingCommunity stopLeavingCommunity = - new StopLeavingCommunity(driveSubsystem); - private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); - private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); - private final InstantCommand displayAimVideo = - new InstantCommand(aimCamera::startCamera, aimCamera); + /* DRIVE */ + private final Drive driveSubsystem = new Drive(limelightManager); + /* ARM */ + private final I2CManager I2CManager = new I2CManager(); + private final ArmBase armBase = new ArmBase(); + private final ArmShoulder armShoulder = new ArmShoulder(); + private final ArmExtender armExtender = new ArmExtender(I2CManager); + private final ArmClaw armClaw = new ArmClaw(); + private final CompilationArm compilationArm = + new CompilationArm(armBase, armClaw, armExtender, armShoulder); + private final ManualArm manualArm = new ManualArm(armBase, armShoulder, compilationArm); + private RetractArm retractArmCommand = new RetractArm(armExtender); + + /* =============================================================== + * COMMANDS + * =============================================================== + */ + + /* AUTO COMMANDS */ + private final StartLeavingCommunity startLeavingCommunity = new StartLeavingCommunity(driveSubsystem); + private final StopLeavingCommunity stopLeavingCommunity = new StopLeavingCommunity(driveSubsystem); + /* CAMERA COMMAND */ + private final InstantCommand displayAimVideo = new InstantCommand(aimCamera::startCamera, aimCamera); + /* BALANCE COMMAND */ private final Balance balance = new Balance(driveSubsystem); + /* DRIVE COMMANDS*/ private final DriveArcade driveRO = new DriveArcade(driveSubsystem); private final DriveTank driveFO = new DriveTank(driveSubsystem); - // private final Command openClaw = armClaw.manualOpenClawCommand(); - // private final Command closeClaw = armClaw.manualCloseClawCommand(); - // private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); - // private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); - // private final ExtendArm extend = new ExtendArm(armExtender); - // private final RetractArm retract = new RetractArm(armExtender); - // private final TaskScheduler taskScheduler = new TaskScheduler(); - private final TrackTrajectory demoTrajectoryTrackingCommand = - new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); - private final TestDrivePID testDrivePID = new TestDrivePID(driveSubsystem); - + private final DriveForwards driveForwards = new DriveForwards(driveSubsystem); + private final DriveReverse driveReverse = new DriveReverse(driveSubsystem); + /* ARM COMMANDS */ + private final Command openClaw = armClaw.manualOpenClawCommand(); + private final Command closeClaw = armClaw.manualCloseClawCommand(); + private final CloseClawCone closeClawCone = new CloseClawCone(armClaw); + private final CloseClawCube closeClawCube = new CloseClawCube(armClaw); + private final ExtendArm extend = new ExtendArm(armExtender); + private final RetractArm retract = new RetractArm(armExtender); + /* TRAJECTORY COMMAND */ + private final TrackTrajectory demoTrajectoryTrackingCommand = new TrackTrajectory(Constants.Drive.Trajectories.DEMO_TRAJECTORY, driveSubsystem); + // Replace with CommandPS4Controller or CommandJoystick if needed public static CommandXboxController controller = new CommandXboxController(OperatorConstants.CONTROLLER_NUMBER); @@ -96,14 +107,9 @@ public RobotContainer() { /* TODO: add ResetDrivePose into auto*/ autoChooser = new SendableChooser<>(); - // autoChooser.setDefaultOption("Fling cube + mobility bonus", - // Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, - // compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new - // WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new - // WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new - // WaitCommand(2.5)))); - // autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, - // armExtender, armClaw, compilationArm)); + autoChooser.setDefaultOption("Fling cube + mobility bonus", + Commands.sequence(Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(0.5)), new ParallelRaceGroup(new DriveReverse(driveSubsystem), new WaitCommand(0.8)), new ParallelRaceGroup(new DriveForwards(driveSubsystem), new WaitCommand(2.5)))); + autoChooser.addOption("Home arm (DOES NOT MOVE)", Autos.homingRoutine(armShoulder, armBase, armExtender, armClaw, compilationArm)); SmartDashboard.putData("Choose auto", autoChooser); @@ -112,9 +118,7 @@ public RobotContainer() { } public Pose containerGetArmPose() { - - // return compilationArm.getArmPose(); - return new Pose(); + return compilationArm.getArmPose(); } /** @@ -128,33 +132,24 @@ public Pose containerGetArmPose() { */ private void configureBindings() { - // controller.y().whileTrue(deployPlunger); - controller.rightBumper().whileTrue(balance); controller.leftBumper().whileTrue(new InstantCommand(driveSubsystem::printPoses)); controller.y().toggleOnTrue(driveFO); - controller.b().whileTrue(deployPlunger); - controller.x().whileTrue(demoTrajectoryTrackingCommand); - controller.a().whileTrue(testDrivePID); + joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, armBase, + armShoulder, armExtender, compilationArm)); + joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, compilationArm)); + joystick.povUp().whileTrue(extend); + joystick.povDown().whileTrue(retract); - // joystick.button(9).whileTrue(MoveToPose.extendingMoveToPose(Constants.Arm.Poses.PICKUP, - // armBase, - // armShoulder, armExtender, compilationArm)); - - // joystick.button(4).whileTrue(Autos.homingNoOpenClaw(armShoulder, armBase, armExtender, - // compilationArm)); - - // joystick.povUp().whileTrue(extend); - // joystick.povDown().whileTrue(retract); - - // joystick.button(11).whileTrue(openClaw); + joystick.button(11).whileTrue(openClaw); /* TODO: get better buttons with Gabriel */ - /* Or just get color sensor working */ + joystick.trigger().whileTrue(closeClaw); + joystick.button(2).whileTrue(closeClawCube); /*joystick .button(2) @@ -222,10 +217,10 @@ private void configureBindings() { compilationArm)); joystick .button(1) - .onTrue(armClaw.manualCloseClawCommand());*/ + .onTrue(armClaw.manualCloseClawCommand()); + */ } - - private void verifyAutoTasks() {} + /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -238,15 +233,13 @@ public Command getAutonomousCommand() { } public Command getResetCommand() { - // return MoveToPose.retractingMoveToPose( - // Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm) - // .alongWith(armClaw.openClawCommand()); - return new InstantCommand(); + return MoveToPose.retractingMoveToPose( + Constants.Arm.Poses.RESET, armBase, armShoulder, armExtender, compilationArm) + .alongWith(armClaw.openClawCommand()); } public Command getRetractCommand() { - // return retractArmCommand; - return new InstantCommand(); + return retractArmCommand; } public Command getDisplayAimVideoCommand() { diff --git a/src/main/java/frc/robot/commands/DriveForwards.java b/src/main/java/frc/robot/commands/DriveForwards.java index 9bb6b22..d95fee2 100644 --- a/src/main/java/frc/robot/commands/DriveForwards.java +++ b/src/main/java/frc/robot/commands/DriveForwards.java @@ -25,7 +25,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drivetrain.moveSimple(0.5, 0.5); + drivetrain.moveTank(0.5, 0.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/DriveReverse.java b/src/main/java/frc/robot/commands/DriveReverse.java index 5759041..dce92a0 100644 --- a/src/main/java/frc/robot/commands/DriveReverse.java +++ b/src/main/java/frc/robot/commands/DriveReverse.java @@ -25,7 +25,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - drivetrain.moveSimple(-0.5, -0.5); + drivetrain.moveTank(-0.5, -0.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index fa2f43d..63122a7 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -247,16 +247,7 @@ public void moveArcade(double x, double z) { public void moveTank(double left, double right) { drivetrain.tankDrive(left, right); } - - /** - * @deprecated Use {@link #moveTank} - */ - @Deprecated(forRemoval = true) - public void moveSimple(double leftSpeed, double rightSpeed) { - left.set(ControlMode.PercentOutput, leftSpeed); - right.set(ControlMode.PercentOutput, rightSpeed); - } - + /** * Sets the velocity setpoint of the left and right sides of the drivetrain. * diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 0b000e7..0d907a2 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -56,14 +56,10 @@ public Limelight(String name) { txSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("tx")).subscribe(0.0); tySubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ty")).subscribe(0.0); taSubscriber = NetworkTableInstance.getDefault().getDoubleTopic(fmtPath("ta")).subscribe(0.0); - /* In theory this won't break. It got mad when I tried to insert the array into the - * method like .subscribe({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) so ¯\_(ツ)_/¯ - */ - double[] defaultBotpos = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; botposSubscriber = NetworkTableInstance.getDefault() .getDoubleArrayTopic(fmtPath("botpose")) - .subscribe(defaultBotpos); + .subscribe(new double[]{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); } /* now its time for getter method chaingun, which I have to write manually because VS Code */