diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 033c72d..c18cd6a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,4 +12,10 @@ *

It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. */ -public final class Constants {} +public final class Constants { + public static final int ARM_MOTOR_CAN_ID = 4; + public static final int ARM_MOTOR_SWITCH_ID = 0; + + public static final int MOVE_UP = 0.7; + public static final int MOVE_DOWN = -0.2; +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5133735..6b61fff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,12 +18,19 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... + public static XboxController xboxController = new XboxController(0); + public static Joystick leftJoystick = new Joystick(1); + public static Joystick rightJoystick = new Joystick(2); + private Arm am; + private RotateArm rotateArm; /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings + arm = new Arm(); + rotateArm = new RotateArm(arm); configureButtonBindings(); } @@ -34,6 +41,8 @@ public RobotContainer() { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { + JoystickButton rotateArmButton = new JoystickButton(xboxController, XboxController.button.kBumperleft.value); + rotateArmButton.whileHeld(new RotateArm(arm)) } /** diff --git a/src/main/java/frc/robot/commands/RotateArm.java b/src/main/java/frc/robot/commands/RotateArm.java new file mode 100644 index 0000000..972e96e --- /dev/null +++ b/src/main/java/frc/robot/commands/RotateArm.java @@ -0,0 +1,40 @@ +package frc.robot.commands; + +public class RotateArm extends CommandBase { + private Arm am; + public RotateArm(Arm arm){ + this.arm = arm; + addRequirements(arm); + } + @Override + public void execute(){ + boolean leftTriggered = RobotContainer.leftJoystick.getTriggerPressed() + boolean rightTriggered = RobotContainer.rightJoystick.getTriggerPressed() + + public void rotate(leftTriggered, rightTriggered){ + if (leftTriggered && !rightTriggered) { + armMotor.moveUp(); + } else if (rightTriggered && leftTriggered) { + armMotor.moveDown(); + } else { + armMotor.setSpeed(0); + } + } + + if (arm.getLimitSwitch()) { + arm.stopRotate(); + } else { + arm.rotate(RobotContainer.leftJoystick, RobotContainer.rightJoystick); + } + } + + @Override + public void end(boolean interrupted){ + + } + + @Override + public boolean isFinished(){ + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..1ee158b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,29 @@ +import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Arm extends SubsystemBase { + private VictorSP armMotor; + private DigitalInput limitSwitch; + + public Arm(){ + armMotor = new VictorSP(Constants.ARM_MOTOR_CAN_ID) + limitSwitch = new DigitalInput(Constants.ARM_MOTOR_SWITCH_ID); + } + + public boolean getLimitSwitch(){ + return limitSwitch.get(); + } + + public void moveUp() { + armMotor.setSpeed(Constants.MOVE_UP); + } + + public void moveDown() { + armMotor.setSpeed(Constants.MOVE_DOWN); + } + + public void stopRotate(){ + armMotor.setSpeed(0); + } +}