diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f4f8c2b..2840cdb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,7 +66,8 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = 1; + public static final int INTAKE_SPARK_ID = 9; + public static final int EXTENDER_SPARK_ID = 10; } public static final class ClimberConstants { @@ -74,7 +75,7 @@ public final class Constants { } public static final class LevelerConstants { - public static final int LEVELER_CAN_ID = 9; + public static final int LEVELER_CAN_ID = -1; } public static final class StorageConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 941e086..8ecddc4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; +import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; @@ -59,7 +60,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); - // drives motor with input from triggers on the opperator controller + // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // drives climber with input from triggers on the opperator controller m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); @@ -87,6 +88,9 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(new RunExtenderOutIn(m_robotIntake)); /* PID Test Command */ // runs velocity PID while driving straight diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java new file mode 100644 index 0000000..b0bb140 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Intake; +import frc4388.utility.controller.IHandController; + +public class RunExtenderOutIn extends CommandBase { + private Intake m_intake; + private boolean isOut = false; + private long startTime; + + /** + * Uses input from opperator to run the extender motor. + * The left bumper will run the extender in and out. + * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunExtenderOutIn(Intake subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_intake = subsystem; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + isOut = !isOut; + startTime = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (isOut){ + m_intake.runExtender(0.3); + } else { + m_intake.runExtender(-0.3); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_intake.runExtender(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (startTime + 3000 < System.currentTimeMillis()) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index a1c1a4c..62bb30a 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,19 +7,38 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; public class Intake extends SubsystemBase { - Spark m_intakeMotor = new Spark(IntakeConstants.INTAKE_SPARK_ID); - + 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; + /** * Creates a new Intake. */ public Intake() { - + m_intakeMotor.restoreFactoryDefaults(); + m_extenderMotor.restoreFactoryDefaults(); + + m_intakeMotor.setIdleMode(IdleMode.kCoast); + m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); + m_extenderMotor.setInverted(false); + + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); + m_extenderForwardLimit.enableLimitSwitch(false); + m_extenderReverseLimit.enableLimitSwitch(false); } @Override @@ -34,4 +53,12 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(input); } + + /** + * Runs extender motor + * @param input the percent output to run motor at + */ + public void runExtender(double input) { + m_extenderMotor.set(input); + } }