diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 272cf88..4af2056 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -156,6 +156,16 @@ public final class Constants { 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 PneumaticsConstants { + public static final int PCM_MODULE_ID = 7; + + public static final int SPEED_SHIFT_FORWARD_ID = 0; + public static final int SPEED_SHIFT_REVERSE_ID = 1; + + public static final int COOL_FALCON_FORWARD_ID = 3; + public static final int COOL_FALCON_REVERSE_ID = 2; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2156bd0..d2761a7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,6 +42,7 @@ import frc4388.robot.commands.TrackTarget; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; +import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -56,6 +57,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* Subsystems */ private final Drive m_robotDrive = new Drive(); + private final Pneumatics m_robotPneumatics = new Pneumatics(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); private final Shooter m_robotShooter = new Shooter(); @@ -75,6 +77,10 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + /* Passing Drive and Pneumatics Subsystems */ + m_robotPneumatics.passRequiredSubsystem(m_robotDrive); + m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + configureButtonBindings(); /* Default Commands */ @@ -120,11 +126,11 @@ public class RobotContainer { /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); /* Operator Buttons */ //TODO: Shooter Buttons @@ -231,7 +237,7 @@ public class RobotContainer { * @param state the gearing of the gearbox (true is high, false is low) */ public void setDriveGearState(boolean state) { - m_robotDrive.setShiftState(state); + m_robotPneumatics.setShiftState(state); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d9f1fb2..bbf917b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.PneumaticsConstants; import frc4388.robot.Gains; public class Drive extends SubsystemBase { - /* Create Motors, Gyros, Solenoids, etc */ + /* Create Motors, Gyros, etc */ public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); - public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1); - public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); /* Drive objects to manage Drive Train */ public DifferentialDrive m_driveTrain; public final DifferentialDriveOdometry m_odometry; public Orchestra m_orchestra; + /* Pneumatics Subsystem */ + Pneumatics m_pneumaticsSubsystem; + /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW; @@ -90,7 +92,6 @@ public class Drive extends SubsystemBase { SendableChooser m_songChooser = new SendableChooser(); /* Misc */ - public boolean m_isSpeedShiftHigh; String m_currentSong = ""; /** @@ -276,10 +277,17 @@ public class Drive extends SubsystemBase { updateTime(); updateAngles(); updatePosition(); - runFalconCooling(); updateSmartDashboard(); } + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Pneumatics subsystem) { + m_pneumaticsSubsystem = subsystem; + } + public void updateTime() { m_lastTimeMs = m_currentTimeMs; m_currentTimeMs = System.currentTimeMillis(); @@ -440,47 +448,6 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - /** - * Set to high or low gear based on boolean state, true = high, false = low - * @param state Chooses between high or low gear - */ - public void setShiftState(boolean state) { - if (state == true) { - m_speedShift.set(DoubleSolenoid.Value.kReverse); - } - if (state == false) { - m_speedShift.set(DoubleSolenoid.Value.kForward); - } - setRightMotorGains(state); - m_isSpeedShiftHigh = state; - } - - /** - * Set to open or close solenoid that cools the falcon, true = open, false = close - * @param state Chooses between open and close - */ - public void coolFalcon(boolean state) { - if (state == true) { - m_coolFalcon.set(DoubleSolenoid.Value.kForward); - } - if (state == false) { - m_coolFalcon.set(DoubleSolenoid.Value.kReverse); - } - } - - /** - * - */ - public void runFalconCooling() { - if (m_currentTimeSec % 30 == 0) { - coolFalcon(true); - SmartDashboard.putBoolean("Solenoid", true); - } else if ((m_currentTimeSec - 1) % 30 == 0) { - coolFalcon(false); - SmartDashboard.putBoolean("Solenoid", false); - } - } - /** * Selects a song to play! * @param song The name of the song to be played @@ -633,7 +600,7 @@ public class Drive extends SubsystemBase { * @return The converted value in inches */ public double ticksToInches(double ticks) { - if (m_isSpeedShiftHigh) { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { return ticks * DriveConstants.INCHES_PER_TICK_HIGH; } else { return ticks * DriveConstants.INCHES_PER_TICK_LOW; @@ -646,7 +613,7 @@ public class Drive extends SubsystemBase { * @return The converted value in ticks */ public double inchesToTicks(double inches) { - if (m_isSpeedShiftHigh) { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { return inches * DriveConstants.TICKS_PER_INCH_HIGH; } else { return inches * DriveConstants.TICKS_PER_INCH_LOW; diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java new file mode 100644 index 0000000..1c373ed --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.java @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* 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.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.PneumaticsConstants; + +public class Pneumatics extends SubsystemBase { + /* Create Solenoids */ + public DoubleSolenoid m_speedShift = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID, + PneumaticsConstants.SPEED_SHIFT_FORWARD_ID, + PneumaticsConstants.SPEED_SHIFT_REVERSE_ID ); + + public DoubleSolenoid m_coolFalcon = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID, + PneumaticsConstants.COOL_FALCON_FORWARD_ID, + PneumaticsConstants.COOL_FALCON_REVERSE_ID ); + + /* Get Drive Subsystem */ + Drive m_driveSubsystem; + + public boolean m_isSpeedShiftHigh; + + /** + * Creates a new Pneumatics. + */ + public Pneumatics() { + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + runFalconCooling(); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Drive subsystem) { + m_driveSubsystem = subsystem; + } + + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + m_speedShift.set(DoubleSolenoid.Value.kReverse); + } + if (state == false) { + m_speedShift.set(DoubleSolenoid.Value.kForward); + } + m_driveSubsystem.setRightMotorGains(state); + m_isSpeedShiftHigh = state; + } + + /** + * Set to open or close solenoid that cools the falcon, true = open, false = close + * @param state Chooses between open and close + */ + public void coolFalcon(boolean state) { + if (state == true) { + m_coolFalcon.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + m_coolFalcon.set(DoubleSolenoid.Value.kReverse); + } + } + + /** + * Runs coolFalcon every 30 seconds for 1 second. + */ + public void runFalconCooling() { + if (m_driveSubsystem.m_currentTimeSec % 30 == 0) { + coolFalcon(true); + } else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) { + coolFalcon(false); + } + } +}