diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4bc7d99..0e563ef 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,10 +63,13 @@ public static class DriverConstants { public static final double[] maxSpeedOptionsRotation = {0.1, 0.75, 1}; } - public static class HangerContstants { + public static class HangerConstants { public static final double HANGER_PID_P = 0; public static final double HANGER_PID_I = 0; public static final double HANGER_PID_D = 0; + + public static final double HANGER_MOTOR_P = 0.3; + public static final double HANGER_MOTOR_N = -0.3; } public static class RobotMovementConstants { diff --git a/src/main/java/frc/robot/commands/HangerMove.java b/src/main/java/frc/robot/commands/HangerMove.java index 32ec519..ab8bb17 100644 --- a/src/main/java/frc/robot/commands/HangerMove.java +++ b/src/main/java/frc/robot/commands/HangerMove.java @@ -67,6 +67,6 @@ public boolean isFinished() { */ @Override public void end(boolean interrupted) { - hangerMotor.stop(); + hangerMotorL.stop(); } } diff --git a/src/main/java/frc/robot/subsystems/Hanger.java b/src/main/java/frc/robot/subsystems/Hanger.java index 82df78d..9bd5d7e 100644 --- a/src/main/java/frc/robot/subsystems/Hanger.java +++ b/src/main/java/frc/robot/subsystems/Hanger.java @@ -6,24 +6,49 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.HangerConstants; public class Hanger extends SubsystemBase { - private final CANSparkMax hangerMotor; - private final CANcoder hangerEncoder; + private final CANSparkMax hangerMotorL; + private final CANSparkMax hangerMotorR; + private final CANcoder hangerEncoderL; + private final CANcoder hangerEncoderR; private final PIDController hangerPIDController; - private final StatusSignal hangerPosition; + private final StatusSignal hangerPositionL; + private final StatusSignal hangerPositionR; + private final DigitalInput magnet; - public Hanger(int hangerMotorID, int hangerEncoderID, int HANGER_PID_P, int HANGER_PID_I, int HANGER_PID_D) { + public Hanger(int hangerMotorLID, int hangerMotorRID, int hangerEncoderLID, int hangerEncoderRID, int HANGER_PID_P, int HANGER_PID_I, int HANGER_PID_D) { - hangerMotor = new CANSparkMax(hangerMotorID, MotorType.kBrushless); - hangerEncoder = new CANcoder(hangerEncoderID); + hangerMotorL = new CANSparkMax(hangerMotorLID, MotorType.kBrushless); + hangerMotorR = new CANSparkMax(hangerMotorRID, MotorType.kBrushless); + hangerEncoderL = new CANcoder(hangerEncoderLID); + hangerEncoderR = new CANcoder(hangerEncoderRID); + DigitalInput magnet = new DigitalInput(0); hangerPIDController = new PIDController(HANGER_PID_P, HANGER_PID_I, HANGER_PID_D); - hangerPosition = hangerEncoder.getAbsolutePosition(); + hangerPositionL = hangerEncoderL.getAbsolutePosition(); + hangerPositionR = hangerEncoderR.getAbsolutePosition(); + } + + public void extendRope() { + hangerMotorL.set(HangerConstants.HANGER_MOTOR_N); + } + + public void intakeRope() { + if (magnet.get()) { + hangerMotorL.set(0); + hangerMotorR.set(0); + } + else { + hangerMotorL.set(HangerConstants.HANGER_MOTOR_P); + hangerMotorR.set(HangerConstants.HANGER_MOTOR_P); + } } @Override