diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8507daf..0a5502a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -123,8 +123,9 @@ public final class Constants { public static final int PID_PRIMARY = 0; /* PID Gains */ - public static final double storkminOutput = -1.0; - public static final Gains STORAGE_GAINS = new Gains(0.1, 0, 1, 0, 0, 1); + + public static final double STORAGE_MIN_OUTPUT = -1.0; + public static final Gains STORAGE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 265f57b..f14c5d9 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; @@ -99,10 +101,10 @@ public class RobotContainer { // runs the drum shooter in idle mode m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); // drives the leveler with an axis input from the driver controller - // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); - // runs storage motor at 50 percent - // m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); + } + /** * Use this method to define your button->command mappings. Buttons can be created by @@ -147,6 +149,17 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); + /* Storage Neo PID Test */ + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + .whileHeld(new TrackTarget(m_robotShooter)); + + //Prepares storage for intaking + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) + .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage)); + + //Runs storage to outtake + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) + .whileHeld(new storageOutake(m_robotStorage)); } /** diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java index b0bb140..3c062a0 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java +++ b/src/main/java/frc4388/robot/commands/RunExtenderOutIn.java @@ -7,14 +7,25 @@ package frc4388.robot.commands; +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; 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; + + 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; + + /** * Uses input from opperator to run the extender motor. @@ -25,19 +36,23 @@ public class RunExtenderOutIn extends CommandBase { // Use addRequirements() here to declare subsystem dependencies. m_intake = subsystem; 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); } // Called when the command is initially scheduled. @Override public void initialize() { - isOut = !isOut; - startTime = System.currentTimeMillis(); + m_intake.isExtended = !m_intake.isExtended; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (isOut){ + if (m_intake.isExtended){ m_intake.runExtender(0.3); } else { m_intake.runExtender(-0.3); @@ -54,9 +69,16 @@ public class RunExtenderOutIn extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (startTime + 3000 < System.currentTimeMillis()) { + if (m_intake.isExtended && m_extenderForwardLimit.get() == true){ return true; } - return false; + + else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){ + return true; + } + + else{ + 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..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/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 7d395cd..1902e0d 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -526,6 +526,7 @@ public class Drive extends SubsystemBase { * Returns the current yaw of the pigeon */ public double getGyroYaw() { + double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 62bb30a..6b716f4 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -17,16 +17,21 @@ import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; 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; + public boolean isExtended = false; /** * Creates a new Intake. */ public Intake() { + + m_intakeMotor.restoreFactoryDefaults(); m_extenderMotor.restoreFactoryDefaults(); @@ -59,6 +64,18 @@ public class Intake extends SubsystemBase { * @param input the percent output to run motor at */ public void runExtender(double input) { - m_extenderMotor.set(input); + if (m_extenderForwardLimit.get()) { + isExtended = true; + } + if (m_extenderReverseLimit.get()) { + isExtended = false; + } + + if (isExtended == false) { + m_extenderMotor.set(input); + } + if (isExtended == true) { + m_extenderMotor.set(-input); + } } -} +} \ 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 18df225..3d460fe 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -9,14 +9,17 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; import com.revrobotics.SparkMax; 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; @@ -29,14 +32,16 @@ public class Storage extends SubsystemBase { CANEncoder m_encoder = m_storageMotor.getEncoder(); - public static Gains m_storageGains = StorageConstants.STORAGE_GAINS; + Gains storageGains = StorageConstants.STORAGE_GAINS; + + Intake m_intake; + /** * Creates a new Storage. */ 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); @@ -55,32 +60,38 @@ public class Storage extends SubsystemBase { * * @param input the voltage to run motor at */ - public void runStorage(final double input) { + + public void runStorage(double input) { m_storageMotor.set(input); - 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(m_storageGains.m_kP); - m_storagePIDController.setI(m_storageGains.m_kI); - m_storagePIDController.setD(m_storageGains.m_kD); - m_storagePIDController.setIZone(m_storageGains.m_kIzone); - m_storagePIDController.setFF(m_storageGains.m_kF); - m_storagePIDController.setOutputRange(StorageConstants.storkminOutput, m_storageGains.m_kmaxOutput); + m_storagePIDController.setP(storageGains.m_kP); + m_storagePIDController.setI(storageGains.m_kI); + m_storagePIDController.setD(storageGains.m_kD); + m_storagePIDController.setIZone(storageGains.m_kIzone); + m_storagePIDController.setFF(storageGains.m_kF); + m_storagePIDController.setOutputRange(StorageConstants.STORAGE_MIN_OUTPUT, storageGains.m_kmaxOutput); m_storagePIDController.setReference(targetPos, ControlType.kPosition); } - public double getEncoderPos() - { + + public double getEncoderPos(){ return m_encoder.getPosition(); } + + public boolean getBeam(int id){ + return m_beamSensors[id].get(); + } + + public void setStoragePID(double position){ + m_storagePIDController.setReference(position , ControlType.kPosition); + } }