From 92feb32651727881c1df41dc80706c09c5497ce3 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:23:42 -0700 Subject: [PATCH 1/4] Reconfigured the motor to get intake to run. --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Intake.java | 5 ++++- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e788406..1904d8d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,11 +66,11 @@ 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 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 cf5b717..5387b64 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,7 +56,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())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index a1c1a4c..09d1827 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,12 +7,15 @@ package frc4388.robot.subsystems; +import com.revrobotics.CANSparkMax; +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); /** * Creates a new Intake. From 9aeac07431f05023d6e386ddfe9ed86a4baf0350 Mon Sep 17 00:00:00 2001 From: KyraRivera <101209@psdschools.org> Date: Sat, 8 Feb 2020 15:58:11 -0700 Subject: [PATCH 2/4] Added RunExtenderOutIn command and defined extender motor --- src/main/java/frc4388/robot/Constants.java | 1 + .../robot/commands/RunExtenderOutIn.java | 59 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 28 ++++++++- 3 files changed, 86 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunExtenderOutIn.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c3640d2..2840cdb 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -67,6 +67,7 @@ public final class Constants { public static final class IntakeConstants { public static final int INTAKE_SPARK_ID = 9; + public static final int EXTENDER_SPARK_ID = 10; } public static final class ClimberConstants { 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..d5a2e29 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + + /** + * Uses input from opperator triggers to control intake motor. + * The right trigger will run the intake in and the left trigger will run it out. + * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the + * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in + * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + */ + public RunExtenderOutIn(Intake subsystem, IHandController controller) { + // 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; + } + + // 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) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 09d1827..62bb30a 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -7,7 +7,10 @@ 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; @@ -16,13 +19,26 @@ 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; + /** * 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 @@ -37,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); + } } From 674523687939b011f3aa651914dae69bfa44354b Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Mon, 10 Feb 2020 17:37:26 -0700 Subject: [PATCH 3/4] Added an intake Extender command to be tested. --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++++ .../java/frc4388/robot/commands/RunExtenderOutIn.java | 11 +++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6accd17..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; @@ -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 index d5a2e29..c9e6de9 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -14,16 +14,14 @@ 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 triggers to control intake motor. * The right trigger will run the intake in and the left trigger will run it out. * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ - public RunExtenderOutIn(Intake subsystem, IHandController controller) { + public RunExtenderOutIn(Intake subsystem) { // Use addRequirements() here to declare subsystem dependencies. m_intake = subsystem; addRequirements(m_intake); @@ -33,6 +31,7 @@ public class RunExtenderOutIn extends CommandBase { @Override public void initialize() { isOut = !isOut; + startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @@ -49,11 +48,15 @@ public class RunExtenderOutIn extends CommandBase { // 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; } } From 80a0198965d70ee00670cd51de0f1fc7d8dce5a4 Mon Sep 17 00:00:00 2001 From: KyraRivera <59713772+KyraRivera@users.noreply.github.com> Date: Mon, 10 Feb 2020 17:50:52 -0700 Subject: [PATCH 4/4] Updated comments to be accurate --- src/main/java/frc4388/robot/commands/RunExtenderOutIn.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index c9e6de9..b0bb140 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -17,8 +17,8 @@ public class RunExtenderOutIn extends CommandBase { private long startTime; /** - * Uses input from opperator triggers to control intake motor. - * The right trigger will run the intake in and the left trigger will run it out. + * 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) {