From 629046c0329094a6373f70ecb4ee3e2f8b006b44 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 21 Feb 2020 20:41:36 -0700 Subject: [PATCH] aaaaaaaaaaaa --- .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/commands/StorageIntakeFinal.java | 46 +++++++++++++++ .../robot/commands/StorageIntakeGroup.java | 27 +++++++++ .../frc4388/robot/commands/storageIntake.java | 58 +++++++++++++++++++ .../frc4388/robot/commands/storageOutake.java | 44 ++++++++++++++ .../robot/commands/storagePrepAim.java | 52 +++++++++++++++++ .../robot/commands/storagePrepIntake.java | 56 ++++++++++++++++++ .../frc4388/robot/subsystems/Storage.java | 51 +++++----------- 8 files changed, 300 insertions(+), 40 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeFinal.java create mode 100644 src/main/java/frc4388/robot/commands/StorageIntakeGroup.java create mode 100644 src/main/java/frc4388/robot/commands/storageIntake.java create mode 100644 src/main/java/frc4388/robot/commands/storageOutake.java create mode 100644 src/main/java/frc4388/robot/commands/storagePrepAim.java create mode 100644 src/main/java/frc4388/robot/commands/storagePrepIntake.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 26d1a69..ac85461 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; +import frc4388.robot.commands.StorageIntakeGroup; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -49,6 +50,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; @@ -171,11 +173,11 @@ public class RobotContainer { //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new RunCommand(() -> m_robotStorage.storageIntake(m_robotIntake))); + .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage)); //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) - .whileHeld(new RunCommand(() -> m_robotStorage.storageOuttake())); + .whileHeld(new storageOutake(m_robotStorage)); } /** 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..9576f35 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Storage; + +public class StorageIntakeFinal extends CommandBase { + Storage m_storage; + /** + * Creates a new StorageIntakeFinal. + */ + public StorageIntakeFinal(Storage subsystem) { + m_storage = subsystem; + addRequirements(m_storage); + } + + // 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() { + if (m_storage.getBeam(1)){ + m_storage.setStoragePID(m_storage.getEncoderPos() + 5); + } + } + + // 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/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java new file mode 100644 index 0000000..767ed69 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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.SequentialCommandGroup; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Storage; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class StorageIntakeGroup extends SequentialCommandGroup { + /** + * Creates a new StorageIntakeGroup. + */ + public StorageIntakeGroup(Intake m_intake, Storage m_storage) { + addCommands( + new storagePrepIntake(m_intake, m_storage), + new storageIntake(m_intake, m_storage), + new StorageIntakeFinal(m_storage)); + } +} diff --git a/src/main/java/frc4388/robot/commands/storageIntake.java b/src/main/java/frc4388/robot/commands/storageIntake.java new file mode 100644 index 0000000..ee30708 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storageIntake.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* 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.robot.subsystems.Storage; + +public class storageIntake extends CommandBase { + public Intake m_intake; + public Storage m_storage; + /** + * Creates a new storageIntake. + */ + public storageIntake(Intake inSub, Storage storeSub) { + m_intake = inSub; + m_storage = storeSub; + addRequirements(m_intake); + addRequirements(m_storage); + } + + // 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() { + + if (m_storage.getBeam(0)){ + m_storage.setStoragePID(m_storage.getEncoderPos() + 2); + 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() { + if (m_storage.getBeam(0)){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storageOutake.java b/src/main/java/frc4388/robot/commands/storageOutake.java new file mode 100644 index 0000000..4820dc0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storageOutake.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Storage; + +public class storageOutake extends CommandBase { + Storage m_storage; + /** + * Creates a new storageOutake. + */ + public storageOutake(Storage storeSub) { + m_storage = storeSub; + addRequirements(m_storage); + } + + // 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() { + m_storage.runStorage(-0.5); + } + + // 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/storagePrepAim.java b/src/main/java/frc4388/robot/commands/storagePrepAim.java new file mode 100644 index 0000000..c8b3bad --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storagePrepAim.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Storage; + +public class storagePrepAim extends CommandBase { + Storage m_storage; + /** + * Creates a new storagePrepAim. + */ + public storagePrepAim(Storage storeSub) { + m_storage = storeSub; + addRequirements(m_storage); + } + + // 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() { + if (m_storage.getBeam(2) == false){ + m_storage.runStorage(0.5); + } + else{ + m_storage.runStorage(0); + } + } + + // 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() { + if (m_storage.getBeam(2)){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/storagePrepIntake.java b/src/main/java/frc4388/robot/commands/storagePrepIntake.java new file mode 100644 index 0000000..7fab016 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/storagePrepIntake.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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.robot.subsystems.Storage; + +public class storagePrepIntake extends CommandBase { + public Intake m_intake; + public Storage m_storage; + /** + * Creates a new storagePrepIntake. + */ + public storagePrepIntake(Intake inSub, Storage storeSub) { + m_intake = inSub; + m_storage = storeSub; + addRequirements(m_intake); + addRequirements(m_storage); + } + + // 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() { + if (m_storage.getBeam(1) == false){ + m_storage.runStorage(-0.5); + } + else{ + m_storage.runStorage(0); + } + } + + // 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() { + if (m_storage.getBeam(1)){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index c71355a..24422c9 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,6 +19,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; @@ -35,12 +36,14 @@ public class Storage extends SubsystemBase { Intake m_intake; + public boolean botReached; + /** * Creates a new Storage. */ public Storage() { resetEncoder(); - + boolean botReached = false; 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); @@ -65,14 +68,12 @@ public class Storage extends SubsystemBase { final boolean beam_on = m_beamSensors[0].get(); } - public void resetEncoder() - { + public void resetEncoder(){ m_encoder.setPosition(0); } /* Storage PID Control */ - public void runStoragePositionPID(double targetPos) - { + public void runStoragePositionPID(double targetPos){ // Set PID Coefficients m_storagePIDController.setP(storageGains.m_kP); m_storagePIDController.setI(storageGains.m_kI); @@ -84,41 +85,15 @@ public class Storage extends SubsystemBase { m_storagePIDController.setReference(targetPos, ControlType.kPosition); } - public void getEncoderPos() - { - m_encoder.getPosition(); + public double getEncoderPos(){ + return m_encoder.getPosition(); } - /** - * Prepares storage for shooting - */ - public void storageAim() { - if (m_beamSensors[2].get() == false){ - m_storageMotor.set(0.5); - } - else{ - m_storageMotor.set(0); - } + public boolean getBeam(int id){ + return m_beamSensors[id].get(); } -public void storageIntake(Intake intake) { - m_intake = intake; - if (m_beamSensors[1].get() == false){ - m_storageMotor.set(-0.5); - } - else{ - m_storageMotor.set(0); - } - if (m_beamSensors[0].get()){ - m_intake.runExtender(-0.3); - m_storagePIDController.setReference(10, ControlType.kPosition); - } -} -public void storageOuttake() { - m_storageMotor.set(1); - - /* - *If shooting move storage motor until top sensor is tripped - *If intaking move storage motor until bottom sensor is tripped - */ + + public void setStoragePID(double position){ + m_storagePIDController.setReference(position , ControlType.kPosition); } }