From dbf0ecfdaa04223114426f9a017cd4ce65edd286 Mon Sep 17 00:00:00 2001 From: Jay Date: Sun, 22 Oct 2023 21:35:26 -0700 Subject: [PATCH] basic bindings to wear in offseason elevator geras --- src/main/java/frc/robot/RobotContainer.java | 160 +----------------- .../robot/commands/TestElevatorCommand.java | 28 +++ .../subsystems/ElevatorTestSubsystem.java | 30 ++++ 3 files changed, 65 insertions(+), 153 deletions(-) create mode 100644 src/main/java/frc/robot/commands/TestElevatorCommand.java create mode 100644 src/main/java/frc/robot/subsystems/ElevatorTestSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20b9d0d..fd1957e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,11 +36,13 @@ import frc.robot.commands.ScoreCommand.ScoreStep; import frc.robot.commands.SetOuttakeModeCommand; import frc.robot.commands.SetZeroModeCommand; +import frc.robot.commands.TestElevatorCommand; import frc.robot.commands.VibrateHIDCommand; import frc.robot.commands.ZeroIntakeCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmState; import frc.robot.subsystems.CANWatchdogSubsystem; +import frc.robot.subsystems.ElevatorTestSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.NetworkWatchdogSubsystem; import frc.robot.subsystems.OuttakeSubsystem; @@ -69,6 +71,8 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... + + private final ElevatorTestSubsystem elevatorSubsystem = new ElevatorTestSubsystem(); private final RGBSubsystem rgbSubsystem = new RGBSubsystem(); private final NetworkWatchdogSubsystem networkWatchdogSubsystem = @@ -87,11 +91,6 @@ public class RobotContainer { private final SharedReference currentNodeSelection = new SharedReference<>(new NodeSelection(NodeSelectorUtility.defaultNodeStack, Height.HIGH)); - /** controller 1 */ - private final CommandXboxController jason = new CommandXboxController(1); - /** controller 1 layer */ - private final Layer jasonLayer = new Layer(jason.rightBumper()); - /** controller 0 */ private final CommandXboxController will = new CommandXboxController(0); /** the sendable chooser to select which auto to run. */ @@ -101,11 +100,6 @@ public class RobotContainer { private final ShuffleboardTab driverView = Shuffleboard.getTab("DriverView"); - /* drive joystick "y" is passed to x because controller is inverted */ - private final DoubleSupplier translationXSupplier = - () -> (-modifyAxis(will.getLeftY()) * Drive.MAX_VELOCITY_METERS_PER_SECOND); - private final DoubleSupplier translationYSupplier = - () -> (-modifyAxis(will.getLeftX()) * Drive.MAX_VELOCITY_METERS_PER_SECOND); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -134,7 +128,6 @@ public RobotContainer() { */ public void containerTeleopInit() { // runs when teleop happens - CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jason.getHID(), 5, .5)); } /** @@ -161,148 +154,9 @@ public void containerMatchStarting() { */ private void configureButtonBindings() { - // vibrate jason controller when in layer - jasonLayer.whenChanged( - (enabled) -> { - final double power = enabled ? .1 : 0; - jason.getHID().setRumble(RumbleType.kLeftRumble, power); - jason.getHID().setRumble(RumbleType.kRightRumble, power); - }); - - jasonLayer - .off(jason.leftBumper()) - .onTrue(new IntakeShootCommand(intakeSubsystem, jason.leftBumper())); - - // outtake states - jasonLayer - .off(jason.leftTrigger()) - .whileTrue( - new ForceOuttakeSubsystemModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE)); - jasonLayer - .off(jason.rightTrigger()) - .onTrue(new SetOuttakeModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.OUTTAKE)); - jasonLayer - .off(jason.x()) - .onTrue(new SetOuttakeModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.OFF)) - .onTrue(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED)); - - // intake presets - // jasonLayer - // .off(jason.a()) - // .onTrue(new ScoreCommand(outtakeSubsystem, armSubsystem, Setpoints.GROUND_INTAKE)) - // .whileTrue( - // new ForceOuttakeSubsystemModeCommand(outtakeSubsystem, - // OuttakeSubsystem.Modes.INTAKE)); - - jasonLayer - .off(jason.b()) - .onTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.SHELF_INTAKE)) - .whileTrue( - new ForceOuttakeSubsystemModeCommand(outtakeSubsystem, OuttakeSubsystem.Modes.INTAKE)); - - // reset - jasonLayer - .off(jason.y()) - .onTrue(new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED)) - .onTrue(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED)); - jason.start().onTrue(new SetZeroModeCommand(armSubsystem)); - - jasonLayer - .off(jason.a()) - .onTrue( - new GroundPickupCommand( - intakeSubsystem, - outtakeSubsystem, - armSubsystem, - () -> - jason.getHID().getPOV() == 180 - ? IntakeSubsystem.Modes.INTAKE_LOW - : IntakeSubsystem.Modes.INTAKE)); - - jason - .povUp() - .onTrue( - new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.OUTTAKE) - .alongWith(new ArmPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF))); - - jason.start().onTrue(new ZeroIntakeCommand(intakeSubsystem)); - - jason - .back() - .whileTrue( - new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE) - .alongWith(new ArmPositionCommand(armSubsystem, Arm.Setpoints.HANDOFF)) - .alongWith( - new ForceOuttakeSubsystemModeCommand( - outtakeSubsystem, OuttakeSubsystem.Modes.OFF))) - .onFalse( - new ArmPositionCommand(armSubsystem, Arm.Setpoints.STOWED) - .alongWith(new IntakeCommand(intakeSubsystem, IntakeSubsystem.Modes.STOWED))); - - // scoring - // jasonLayer - // .on(jason.a()) - // low - - jasonLayer - .on(jason.a()) - .onTrue( - new InstantCommand( - () -> - currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.LOW)))); - - jasonLayer - .on(jason.b()) - .onTrue( - new InstantCommand( - () -> currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.MID)), - armSubsystem)); - - jasonLayer - .on(jason.y()) - .onTrue( - new InstantCommand( - () -> - currentNodeSelection.apply(n -> n.withHeight(NodeSelectorUtility.Height.HIGH)), - armSubsystem)); - - var scoreCommandMap = new HashMap(); - - for (var scoreType : Constants.SCORE_STEP_MAP.keySet()) - scoreCommandMap.put( - scoreType, - new ScoreCommand( - outtakeSubsystem, - armSubsystem, - Constants.SCORE_STEP_MAP.get(scoreType), - jason.leftBumper())); - - jasonLayer - .on(jason.x()) - .onTrue( - new HashMapCommand<>( - scoreCommandMap, () -> currentNodeSelection.get().getScoreTypeIdentifier())); - - jason.povRight().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(1)))); - jason.povLeft().onTrue(new InstantCommand(() -> currentNodeSelection.apply(n -> n.shift(-1)))); - - // control the lights - currentNodeSelection.subscribe( - nodeSelection -> - currentNodeSelection.subscribeOnce( - rgbSubsystem.showMessage( - nodeSelection.nodeStack().type() == NodeSelectorUtility.NodeType.CUBE - ? Constants.Lights.Colors.PURPLE - : Constants.Lights.Colors.YELLOW, - RGBSubsystem.PatternTypes.PULSE, - RGBSubsystem.MessagePriority.F_NODE_SELECTION_COLOR) - ::expire)); - - // show the current node selection - driverView - .addString("Node Selection", () -> currentNodeSelection.get().toString()) - .withPosition(0, 1) - .withSize(2, 1); + will.povUp().onTrue(new TestElevatorCommand(elevatorSubsystem, 1)); + will.povRight().onTrue(new TestElevatorCommand(elevatorSubsystem, 0.5)); + will.povDown().onTrue(new TestElevatorCommand(elevatorSubsystem, 0)); } /** diff --git a/src/main/java/frc/robot/commands/TestElevatorCommand.java b/src/main/java/frc/robot/commands/TestElevatorCommand.java new file mode 100644 index 0000000..0d32f3c --- /dev/null +++ b/src/main/java/frc/robot/commands/TestElevatorCommand.java @@ -0,0 +1,28 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ElevatorTestSubsystem; + +public class TestElevatorCommand extends CommandBase { + + private ElevatorTestSubsystem subsystem; + private double power = 0; + public TestElevatorCommand(ElevatorTestSubsystem subsys, double motorPower) { + subsystem = subsys; + + power = motorPower; + + addRequirements(subsystem); + } + + @Override + public void initialize() { + subsystem.setMotorPower(power); + } + + @Override + public void end(boolean interrupted) { + subsystem.setMotorPower(0); + } + +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorTestSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorTestSubsystem.java new file mode 100644 index 0000000..5e44010 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorTestSubsystem.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ElevatorTestSubsystem extends SubsystemBase { + +private TalonFX testMotor, testMotor2; + + private double motorPower = 0; + + public ElevatorTestSubsystem() { + testMotor = new TalonFX(1000); + testMotor2 = new TalonFX(1001); //FIXME Placeholder + + } + + + public void setMotorPower(double power) { + motorPower = power; + } + + @Override + public void periodic() { + testMotor.set(ControlMode.PercentOutput, motorPower); + testMotor2.set(ControlMode.PercentOutput, motorPower); + } +} \ No newline at end of file