diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 679aac3..63cb587 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -22,14 +22,15 @@ import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); - + CANDigitalInput m_extenderForwardLimit; + CANDigitalInput m_extenderReverseLimit; + boolean extended = false; /** * Creates a new Intake. */ public Intake() { - CANDigitalInput m_extenderForwardLimit; - CANDigitalInput m_extenderReverseLimit; + m_intakeMotor.restoreFactoryDefaults(); m_extenderMotor.restoreFactoryDefaults(); @@ -63,6 +64,18 @@ public class Intake extends SubsystemBase { * @param input the percent output to run motor at */ public void runExtender(final double input) { - m_extenderMotor.set(input); + if (m_extenderForwardLimit.get()) { + extended = true; + } + if (m_extenderReverseLimit.get()) { + extended = false; + } + + if (extended == false) { + m_extenderMotor.set(0.5); + } + if (extended == true) { + m_extenderMotor.set(-0.5); + } } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7c35066..db3d2fe 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -106,7 +106,7 @@ public void storageIntake(Intake intake) { m_storageMotor.set(0); } if (m_beamSensors[0].get()){ - m_intake.runExtender(-0.3); + m_intake.runExtender(-0.3); m_storagePIDController.setReference(10, ControlType.kPosition); } }