Initial commit

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

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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;
+
+import frc4388.utility.LEDPatterns;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+    public static final class DriveConstants {
+        public static final int DRIVE_PIGEON_ID = 6;
+
+        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+    }
+    
+    public static final class LEDConstants {
+        public static final int LED_SPARK_ID = 0;
+
+        public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+    }
+
+    public static final class OIConstants {
+        public static final int XBOX_DRIVER_ID = 0;
+        public static final int XBOX_OPERATOR_ID = 1;
+    }
+}

src/main/java/frc4388/robot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.RobotTime;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  Command m_autonomousCommand;
+  
+  private RobotTime m_robotTime = RobotTime.getInstance();
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be
+   * used for any initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use
+   * this for items like diagnostics that you want ran during disabled,
+   * autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    m_robotTime.updateTimes();
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   * You can use it to reset any subsystem information you want to clear when
+   * the robot is disabled.
+   */
+  @Override
+  public void disabledInit() {
+    m_robotTime.endMatchTime();
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+    m_robotTime.startMatchTime();
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+    m_robotTime.startMatchTime();
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+    
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

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

@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc4388.robot.Constants.*;
+import frc4388.robot.subsystems.LED;
+import frc4388.utility.LEDPatterns;
+import frc4388.utility.controller.IHandController;
+import frc4388.utility.controller.XboxController;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+    /* RobotMap */
+    private final RobotMap m_robotMap = new RobotMap();
+
+    /* Subsystems */
+    private final LED m_robotLED = new LED(m_robotMap.LEDController);
+
+    /* Controllers */
+    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+        configureButtonBindings();
+
+        /* Default Commands */
+        // drives the robot with a two-axis input from the driver controller
+        // continually sends updates to the Blinkin LED controller to keep the lights on
+        m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+    }
+
+    /**
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by instantiating a {@link GenericHID} or one of its subclasses
+     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+        /* Driver Buttons */
+        // test command to spin the robot while pressing A on the driver controller
+
+        /* Operator Buttons */
+        // activates "Lit Mode"
+        new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
+                .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+                .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        // no auto
+        return new InstantCommand();
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public IHandController getDriverController() {
+        return m_driverXbox;
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public IHandController getOperatorController() {
+        return m_operatorXbox;
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public Joystick getOperatorJoystick() {
+        return m_operatorXbox.getJoyStick();
+    }
+
+    /**
+     * Add your docs here.
+     */
+    public Joystick getDriverJoystick() {
+        return m_driverXbox.getJoyStick();
+    }
+}

src/main/java/frc4388/robot/RobotMap.java

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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;
+
+import com.ctre.phoenix.motorcontrol.InvertType;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.RobotGyro;
+
+/**
+ * Defines and holds all I/O objects on the Roborio. This is useful for unit
+ * testing and modularization.
+ */
+public class RobotMap {
+
+    public RobotMap() {
+        configureLEDMotorControllers();
+        configureDriveMotorControllers();
+    }
+
+    /* LED Subsystem */
+    public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+
+    void configureLEDMotorControllers() {
+        
+    }
+
+    void configureDriveMotorControllers() {
+    }
+}

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

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.utility.RobotGyro;
+import frc4388.utility.RobotTime;
+
+/**
+ * Add your docs here.
+ */
+public class DiffDrive extends SubsystemBase {
+  // Put methods for controlling this subsystem
+  // here. Call these from Commands.
+
+  private RobotTime m_robotTime = RobotTime.getInstance();
+
+  private WPI_TalonFX m_leftFrontMotor;
+  private WPI_TalonFX m_rightFrontMotor;
+  private WPI_TalonFX m_leftBackMotor;
+  private WPI_TalonFX m_rightBackMotor;
+  private DifferentialDrive m_driveTrain;
+  private RobotGyro m_gyro;
+
+  /**
+   * Add your docs here.
+   */
+  public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
+      WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
+
+    m_leftFrontMotor = leftFrontMotor;
+    m_rightFrontMotor = rightFrontMotor;
+    m_leftBackMotor = leftBackMotor;
+    m_rightBackMotor = rightBackMotor;
+    m_driveTrain = driveTrain;
+    m_gyro = gyro;
+  }
+
+  @Override
+  public void periodic() {
+    m_gyro.updatePigeonDeltas();
+
+    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+      updateSmartDashboard();
+    }
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void driveWithInput(double move, double steer) {
+    m_driveTrain.arcadeDrive(move, steer);
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void tankDriveWithInput(double leftMove, double rightMove) {
+    m_leftFrontMotor.set(leftMove);
+    m_rightFrontMotor.set(rightMove);
+  }
+
+  /**
+   * Add your docs here.
+   */
+  private void updateSmartDashboard() {
+
+    // Examples of the functionality of RobotGyro
+    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
+    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
+    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
+    SmartDashboard.putData(m_gyro);
+  }
+}

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

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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.Spark;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
+ * Driver
+ */
+public class LED extends SubsystemBase {
+
+  private LEDPatterns m_currentPattern;
+  private Spark m_LEDController;
+
+  /**
+   * Add your docs here.
+   */
+  public LED(Spark LEDController){
+    m_LEDController = LEDController;
+    setPattern(LEDConstants.DEFAULT_PATTERN);
+    updateLED();
+    System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+  }
+
+  @Override
+  public void periodic(){
+    SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void updateLED(){
+    m_LEDController.set(m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   */
+  public void setPattern(LEDPatterns pattern){
+    m_currentPattern = pattern;
+    m_LEDController.set(m_currentPattern.getValue());
+  }
+
+  /**
+   * Add your docs here.
+   * @return
+   */
+  public LEDPatterns getPattern() {
+    return m_currentPattern;
+  }
+}

src/main/java/frc4388/utility/LEDPatterns.java

@@ -0,0 +1,45 @@
+package frc4388.utility;
+
+/**
+ * Add your docs here.
+ */
+public enum LEDPatterns {
+  /* PALLETTE PATTERNS */
+  RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
+    RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
+    PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
+    PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
+    RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
+    RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
+    RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
+    BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
+    GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
+  
+  /* COLOR 1 PATTERNS */
+  C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
+    C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
+    
+  /* COLOR 2 PATTERNS */
+  C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), 
+    C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
+    
+  /* COLOR 1 AND 2 PATTERNS */
+  C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
+    C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
+     
+  /* SOLID COLORS */
+  SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
+    SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
+    SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
+    SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
+    SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
+  
+  /* GETTERS/SETTERS */
+  private final float id;
+  LEDPatterns(float id) {
+    this.id = id;
+  }
+  public float getValue() {
+    return id;
+  }
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.utility;
+
+import com.ctre.phoenix.sensors.PigeonIMU;
+import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.wpilibj.GyroBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+
+/**
+ * Gyro class that allows for interchangeable use between a pigeon and a navX
+ */
+public class RobotGyro extends GyroBase {
+    private RobotTime m_robotTime = RobotTime.getInstance();
+
+    private PigeonIMU m_pigeon = null;
+    private AHRS m_navX = null;
+    public boolean m_isGyroAPigeon; //true if pigeon, false if navX
+
+    private double m_lastPigeonAngle;
+    private double m_deltaPigeonAngle;
+
+    /**
+     * Creates a Gyro based on a pigeon
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(PigeonIMU gyro) {
+        m_pigeon = gyro;
+        m_isGyroAPigeon = true;
+    }
+
+    /**
+     * Creates a Gyro based on a navX
+     * @param gyro the gyroscope to use for Gyro
+     */
+    public RobotGyro(AHRS gyro){
+        m_navX = gyro;
+        m_isGyroAPigeon = false;
+    }
+
+    /**
+     * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
+     * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
+     */
+    public void updatePigeonDeltas() {
+        double currentPigeonAngle = getAngle();
+        m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
+        m_lastPigeonAngle = currentPigeonAngle;
+    }
+
+    /**
+     * <p>NavX:
+     * <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
+     * the center value as the Accumulator center value for subsequent measurements. It's important to
+     * make sure that the robot is not moving while the centering calculations are in progress, this
+     * is typically done when the robot is first turned on while it's sitting at rest before the
+     * competition starts.
+     * 
+     * <p>Pigeon:
+     * <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
+     * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
+     * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
+     * make sure that the robot is not moving while the tempurature calculations are in progress, this
+     * is typically done when the robot is first turned on while it's sitting at rest before the
+     * competition starts.
+     */
+    @Override
+    public void calibrate() {
+        if (m_isGyroAPigeon) {
+            m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
+        } else {
+            m_navX.calibrate();
+        }
+    }
+
+    @Override
+    public void reset() {
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(0);
+        } else {
+            m_navX.reset();
+        }
+    }
+
+    /**
+	 * Get Yaw, Pitch, and Roll data.
+	 *
+	 * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
+	 *					Yaw is within [-368,640, +368,640] degrees.
+	 *					Pitch is within [-90,+90] degrees.
+	 *					Roll is within [-90,+90] degrees.
+     */
+    private double[] getPigeonAngles() {
+        double[] angles = new double[3];
+        m_pigeon.getYawPitchRoll(angles);
+        return angles;
+    }
+
+    @Override
+    public double getAngle() {
+        if (m_isGyroAPigeon) {
+            return getPigeonAngles()[0];
+        } else {
+            return m_navX.getAngle();
+        }
+    }
+
+    /**
+     * Gets an absolute heading of the robot
+     * @return heading from -180 to 180 degrees
+     */
+    public double getHeading() {
+        return getHeading(getAngle());
+    }
+
+    /**
+     * Gets an absolute heading of the robot
+     * @return heading from -180 to 180 degrees
+     */
+    public double getHeading(double angle) {
+        return Math.IEEEremainder(angle, 360);
+    }
+
+    /**
+     * Returns the current pitch value (in degrees, from -90 to 90)
+     * reported by the sensor.  Pitch is a measure of rotation around
+     * the Y Axis.
+     * @return The current pitch value in degrees (-90 to 90).
+     */
+    public double getPitch() {
+        if (m_isGyroAPigeon) {
+            return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
+        } else {
+            return MathUtil.clamp(m_navX.getPitch(), -90, 90);
+        }
+    }
+
+    /**
+     * Returns the current roll value (in degrees, from -90 to 90)
+     * reported by the sensor.  Roll is a measure of rotation around
+     * the X Axis.
+     * @return The current roll value in degrees (-90 to 90).
+     */
+    public double getRoll() {
+        if (m_isGyroAPigeon) {
+            return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
+        } else {
+            return MathUtil.clamp(m_navX.getRoll(), -90, 90);
+        }
+    }
+
+    @Override
+    public double getRate() {
+        if (m_isGyroAPigeon) {
+            return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
+        } else {
+            return m_navX.getRate();
+        }
+    }
+
+    public PigeonIMU getPigeon(){
+        return m_pigeon;
+    }
+
+    public AHRS getNavX(){
+        return m_navX;
+    }
+
+    @Override
+    public void close() throws Exception {
+
+    }
+}

src/main/java/frc4388/utility/RobotTime.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.utility;
+
+/**
+ * <p>Keeps track of Robot times like time passed, delta time, etc
+ * <p>All times are in milliseconds
+ */
+public class RobotTime {
+    private long m_currTime = System.currentTimeMillis();
+    public long m_deltaTime = 0;
+
+    private long m_startRobotTime = m_currTime;
+    public long m_robotTime = 0;
+    public long m_lastRobotTime = 0;
+
+    private long m_startMatchTime = 0;
+    public  long m_matchTime = 0;
+    public long m_lastMatchTime = 0;
+
+    public long m_frameNumber = 0;
+
+    /**
+     * Private constructor prevents other classes from instantiating
+     */
+    private RobotTime(){}
+
+    private static RobotTime instance = null;
+
+    /**
+     * Gets the instance of Robot Time. If there is no instance running one will be created.
+     * @return instance of Robot Time
+     */
+    public static RobotTime getInstance() {
+        if (instance == null) {
+            instance = new RobotTime();
+        }
+        return instance;
+    }
+
+    /**
+     * Call this once per periodic loop.
+     */
+    public void updateTimes() {
+        m_lastRobotTime = m_robotTime;
+        m_lastMatchTime = m_matchTime;
+
+        m_currTime = System.currentTimeMillis();
+        m_robotTime = m_currTime - m_startRobotTime;
+        m_deltaTime = m_robotTime - m_lastRobotTime;
+        m_frameNumber++;
+
+        if (m_startMatchTime != 0) {
+            m_matchTime = m_currTime - m_startMatchTime;
+        }
+    }
+
+    /**
+     * Call this in both the auto and periodic inits
+     */
+    public void startMatchTime() {
+        if (m_startMatchTime == 0) {
+            m_startMatchTime = m_currTime;
+        }
+    }
+
+    /**
+     * Call this in disabled init
+     */
+    public void endMatchTime() {
+        m_startMatchTime = 0;
+        m_matchTime = 0;
+    }
+}

src/main/java/frc4388/utility/RobotUnits.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Aarav's good units class (better than WPILib)
+ * @author Aarav Shah */
+
+public class RobotUnits {
+  // constants
+
+  // angle conversions
+  public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
+
+  public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
+
+  // falcon conversions
+  public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
+
+  public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
+
+  // distance conversions
+  public static double metersToFeet(final double meters) {return meters * 3.28084;}
+
+  public static double feetToMeters(final double feet) {return feet / 3.28084;}
+}

src/main/java/frc4388/utility/controller/IHandController.java

@@ -0,0 +1,21 @@
+package frc4388.utility.controller;
+
+/**
+ * Add your docs here.
+ */
+public interface IHandController {
+
+	public double getLeftXAxis();
+
+	public double getLeftYAxis();
+
+	public double getRightXAxis();
+	
+	public double getRightYAxis();
+
+	public double getLeftTriggerAxis();
+
+	public double getRightTriggerAxis();
+
+	public int getDpadAngle();
+}

src/main/java/frc4388/utility/controller/XboxController.java

@@ -0,0 +1,218 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * This is a wrapper for the WPILib Joystick class that represents an XBox 
+ * controller.
+ * @author frc1675
+ */
+public class XboxController implements IHandController
+{
+	public static final int LEFT_X_AXIS = 0;
+	public static final int LEFT_Y_AXIS = 1;
+	public static final int LEFT_TRIGGER_AXIS = 2;
+	public static final int RIGHT_TRIGGER_AXIS = 3;
+	public static final int RIGHT_X_AXIS = 4;
+	public static final int RIGHT_Y_AXIS = 5;
+	public static final int LEFT_RIGHT_DPAD_AXIS = 6;
+	public static final int TOP_BOTTOM_DPAD_AXIS = 6;
+
+	public static final int A_BUTTON = 1;
+	public static final int B_BUTTON = 2;
+	public static final int X_BUTTON = 3;
+	public static final int Y_BUTTON = 4;
+	public static final int LEFT_BUMPER_BUTTON = 5;
+	public static final int RIGHT_BUMPER_BUTTON = 6;
+	public static final int BACK_BUTTON = 7;
+	public static final int START_BUTTON = 8;
+
+	public static final int LEFT_JOYSTICK_BUTTON = 9;
+	public static final int RIGHT_JOYSTICK_BUTTON = 10;
+
+	private static final double LEFT_DPAD_TOLERANCE = -0.9;
+	private static final double RIGHT_DPAD_TOLERANCE = 0.9;
+	private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
+	private static final double TOP_DPAD_TOLERANCE = 0.9;
+
+	private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
+	private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
+
+	private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double DEADZONE = 0.1;
+
+	private Joystick m_stick;
+
+	/**
+     * Add your docs here.
+     */
+	public XboxController(int portNumber){
+		m_stick = new Joystick(portNumber);
+	}
+
+	/**
+     * Add your docs here.
+     */
+	public Joystick getJoyStick() {
+		return m_stick;
+	}
+	
+	/**
+	 * Checks if the input falls within the deadzone.
+	 * @param input from an axis on the controller
+	 * @return true if input falls in deadzone, false if input falls outside deadzone
+	 */
+	private boolean inDeadZone(double input){
+		return (Math.abs(input) < DEADZONE);
+	}
+
+	/**
+	 * Updates an input to have a deadzone around the 0 position
+	 * @param input from an axis on the controller
+	 * @return updated input
+	 */
+	private double getAxisWithDeadZoneCheck(double input){
+		if(inDeadZone(input)){
+			return 0.0;       
+		} else {
+			return input;
+		}
+	}
+
+	public boolean getAButton(){
+		return m_stick.getRawButton(A_BUTTON);
+	}
+
+	public boolean getXButton(){
+		return m_stick.getRawButton(X_BUTTON);    
+	}
+
+	public boolean getBButton(){
+		return m_stick.getRawButton(B_BUTTON);
+	}
+
+	public boolean getYButton(){
+		return m_stick.getRawButton(Y_BUTTON);
+	} 
+
+	public boolean getBackButton(){
+		return m_stick.getRawButton(BACK_BUTTON);
+	}
+
+	public boolean getStartButton(){
+		return m_stick.getRawButton(START_BUTTON);
+	} 
+
+	public boolean getLeftBumperButton(){
+		return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
+	}
+
+	public boolean getRightBumperButton(){
+		return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
+	}
+
+	public boolean getLeftJoystickButton(){
+		return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
+	}
+
+	public boolean getRightJoystickButton(){
+		return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
+	}
+
+	public double getLeftXAxis(){         
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); 
+	}
+
+	public double getLeftYAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
+	}
+
+	public double getRightXAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
+	}
+
+	public double getRightYAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); 
+	}
+
+	public double getLeftTriggerAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); 
+	}
+
+	public double getRightTriggerAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); 
+	}
+
+	/**
+	 * Get the angle input from the dpad.
+	 * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
+	 */
+	public int getDpadAngle() {
+		return m_stick.getPOV(0);
+	}
+
+	public boolean getDPadLeft(){
+      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadRight(){
+      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadTop(){
+      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadBottom(){
+      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
+  	}
+
+	public boolean getLeftTrigger(){
+		return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
+	}
+
+	public boolean getRightTrigger(){
+		return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
+	}   
+
+	public boolean getRightAxisUpTrigger(){
+		return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getRightAxisDownTrigger(){
+		return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getRightAxisLeftTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getRightAxisRightTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisUpTrigger(){
+		return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisDownTrigger(){
+		return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisLeftTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisRightTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
+	}
+}

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -0,0 +1,69 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj2.command.button.Button;
+import frc4388.utility.controller.XboxController;
+
+/**
+ * Mapping for the Xbox controller triggers to allow triggers to be defined as
+ * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger 
+ * exceeds a tolerance defined in {@link XboxController}.
+ */
+public class XboxTriggerButton extends Button {
+	public static final int RIGHT_TRIGGER = 0;
+	public static final int LEFT_TRIGGER = 1;
+	public static final int RIGHT_AXIS_UP_TRIGGER = 2;
+	public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
+	public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
+	public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
+	public static final int LEFT_AXIS_UP_TRIGGER = 6;
+	public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
+	public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
+	public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
+
+	private XboxController m_controller;
+	private int m_trigger;
+
+	/**
+	 * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
+	 */
+	public XboxTriggerButton(XboxController controller, int trigger) {
+		m_controller = controller;
+		m_trigger = trigger;
+	}
+
+	/** {@inheritDoc} */
+	@Override
+	public boolean get() {
+		if (m_trigger == RIGHT_TRIGGER) {
+			return m_controller.getRightTrigger();
+		}
+		else if (m_trigger == LEFT_TRIGGER) {
+			return m_controller.getLeftTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
+			return m_controller.getRightAxisUpTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getRightAxisDownTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getRightAxisRightTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getRightAxisLeftTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
+			return m_controller.getLeftAxisUpTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getLeftAxisDownTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getLeftAxisRightTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getLeftAxisLeftTrigger();
+		}
+		return false;
+	}
+}