diff --git a/.vscode/settings.json b/.vscode/settings.json index 3103965..0ad7d23 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -68,5 +68,6 @@ "editor.detectIndentation": false, "editor.insertSpaces": false, "java.format.settings.url": "eclipse-formatter.xml", - "java.sources.organizeImports.staticStarThreshold": 1 + "java.sources.organizeImports.staticStarThreshold": 1, + "java.debug.settings.onBuildFailureProceed": true } \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..cc09b03 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "030000004c050000cc09000000000000" + }, + { + "guid": "030000004c050000cc09000000000000" + } + ] +} diff --git a/src/main/java/frc/robot/CommandComposer.java b/src/main/java/frc/robot/CommandComposer.java new file mode 100644 index 0000000..e209905 --- /dev/null +++ b/src/main/java/frc/robot/CommandComposer.java @@ -0,0 +1,63 @@ +package frc.robot; + +import static edu.wpi.first.wpilibj2.command.Commands.*; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.AlgaeGrabberSubsystem; +import frc.robot.subsystems.CheeseStickSubsystem; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.WristSubsystem; + +public class CommandComposer { + private static DriveSubsystem m_driveSubsystem; + private static AlgaeGrabberSubsystem m_algaeGrabberSubsystem; + private static CheeseStickSubsystem m_cheeseStickSubsystem; + private static ClimberSubsystem m_climberSubsystem; + private static ElevatorSubsystem m_elevatorSubsystem; + private static WristSubsystem m_wristSubsystem; + + private static Command scoreLevel(Supplier levelCommand) { + return sequence( + levelCommand.get(), + m_wristSubsystem.goToAngle(35), + m_elevatorSubsystem.lowerToScore(), + m_cheeseStickSubsystem.goLeft(), // TODO: Left is release? + m_cheeseStickSubsystem.goRight(), + levelCommand.get(), + m_wristSubsystem.goToAngle(-90)); + } + + public static Command scoreLevelFour() { + return scoreLevel(m_elevatorSubsystem::goToLevelFourHeight); + } + + public static Command scoreLevelThree() { + return scoreLevel(m_elevatorSubsystem::goToLevelThreeHeight); + } + + public static Command scoreLevelTwo() { + return scoreLevel(m_elevatorSubsystem::goToLevelTwoHeight); + } + + public static Command scoreLevelOne() { + return scoreLevel(m_elevatorSubsystem::goToLevelOneHeight); + } + + public static Command prepareForCoralPickup() { + return sequence( + m_elevatorSubsystem.goToCoralStationHeight(), + m_wristSubsystem.goToAngle(-90)); + } + + public static Command pickupAtCoralStation() { + return sequence( + m_cheeseStickSubsystem.goLeft(), + m_elevatorSubsystem.goToCoralStationHeight(), + m_cheeseStickSubsystem.goRight()); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 436af9f..1795ff1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -119,6 +119,8 @@ public static final class ElevatorConstants { public static final double kLevelTwoHeight = kLevelOneHeight; // same as level 1 public static final double kLevelThreeHeight = Units.inchesToMeters(60 - 24); public static final double kLevelFourHeight = Units.inchesToMeters(90 - 24); + public static final double kToScoreHeightDecrease = 0; // The amount that the elevator decreases in order to + // score public static final double kCoralStationHeight = 0; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dcf44f7..ebb7786 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import static frc.robot.CommandComposer.*; import static frc.robot.Constants.AlgaeConstants.*; import static frc.robot.Constants.ClimberConstants.*; import static frc.robot.Constants.ControllerConstants.*; @@ -45,7 +46,6 @@ public class Robot extends TimedRobot { private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem( m_mechanism.getRoot("anchor", Units.inchesToMeters(23), 0)); private final WristSubsystem m_wristSubsystem = new WristSubsystem(m_elevatorSubsystem.getWristMount()); - // Changed to PS5 private final CommandPS5Controller m_driverController = new CommandPS5Controller(kDriverControllerPort); private final CommandPS5Controller m_operatorController = new CommandPS5Controller(kOperatorControllerPort); private final PowerDistribution m_pdh = new PowerDistribution(); @@ -81,27 +81,28 @@ public void bindDriveControls() { } public void bindElevatorControls() { - m_operatorController.circle().onTrue(m_elevatorSubsystem.goToLevelFourCommand()); - m_operatorController.triangle().onTrue(m_elevatorSubsystem.goToLevelThreeCommand()); - m_operatorController.square().onTrue(m_elevatorSubsystem.goToLevelTwoCommand()); - m_operatorController.cross().onTrue(m_elevatorSubsystem.goToLevelOneCommand()); - m_operatorController.povLeft().onTrue(m_elevatorSubsystem.goToCoralStationCommand()); + m_operatorController.circle().onTrue(scoreLevelFour()); + m_operatorController.triangle().onTrue(scoreLevelThree()); + m_operatorController.square().onTrue(scoreLevelTwo()); + m_operatorController.cross().onTrue(scoreLevelOne()); + m_operatorController.povLeft().onTrue(m_elevatorSubsystem.goToCoralStationHeight()); + m_operatorController.povRight().onTrue(m_elevatorSubsystem.goToBaseHeight()); // TODO: Add manual movement } public void bindAlgaeControls() { m_operatorController.R1().onTrue( - m_algaeGrabberSubsystem.deployGrabberCommand(GrabberState.DOWN) - .andThen(m_algaeGrabberSubsystem.runFlywheelCommand())); // TODO: Come up after? + m_algaeGrabberSubsystem.deployGrabber(GrabberState.DOWN) + .andThen(m_algaeGrabberSubsystem.runFlywheel())); // TODO: Come up after? m_operatorController.L1().onTrue( - m_algaeGrabberSubsystem.runFlywheelReverseCommand() + m_algaeGrabberSubsystem.runFlywheelReverse() .until(m_algaeGrabberSubsystem::checkCurrentOnFlywheel) - .andThen(m_algaeGrabberSubsystem.stopFlywheelCommand())); + .andThen(m_algaeGrabberSubsystem.stopFlywheel())); } public void bindWristControls() { - m_driverController.circle().onTrue(m_wristSubsystem.reverseMotor()); - m_driverController.square().onTrue(m_wristSubsystem.forwardMotor()); + // m_driverController.circle().onTrue(m_wristSubsystem.reverseMotor()); + // m_driverController.square().onTrue(m_wristSubsystem.forwardMotor()); } public void bindCheeseStickControls() { @@ -112,7 +113,7 @@ public void bindCheeseStickControls() { public void bindClimberControls() { m_operatorController.L2().whileTrue(m_climberSubsystem.moveForward()) - .onFalse(m_climberSubsystem.moveBackward()); + .onFalse(m_climberSubsystem.moveBackward()); // TODO: will this run forever? (no) } @Override diff --git a/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java index cf12cbf..b966223 100644 --- a/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java @@ -99,7 +99,7 @@ public boolean checkCurrentOnFlywheel() { * * @return (setVelocity(0)) sets the flywheel velocity to 0 */ - public Command stopFlywheelCommand() { + public Command stopFlywheel() { return runOnce(() -> { setVelocity(0); }); @@ -110,7 +110,7 @@ public Command stopFlywheelCommand() { * * @return (setVelocity(.75)) sets the flywheel velocity to 75% */ - public Command runFlywheelCommand() { + public Command runFlywheel() { return runOnce(() -> { setVelocity(.75); }); @@ -121,7 +121,7 @@ public Command runFlywheelCommand() { * * @return (setVelocity(-.75)) sets the flywheel velocity to -75% */ - public Command runFlywheelReverseCommand() { + public Command runFlywheelReverse() { return runOnce(() -> { setVelocity(-.75); }); @@ -136,7 +136,7 @@ public Command runFlywheelReverseCommand() { * @return moves the whole grabber setup using a PID based on the grabberState * enum given */ - public Command deployGrabberCommand(GrabberState state) { + public Command deployGrabber(GrabberState state) { return runOnce(() -> { if (GrabberState.DOWN == state) { m_grabberClosedLoopController.setReference(2, ControlType.kPosition); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 607d0e3..7720c2f 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -230,11 +230,11 @@ public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSp * * @return A command to reset the gyro heading. */ - public Command resetHeadingCommand() { + public Command resetHeading() { return runOnce(m_gyro::zeroYaw).withName("ResetHeadingCommand"); } - public Command resetOdometryCommand(Pose2d pose) { + public Command resetOdometry(Pose2d pose) { return runOnce(() -> m_odometry.resetPosition(getHeading(), getModulePositions(), pose)) .withName("ResetOdometryCommand"); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index b7d811d..84f24c7 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -108,8 +108,7 @@ public boolean atSetpoint() { /** * Sets the position of the motor (using PID) * - * @param position Position to set the master motor to (the follower - * will...follow) + * @param position Position to set the master motor */ public void setPosition(double position) { m_setPosition = position; @@ -153,7 +152,7 @@ public void periodic() { * * @return Sets the speed to 0 */ - public Command stopMotorCommand() { + public Command stopMotor() { return runOnce(() -> setSpeed(0)).withName("Elevator motor stop"); } @@ -162,7 +161,7 @@ public Command stopMotorCommand() { * * @return the command to set the speed */ - public Command reverseMotorCommand() { + public Command reverseMotor() { return runOnce(() -> setSpeed(-1)).withName("Elevator motor backwards"); } @@ -171,28 +170,36 @@ public Command reverseMotorCommand() { * * @return the command to set the speed */ - public Command forwardMotorCommand() { + public Command forwardMotor() { return runOnce(() -> setSpeed(1)).withName("Elevator motor forward"); } - public Command goToLevelOneCommand() { + public Command goToLevelOneHeight() { return runOnce(() -> setPosition(kLevelOneHeight)).withName("Elevator to Level 1"); } - public Command goToLevelTwoCommand() { + public Command goToLevelTwoHeight() { return runOnce(() -> setPosition(kLevelTwoHeight)).withName("Elevator to Level 2"); } - public Command goToLevelThreeCommand() { + public Command goToLevelThreeHeight() { return runOnce(() -> setPosition(kLevelThreeHeight)).withName("Elevator to Level 3"); } - public Command goToLevelFourCommand() { + public Command goToLevelFourHeight() { return runOnce(() -> setPosition(kLevelFourHeight)).withName("Elevator to Level 4"); } - public Command goToCoralStationCommand() { + public Command goToCoralStationHeight() { return runOnce(() -> setPosition(kCoralStationHeight)).withName("Elevator to Coral Station"); } + public Command goToBaseHeight() { + return runOnce(() -> setPosition(0)).withName("Go to Base Height"); + } + + public Command lowerToScore() { + return runOnce(() -> setPosition(getPosition() - kToScoreHeightDecrease)).withName("Lower Elevator to Score"); + } + } \ No newline at end of file