Merge pull request #45 from Team4388/fix-drive-train

Fix Drive Train

src/main/java/frc4388/robot/Constants.java

@@ -30,23 +30,30 @@ public final class Constants {
         public static final int PIGEON_ID = 6;
 
         /* Drive Inversions */
-        public static final boolean isRightMotorInverted = false;
+        public static final boolean isRightMotorInverted = true;
         public static final boolean isLeftMotorInverted = false;
         public static final boolean isRightArcadeInverted = false;
         public static final boolean isAuxPIDInverted = false;
 
         /* Drive Configuration */
-        public static final double OPEN_LOOP_RAMP_RATE = 0.1;
+        public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
+        public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor
         public static final double NEUTRAL_DEADBAND = 0.04;
         public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
             new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01);
-        public static final int CLOSED_LOOP_TIME_MS = 1;
+        public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
+
+        /* Drive Train Characteristics */
+        public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
+        public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
+        public static final double WHEEL_DIAMETER_INCHES = 6;
+        public static final double TICKS_PER_GYRO_REV = 8192;
+        public static final double TICKS_PER_MOTOR_REV = 2048;
 
         /* PID Constants Drive*/
-        public static final int DRIVE_TIMEOUT_MS = 30;
         public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5);
         public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0);
-        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55);
+        public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55);
         public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
         public static final int DRIVE_CRUISE_VELOCITY = 20000;
         public static final int DRIVE_ACCELERATION = 7000;
@@ -78,13 +85,6 @@ public final class Constants {
 	    public final static int SLOT_TURNING = 2;
 	    public final static int SLOT_MOTION_MAGIC = 3;
         
-        /* Drive Train Characteristics */
-        public static final double TICKS_PER_MOTOR_REV = 2048;
-        public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;
-        public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15;
-        public static final double WHEEL_DIAMETER_INCHES = 6;
-        public static final double TICKS_PER_GYRO_REV = 8192;
-        
         /* Ratio Calculation */
         public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
         public static final double TICK_TIME_TO_SECONDS = 0.1;
@@ -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;

src/main/java/frc4388/robot/Robot.java

@@ -105,7 +105,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
-    m_robotContainer.setDriveGearState(true);
+    //m_robotContainer.setDriveGearState(true);
     //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
     // This makes sure that the autonomous stops running when

src/main/java/frc4388/robot/RobotContainer.java

@@ -21,12 +21,15 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
 import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.DrivePositionMPAux;
+import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -39,9 +42,12 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.Climber;
 import frc4388.robot.commands.RunLevelerWithJoystick;
 import frc4388.robot.commands.TrackTarget;
+import frc4388.robot.commands.TurnDegrees;
+import frc4388.robot.commands.Wait;
 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 +62,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 +82,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 */
@@ -103,11 +114,11 @@ public class RobotContainer {
         /* Test Buttons */
         // A driver test button
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new TurnDegrees(90, m_robotDrive));
         
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new InstantCommand());
+            .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000));
 
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
@@ -120,11 +131,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
@@ -175,11 +186,32 @@ public class RobotContainer {
         RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
         // Run path following command, then stop at the end.
         //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+       
+        //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
+        //This assumes that we are positioned against the right wall with our shooter facing the target.
+        return new SequentialCommandGroup(new Wait(2, m_robotDrive), 
+                                    //add aim command
+                                    //add shooter command
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0),
+                                    new ParallelCommandGroup(
+                                        new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
+                                    new ParallelCommandGroup(
+                                         new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                            new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)),
+                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                    //add aim command 
+                                    //add shooter command
+//Below this would be the picking up additional balls outside of those in the trench directly behind us
 
-        // return new InstantCommand();
+                                    new TurnDegrees(-150, m_robotDrive),
-        return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0),
+                                    new StorageIntakeGroup(m_robotIntake, m_robotStorage),
+                                    new TurnDegrees(75, m_robotDrive),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0),
+                                    new TurnDegrees(-45, m_robotDrive),
+                                    new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0));
     }
-
     TrajectoryConfig getTrajectoryConfig() {
         return new TrajectoryConfig(
             DriveConstants.MAX_SPEED_METERS_PER_SECOND,
@@ -231,7 +263,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);
     }
 
     /**

src/main/java/frc4388/robot/commands/DrivePositionMPAux.java

@@ -23,17 +23,18 @@ public class DrivePositionMPAux extends CommandBase {
   double m_rampAcc;
   long m_startTime;
   long m_rampRate;
+  int m_counter;
 
   /**
    * Creates a new DrivePositionMPAux.
    * 
    * @param subsystem The drive subsystem
-   * @param cruiseVel The target velocity for the motors in units
+   * @param cruiseVel The target velocity for the motors in in/s
    * @param rampDist  The distance before cruise velocity is reached in inches
    * @param rampRate  The time to reach the cruise velocity in seconds
    * @param targetPos The target position
    */
-  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
+  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
     m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
@@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase {
     m_targetVel = m_currentVel;
     m_startTime = System.currentTimeMillis();
     m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+    m_counter = 0;
   }
 
   // Called every m_isRamptime the scheduler runs while the command is scheduled.
@@ -72,6 +74,7 @@ public class DrivePositionMPAux extends CommandBase {
       // Deramp PID
       m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
     }
