From 923c04bf34a7be207f0bd5c51a5c120cedf082e4 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 2 Mar 2020 18:04:08 -0700 Subject: [PATCH] Fixes of intake and beams (untested) --- src/main/java/frc4388/robot/Constants.java | 10 ++--- .../java/frc4388/robot/RobotContainer.java | 34 ++++++++++------ .../robot/commands/RunExtenderOutIn.java | 6 +-- .../robot/commands/RunIntakeWithTriggers.java | 12 ++---- .../frc4388/robot/commands/StorageIntake.java | 4 +- .../robot/commands/StorageIntakeFinal.java | 40 +++++++++++++++++++ .../robot/commands/StoragePrepIntake.java | 4 +- .../java/frc4388/robot/subsystems/Intake.java | 19 +++++++-- .../frc4388/robot/subsystems/Storage.java | 10 ++--- 9 files changed, 95 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e09b86a..75703cc 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -159,12 +159,10 @@ public final class Constants { public static final double STORAGE_TIMEOUT = 2000; /* Ball Indexes */ - public static final int BEAM_SENSOR_DIO_0 = 0; - public static final int BEAM_SENSOR_DIO_1 = 1; - public static final int BEAM_SENSOR_DIO_2 = 2; - public static final int BEAM_SENSOR_DIO_3 = 3; - public static final int BEAM_SENSOR_DIO_4 = 4; - public static final int BEAM_SENSOR_DIO_5 = 5; + public static final int BEAM_SENSOR_SHOOTER = 1; + public static final int BEAM_SENSOR_USELESS = 2; + public static final int BEAM_SENSOR_STORAGE = 3; + public static final int BEAM_SENSOR_INTAKE = 4; /* PID Values */ public static final int SLOT_DISTANCE = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 718abcb..3afc1a3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -136,7 +136,8 @@ public class RobotContainer { m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); + // runs the storage not + m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -188,8 +189,8 @@ public class RobotContainer { // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(new RunExtenderOutIn(m_robotIntake)); - + //.whenPressed(new RunExtenderOutIn(m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.3))); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) @@ -197,27 +198,34 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) - .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + //.whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + //.whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3))); //Prepares storage for intaking - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); + //new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) + // .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); //Runs storage to outtake - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) - .whileHeld(new StorageOutake(m_robotStorage)); + //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + // .whileHeld(new StorageOutake(m_robotStorage)); //TEST FOR HOOD new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3))) - .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0.3))) + //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeOut(0.1))) + .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeOut(0.0))); + //.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.3))); //TEST FOR HOOD new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3))) - .whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + //.whileHeld(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(-0.3))) + //.whenReleased(new RunCommand(() -> m_robotShooter.m_angleAdjustMotor.set(0))); + .whileHeld(new RunCommand(() -> m_robotIntake.runIntakeIn(0.1))) + .whenReleased(new RunCommand(() -> m_robotIntake.runIntakeIn(0.0))); + //Trims shooter new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS) diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index 3c062a0..64139a0 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -38,9 +38,9 @@ public class RunExtenderOutIn extends CommandBase { addRequirements(m_intake); m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_extenderForwardLimit.enableLimitSwitch(false); - m_extenderReverseLimit.enableLimitSwitch(false); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderForwardLimit.enableLimitSwitch(true); + m_extenderReverseLimit.enableLimitSwitch(true); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java index 5f9dc4a..b9f6298 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java @@ -41,16 +41,12 @@ public class RunIntakeWithTriggers extends CommandBase { double rightTrigger = m_controller.getRightTriggerAxis(); double leftTrigger = m_controller.getLeftTriggerAxis(); double output = 0; - if (leftTrigger < .5) { - if(leftTrigger > rightTrigger) { - output = leftTrigger; - } - if (rightTrigger > leftTrigger) { - output = -leftTrigger; - } - } else { + if(leftTrigger >= rightTrigger) { output = leftTrigger; } + if (rightTrigger > leftTrigger) { + output = -rightTrigger; + } m_intake.runIntake(output); } diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java index baf1630..2a1123a 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/StorageIntake.java @@ -39,7 +39,7 @@ public class StorageIntake extends CommandBase { @Override public void execute() { - if (!m_storage.getBeam(0)){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE)){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); intook = true; } @@ -56,7 +56,7 @@ public class StorageIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(0) && intook){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE) && intook){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java new file mode 100644 index 0000000..2a7d10a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +public class StorageIntakeFinal extends CommandBase { + /** + * Creates a new StorageIntakeFinal. + */ + public StorageIntakeFinal() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // 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/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java index 44a2d2e..2fa52e1 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java @@ -38,7 +38,7 @@ public class StoragePrepIntake extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(0)){ + if (m_storage.getBeam(StorageConstants.BEAM_SENSOR_STORAGE)){ m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ @@ -54,7 +54,7 @@ public class StoragePrepIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(0) || startTime + StorageConstants.STORAGE_TIMEOUT <= System.currentTimeMillis()){ + if (!m_storage.getBeam(StorageConstants.BEAM_SENSOR_INTAKE) || startTime + StorageConstants.STORAGE_TIMEOUT <= 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 f074b33..1de90f4 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -14,6 +14,7 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.IntakeConstants; @@ -38,10 +39,10 @@ public class Intake extends SubsystemBase { m_intakeMotor.setIdleMode(IdleMode.kCoast); m_extenderMotor.setIdleMode(IdleMode.kBrake); m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(false); + m_extenderMotor.setInverted(true); - m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); m_extenderForwardLimit.enableLimitSwitch(true); m_extenderReverseLimit.enableLimitSwitch(true); } @@ -58,7 +59,14 @@ public class Intake extends SubsystemBase { public void runIntake(double input) { m_intakeMotor.set(input); } + + public void runIntakeIn(double input){ + m_extenderMotor.set(-input); + } + public void runIntakeOut(double input){ + m_extenderMotor.set(input); + } /** * Runs extender motor * @param input the percent output to run motor at @@ -67,9 +75,12 @@ public class Intake extends SubsystemBase { if (m_extenderForwardLimit.get()) { isExtended = true; } - if (m_extenderReverseLimit.get()) { + else if (m_extenderReverseLimit.get()) { isExtended = false; } + else{ + m_extenderMotor.set(-input); + } if (isExtended == false) { m_extenderMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 7bcc411..604176c 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -43,12 +43,10 @@ public class Storage extends SubsystemBase { */ public Storage() { resetEncoder(); - m_beamSensors[0] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_0); - m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_1); - m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_2); - m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_3); - m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_4); - m_beamSensors[5] = new DigitalInput(StorageConstants.BEAM_SENSOR_DIO_5); + m_beamSensors[1] = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); + m_beamSensors[2] = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); + m_beamSensors[3] = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); + m_beamSensors[4] = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); } @Override