diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c6b23bf..a0dcc1e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -114,9 +114,13 @@ public final class Constants { public static final class HooksConstants { public static final int LEFT_HOOK_ID = -1; public static final int RIGHT_HOOK_ID = -1; + public static final int LEFT_LIMIT_ID = -1; + public static final int RIGHT_LIMIT_ID = -1; public static final double OPEN_POSITION = 0; public static final double CLOSE_POSITION = 0; + + public static final double CALIBRATION_SPEED = 0; } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b8923b..4d2bad0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - //private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook, m_robotMap.leftHookLimit, m_robotMap.rightHookLimit); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -87,9 +87,9 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - // .whenPressed(() -> m_hooks.setOpen(true)) - // .whenReleased(() -> m_hooks.setOpen(false)); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_hooks.setOpen(true)) + .whenReleased(() -> m_hooks.setOpen(false)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 6de2f65..6483ebc 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,10 +4,15 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.CAN; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; @@ -102,6 +107,9 @@ public class RobotMap { public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO /* Hooks Subsystem */ - //public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID); - //public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID); + public final CANSparkMax leftHook = new CANSparkMax(HooksConstants.LEFT_HOOK_ID, MotorType.kBrushless); + public final CANSparkMax rightHook = new CANSparkMax(HooksConstants.RIGHT_HOOK_ID, MotorType.kBrushless); + + public final DigitalInput leftHookLimit = new DigitalInput(HooksConstants.LEFT_LIMIT_ID); + public final DigitalInput rightHookLimit = new DigitalInput(HooksConstants.RIGHT_LIMIT_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java index de07e93..0348ccb 100644 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.HooksConstants; @@ -10,23 +11,25 @@ public class Hooks extends SubsystemBase { private CANSparkMax m_leftHook; private CANSparkMax m_rightHook; - private LimitSwitchNormal m_limitSwitch; + private DigitalInput m_leftLimitSwitch; + private DigitalInput m_rightLimitSwitch; private double m_leftOffset; private double m_rightOffset; private boolean m_open; - public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, LimitSwitchNormal limitSwitch) { + public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, DigitalInput leftLimitSwitch, DigitalInput rightLimitSwitch) { m_leftHook = leftHook; m_rightHook = rightHook; - m_limitSwitch = limitSwitch; + m_leftLimitSwitch = leftLimitSwitch; + m_rightLimitSwitch = rightLimitSwitch; m_open = false; - m_leftHook.set(.1); - m_rightHook.set(.1); + m_leftHook.set(HooksConstants.CALIBRATION_SPEED); + m_rightHook.set(HooksConstants.CALIBRATION_SPEED); } public void setOpen(boolean open) { @@ -47,9 +50,10 @@ public class Hooks extends SubsystemBase { @Override public void periodic() { - if(m_limitSwitch.compareTo(LimitSwitchNormal.NormallyClosed) == 1) { + if(m_leftLimitSwitch.get()) m_leftOffset = m_leftHook.getEncoder().getPosition(); - m_leftOffset = m_leftHook.getEncoder().getPosition(); - } + + if(m_rightLimitSwitch.get()) + m_rightOffset = m_rightHook.getEncoder().getPosition(); } }