+    m_counter ++;
   }
 
   // Called once the command ends or is interrupted.
@@ -82,8 +85,8 @@ public class DrivePositionMPAux extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
+    if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) {
-      return true;
+      //return true;
     }
     return false;
   }

src/main/java/frc4388/robot/commands/DriveWithJoystick.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.Drive;
 import frc4388.utility.controller.IHandController;
@@ -38,26 +39,27 @@ public class DriveWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double moveInput = -m_controller.getLeftYAxis();
+    double moveInput = m_controller.getRightXAxis();
-    double steerInput = m_controller.getRightXAxis();
+    double steerInput = -m_controller.getLeftYAxis();
     double moveOutput = 0;
     double steerOutput = 0;
-    if (moveInput >= 0){
+    if (steerInput >= 0){
-      moveOutput = -Math.cos(1.571*moveInput)+1;
+      steerOutput = -Math.cos(1.571*steerInput)+1;
     } else {
-      moveOutput = Math.cos(1.571*moveInput)-1;
+      steerOutput = Math.cos(1.571*steerInput)-1;
     }
 
-    double cosMultiplier = .55;
+    double cosMultiplier = 1.0;
     double deadzone = .1;
-    if (steerInput > 0){
+    if (moveInput > 0){
-      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
+      moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier;
-    } else if (steerInput < 0) {
+    } else if (moveInput < 0) {
-      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
+      moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier;
     } else {
-      steerOutput = 0;
+      moveOutput = 0;
     }
-
+    
+    SmartDashboard.putNumber("Steer Output Test", moveOutput);
     m_drive.driveWithInput(moveOutput, steerOutput);
   }
 

src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java

@@ -12,10 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetGyro, m_currentGyro;
   double m_stopPos;
   long m_currTime, m_deltaTime;
@@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
    * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
    * Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
-   * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
    * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
    * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
    */
-  public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
+  public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) {
     // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
+    m_drive = subsystemDrive;
+    m_pneumatics = subsystemPneumatics;
     m_controller = controller;
     addRequirements(m_drive);
   }
@@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    if (m_drive.m_isSpeedShiftHigh) {
+    if (m_pneumatics.m_isSpeedShiftHigh) {
       runDriveWithInput(moveOutput, steerInput);
       resetGyroTarget();
     }

src/main/java/frc4388/robot/commands/TurnDegrees.java

@@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
+    if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) {
       return true;
     }
     return false;

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<String> m_songChooser = new SendableChooser<String>();
   
   /* Misc */
-  public boolean m_isSpeedShiftHigh;
   String m_currentSong = "";
 
   /**
@@ -105,13 +106,6 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
-    /* flip input so forward becomes back, etc */
-    m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
-    m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
-    m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
-    m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
-
     /* Config Open Loop Ramp so we don't make sudden output changes */
     m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -119,10 +113,10 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Config Supply Current Limit (Use only for debugging) */
-    m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
-    m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
+    //m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
 
     /* Config deadbands so that  */
     m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -251,6 +245,13 @@ public class Drive extends SubsystemBase {
 
     /* Set up Orchestra */
     m_orchestra = new Orchestra();
+
+    /* flip input so forward becomes back, etc */
+    m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
+    m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
+    m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
+    m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
+    //m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
     
     /* Set up music for drive train */
     m_orchestra.addInstrument(m_leftBackMotor);
@@ -273,14 +274,33 @@ public class Drive extends SubsystemBase {
 
   @Override
   public void periodic() {
+    updateTime();
+    updateAngles();
+    updatePosition();
+    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();
     m_currentTimeSec = m_currentTimeMs / 1000;
     m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
+  }
 
+  public void updateAngles() {
     m_lastAngleYaw = m_currentAngleYaw;
     m_currentAngleYaw = getGyroYaw();
+  }
 
+  public void updatePosition() {
     m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
     m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
 
@@ -295,9 +315,6 @@ public class Drive extends SubsystemBase {
     m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               getDistanceInches(m_leftFrontMotor),
                                               -getDistanceInches(m_rightFrontMotor));
-
-    runFalconCooling();
-    updateSmartDashboard();
   }
 
   /**
@@ -305,7 +322,7 @@ public class Drive extends SubsystemBase {
    * using the Differential Drive class to manage the two inputs
    */
   public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(steer, move);
+    m_driveTrain.arcadeDrive(move, steer);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
   }
@@ -431,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
@@ -624,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;
@@ -637,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;
@@ -747,8 +723,8 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
       SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
       SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
-      SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
+      //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
-      SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
+      //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
 
       //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
       //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
@@ -780,9 +756,9 @@ public class Drive extends SubsystemBase {
       SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
 
       //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
-      //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
+      SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
       //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
-      //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
+      SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
       //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
       //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
 
@@ -790,7 +766,7 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("Odometry Heading", getHeading());
 
       //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
-      //SmartDashboard.putNumber("Delta Time", m_deltaTime);
+      SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
 
       if (m_currentSong != m_songChooser.getSelected()){
         m_currentSong = m_songChooser.getSelected();

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);
+    }
+  }
+}

src/main/java/frc4388/robot/subsystems/Storage.java

@@ -19,7 +19,6 @@ 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;