diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c4e5c57..d976918 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,26 +8,46 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.constants.ArmConstants; import frc.robot.constants.IOConstants; import frc.robot.subsystems.Arm; +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.Grabber; public class RobotContainer { - CommandXboxController driver; + CommandPS5Controller driver; Arm arm; + DriveTrain driveTrain; + Grabber grabber; + public RobotContainer() { arm = new Arm(); - driver = new CommandXboxController(IOConstants.kDriverID0); + driveTrain = new DriveTrain(); + grabber = new Grabber(); + driver = new CommandPS5Controller(IOConstants.kDriverID0); configureBindings(); } private void configureBindings() { + driveTrain.setDefaultCommand( + driveTrain.driveArcade( + driver::getLeftY, + driver::getLeftX + ) + ); + grabber.setDefaultCommand(grabber.stop()); + driver.R1().whileTrue(grabber.runGrabberMotor(1)); + driver.L1().whileTrue(grabber.runGrabberMotor(-1)); + arm.setDefaultCommand(arm.stop()); + driver.povUp().whileTrue(arm.runArmMotor(1)); + driver.povDown().whileTrue(arm.runArmMotor(-1)); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GrabberConstants.java b/src/main/java/frc/robot/constants/GrabberConstants.java new file mode 100644 index 0000000..0c01302 --- /dev/null +++ b/src/main/java/frc/robot/constants/GrabberConstants.java @@ -0,0 +1,6 @@ +package frc.robot.constants; + +public class GrabberConstants { + public static final int kGrabberLtMotorID6 = 6; + public static final int kGrabberRtMotorID7 = 7; +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 2a90e2e..1c3102e 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -18,4 +18,8 @@ public class Arm extends SubsystemBase { motor.set(speed); }); } + + public Command stop() { + return run(() -> motor.set(0)); + } } diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java new file mode 100644 index 0000000..fa4f994 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.ArmConstants; +import frc.robot.constants.DriveTrainConstants; +import frc.robot.constants.GrabberConstants; + +public class Grabber extends SubsystemBase { + private WPI_TalonSRX motorRight; + private WPI_TalonSRX motorLeft; + + + public Grabber() { + motorRight = new WPI_TalonSRX(GrabberConstants.kGrabberRtMotorID7); + motorLeft = new WPI_TalonSRX(GrabberConstants.kGrabberLtMotorID6); + + motorLeft.follow(motorRight); + } + public Command runGrabberMotor(double speed) { + return run(() -> { + motorRight.set(speed); + }); + } + public Command stop() { + return run(() -> motorRight.set(0)); + } +}