Merge branch 'master' into add-storage

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

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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.cscore.MjpegServer;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+
+public class Camera extends SubsystemBase {
+  CameraServer camServ = CameraServer.getInstance();
+  /**
+   * Creates a new Camera.
+   * Makes a Camera and sends the stream to a CameraServer, to be viewed in Shuffle Board.
+   * @param name Name of the Camera in Shuffle Board.
+   * @param id USB Id of the Camera.
+   * @param width Resolution width.
+   * @param height Resolution height.
+   * @param brightness Percent brightness of the stream.
+   */
+  public Camera(String name, int id, int width, int height, int brightness) {
+    try{
+      UsbCamera cam = new UsbCamera(name, id);
+      cam.setResolution(width, height);
+      cam.setBrightness(brightness);
+      cam.setFPS(10);
+      VideoSource camera = cam;
+      camServ.startAutomaticCapture(camera);
+    } 
+    catch(Exception e){
+      System.err.println("Camera broken, pls nerf");
+    }
+
+  }
+
+  @Override
+  public void periodic() {
+  }
+}

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

@@ -10,6 +10,7 @@ package frc4388.robot.subsystems;
 import com.revrobotics.CANDigitalInput;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -17,18 +18,23 @@ import frc4388.robot.Constants.ClimberConstants;
 
 public class Climber extends SubsystemBase {
   CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless);
-  CANDigitalInput m_forwardLimit, m_reverseLimit;
+  CANDigitalInput m_climberForwardLimit, m_climberReverseLimit;
+
+  public boolean climberSafety = false;
+
   /**
    * Creates a new Climber.
    */
   public Climber() {
     m_climberMotor.restoreFactoryDefaults();
-    
-    m_forwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
-    m_reverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
 
-    m_forwardLimit.enableLimitSwitch(false);
-    m_reverseLimit.enableLimitSwitch(false);
+    m_climberMotor.setIdleMode(IdleMode.kBrake);
+    m_climberMotor.setInverted(false);
+    
+    m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_climberForwardLimit.enableLimitSwitch(false);
+    m_climberReverseLimit.enableLimitSwitch(false);
   }
 
   @Override
@@ -41,6 +47,23 @@ public class Climber extends SubsystemBase {
    * @param input the voltage to run motor at
    */
   public void runClimber(double input) {
-    m_climberMotor.set(input);
+    if(climberSafety){
+      m_climberMotor.set(input);
+    }
+    else{
+      m_climberMotor.set(0);
+    }
+  }
+
+  /* Safety Button for Climber */
+  public void setSafetyPressed()
+  {
+    climberSafety = true;
+  }
+
+  /* Safety Button for Climber set back to false */
+  public void setSafetyNotPressed()
+  {
+    climberSafety = false;
   }
 }

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

@@ -7,6 +7,16 @@
 
 package frc4388.robot.subsystems;
 
+import java.io.File;
+import java.io.FilenameFilter;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.stream.Collectors;
+import java.util.stream.Stream;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
@@ -18,15 +28,25 @@ import com.ctre.phoenix.motorcontrol.SensorTerm;
 import com.ctre.phoenix.motorcontrol.StatusFrame;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.music.Orchestra;
 import com.ctre.phoenix.sensors.PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
 
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Gains;
 
@@ -42,6 +62,11 @@ public class Drive extends SubsystemBase {
   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 Orchestra m_orchestra = new Orchestra();
+
+  public double m_rightFrontMotorPos;
+
+  public double m_rightFrontMotorVel;
 
   public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
 
@@ -49,9 +74,19 @@ public class Drive extends SubsystemBase {
   public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
   public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
   public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
-  public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
+  //public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
+  
+  public final DifferentialDriveOdometry m_odometry;
+  
+  public DoubleSolenoid m_speedShift;
+  public DoubleSolenoid m_coolFalcon;
 
-  public DoubleSolenoid speedShift;
+  SendableChooser<String> m_songChooser = new SendableChooser<String>();
+
+  public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
+  public long m_lastTime, m_deltaTime; //in milliseconds
+
+  public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
 
   /**
    * Add your docs here.
@@ -65,31 +100,40 @@ public class Drive extends SubsystemBase {
     m_pigeon.configFactoryDefault();
     resetGyroYaw();
 
-    speedShift = new DoubleSolenoid(7,0,1);
+    m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), 
+                                                new Pose2d(0, 0, new Rotation2d()) );
 
+    m_speedShift = new DoubleSolenoid(7,0,1);
+    m_coolFalcon = new DoubleSolenoid(7,3,2);
+
+    coolFalcon(false);
+    
     /* set back motors as followers */
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
 
+    /* flip input so forward becomes back, etc */
+    m_leftFrontMotor.setInverted(false);
+    m_rightFrontMotor.setInverted(true);
+    //m_driveTrain.setRightSideInverted(false);
+    m_leftBackMotor.setInverted(InvertType.FollowMaster);
+    m_rightBackMotor.setInverted(InvertType.FollowMaster);
+
     setDriveTrainNeutralMode(NeutralMode.Coast);
 
     /* deadbands */
     m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
-    m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); //Ensures motors run at the same speed
+    m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
+    //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
+    //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
 
-    /* flip input so forward becomes back, etc */
-    m_leftFrontMotor.setInverted(false);
-    m_rightFrontMotor.setInverted(true);
-    m_driveTrain.setRightSideInverted(false);
-    m_leftBackMotor.setInverted(InvertType.FollowMaster);
-    m_rightBackMotor.setInverted(InvertType.FollowMaster);
-
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    /* PID for Front Motor Control in Teleop */
+    //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+    //m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
     m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -97,47 +141,65 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    
+
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
     m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);    
+    m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
-    m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
+    //m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    /* PID for Back Motor control in Auto */
+    m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+    m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+    m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Setup Sensors for WPI_TalonFXs */
-    m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
+    resetEncoders();
 
     /* Configure the left Talon's selected sensor as local QuadEncoder */
-		m_leftFrontMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor,    // Local Feedback Source
-                                                    DriveConstants.PID_PRIMARY,				  // PID Index for Source [0, 1]
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);	  // Configuration Timeout
-                                                    
-    /*m_rightFrontMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor, // Local Feedback Source
-                                                    DriveConstants.PID_PRIMARY,				  // PID Index for Source [0, 1]
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);*/	// Configuration Timeout
+    m_leftFrontMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor, // Local Feedback Source
+                                                    DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
+                                                    DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
+
+    /* Configure the left back Talon's selected sensor as local QuadEncoder */
+    m_leftBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
+                                                  DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
+                                                  DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
+                                                  
+    /* Configure the right back Talon's selected sensor as local QuadEncoder */
+    m_rightBackMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor, // Local Feedback Source
+                                                    DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
+                                                    DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
 
     /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
-		m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(),					      // Device ID of Source
-                                                  RemoteSensorSource.TalonSRX_SelectedSensor,
-                                                  DriveConstants.REMOTE_0,							        // Source number [0, 1]
-                                                  DriveConstants.DRIVE_TIMEOUT_MS);				  // Configuration Timeout
+    m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
+                                                  RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
+                                                  DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
 
     /* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */
-		m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(),
-                                                  RemoteSensorSource.Pigeon_Yaw,
-                                                  DriveConstants.REMOTE_1,	
-                                                  DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
+                                                  DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Setup Sum signal to be used for Distance */
     m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
@@ -148,130 +210,170 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
-		m_rightFrontMotor.configSelectedFeedbackSensor(	FeedbackDevice.SensorDifference,
-                                                    DriveConstants.PID_PRIMARY,
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);
- 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
-	  m_rightFrontMotor.configSelectedFeedbackCoefficient(  1, 						                    // Coefficient
-                                                          DriveConstants.PID_PRIMARY,		      // PID Slot of Source 
-                                                          DriveConstants.DRIVE_TIMEOUT_MS);   // Configuration Timeout
+    configMotorSensor(FeedbackDevice.SensorDifference);
 
-    m_rightFrontMotor.configSelectedFeedbackSensor(	FeedbackDevice.RemoteSensor1, 
+    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
+    /*
+    m_rightFrontMotor.configSelectedFeedbackCoefficient(  1, // Coefficient
+                                                          DriveConstants.PID_PRIMARY, // PID Slot of Source
+                                                          DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
+    */
+
+    m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
                                                     DriveConstants.PID_TURN,
                                                     DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
-    m_rightFrontMotor.configSelectedFeedbackCoefficient(  1,
-                                                          DriveConstants.PID_TURN,
-                                                          DriveConstants.DRIVE_TIMEOUT_MS);
+    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
+    //m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
-	  m_leftFrontMotor.configSelectedFeedbackCoefficient( 1,
-                                                        DriveConstants.PID_PRIMARY,
-                                                        DriveConstants.DRIVE_TIMEOUT_MS);
-                                                        
-   /* Set status frame periods to ensure we don't have stale data */
-		m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
-		m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
-		m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
+    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
+    //m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    /* Set status frame periods to ensure we don't have stale data */
+    m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
     m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
     m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Smart Dashboard Initial Values */
-
-      /* Set up Chooser */
-    m_chooser.setDefaultOption("Distance PID", m_gainsDistance);
-    //setDriveTrainGains("Distance PID", m_gainsDistance);
-    m_chooser.addOption("Velocity PID", m_gainsVelocity);
-    //setDriveTrainGains("Velocity PID", m_gainsVelocity);
-    m_chooser.addOption("Turning PID", m_gainsTurning);
-    //setDriveTrainGains("Turning PID", m_gainsTurning);
-    m_chooser.addOption("Motion Magic PID", m_gainsMotionMagic);
-    //setDriveTrainGains("Motion Magic PID", m_gainsMotionMagic);
-    Shuffleboard.getTab("PID").add(m_chooser);
-
-      /* Gyro */
-    SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
-    SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
-    SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
-
-      /* Sensor Values */
-    SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
-    SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
-    SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
-    SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
-
-      /* PID */
-    Gains gains = m_chooser.getSelected();
-    Shuffleboard.getTab("PID").add("P Value Drive", gains.m_kP);
-    Shuffleboard.getTab("PID").add("I Value Drive", gains.m_kI);
-    Shuffleboard.getTab("PID").add("D Value Drive", gains.m_kD);
-    Shuffleboard.getTab("PID").add("F Value Drive", gains.m_kF);
-
-        
-   /**
-		 * Max out the peak output (for all modes).  
-		 * However you can limit the output of a given PID object with configClosedLoopPeakOutput().
-		 */
-		m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
-		m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
-		m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
-		m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /**
-		 * 1ms per loop.  PID loop can be slowed down if need be.
-		 * For example,
-		 * - if sensor updates are too slow
-		 * - sensor deltas are very small per update, so derivative error never gets large enough to be useful.
-		 * - sensor movement is very slow causing the derivative error to be near zero.
-		 */
+     * Max out the peak output (for all modes). However you can limit the output of
+     * a given PID object with configClosedLoopPeakOutput().
+     */
+    m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    /**
+     * 1ms per loop. PID loop can be slowed down if need be. For example, - if
+     * sensor updates are too slow - sensor deltas are very small per update, so
+     * derivative error never gets large enough to be useful. - sensor movement is
+     * very slow causing the derivative error to be near zero.
+     */
     int closedLoopTimeMs = 1;
-    m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
-    /**
-		 * configAuxPIDPolarity(boolean invert, int timeoutMs)
-		 * false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
-		 * true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
-		 */
-    m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
-  }
+    m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
+                                              closedLoopTimeMs, 
+                                              DriveConstants.DRIVE_TIMEOUT_MS);
 
+    m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN, 
+                                              closedLoopTimeMs,
+                                              DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
+                                              closedLoopTimeMs,
+                                              DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, 
+                                              closedLoopTimeMs,
+                                              DriveConstants.DRIVE_TIMEOUT_MS);
+    
+    /**
+     * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
+     * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
+     * local output is PID0 - PID1, and other side Talon is PID0 + PID1
+     */
+    m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_lastTime = System.currentTimeMillis();
+    
+    m_orchestra.addInstrument(m_leftBackMotor);
+    m_orchestra.addInstrument(m_rightFrontMotor);
+    m_orchestra.addInstrument(m_rightBackMotor);
+    m_orchestra.addInstrument(m_leftFrontMotor);
+
+    File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
+    System.err.println(songsDir.getPath());
+    String[] songsStrings = songsDir.list();
+    for (String songString : songsStrings){
+      m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
+    }
+    Shuffleboard.getTab("Songs").add(m_songChooser);
+  } 
+
+  String currentSong = "";
   @Override
   public void periodic() {
+    m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
+    SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis());
+
+    if (m_currentTimeSec % 30 == 0) {
+      coolFalcon(true);
+      SmartDashboard.putBoolean("Solenoid", true);
+    } else if ((m_currentTimeSec - 1) % 30 == 0) {
+      coolFalcon(false);
+      SmartDashboard.putBoolean("Solenoid", false);
+    }
+
+    m_deltaTime = System.currentTimeMillis() - m_lastTime;
+    m_lastTime = System.currentTimeMillis();
+    m_lastAngleYaw = m_currentAngleYaw;
+    m_currentAngleYaw = getGyroYaw();
+
+    m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
+    m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
+    
     try {
       SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
-      SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
-      SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
-
-      SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
-      SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
-      SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
-      SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
+      //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
+      //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
+      SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.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());
+      //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
+      //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
       SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
       SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
 
-      SmartDashboard.putNumber("Right Front Motor Current", m_rightFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
+      SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
 
-      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 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
-      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));
+      //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
+      //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
+      //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
+      //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
 
+      //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 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
+      //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));
+
+      SmartDashboard.putString("Odometry Values Meters", getPose().toString());
+      SmartDashboard.putNumber("Odometry Heading", getHeading());
+
+      SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
+      //SmartDashboard.putNumber("Delta Time", m_deltaTime);
+
+      if (currentSong != m_songChooser.getSelected()){
+        currentSong = m_songChooser.getSelected();
+        selectSong(currentSong);
+        System.err.println(currentSong);
+      }
     } catch (Exception e) {
       System.err.println("Error in the Drive Subsystem");
-      //e.printStackTrace(System.err);
+      // e.printStackTrace(System.err);
     }
+
+    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+                                              inchesToMeters(getDistanceInches(m_leftBackMotor)),
+                                              -inchesToMeters(getDistanceInches(m_rightBackMotor)));
   }
 
   /**
    * Sets Motors to a NeutralMode.
+   * 
    * @param mode NeutralMode to set motors to
    */
   public void setDriveTrainNeutralMode(NeutralMode mode) {
@@ -282,120 +384,142 @@ public class Drive extends SubsystemBase {
   }
 
   /**
-   * Initializes the drive train gains kP, kI, kD, and kF
-   * @param slot Either "Distance PID", "Velocity PID", "Motion Magic PID", or "Turning PID"
-   * @param gains A gains object which is the gains that are set for the slot
+   * Runs percent output control on the moving and steering of the drive train,
+   * using the Differential Drive class to manage the two inputs
    */
-  public void setDriveTrainGains(String slot, Gains gains){
-    /* Distance */
-    if (slot.equals("Distance PID")) {
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    }
-
-    /* Velocity */
-    if (slot.equals("Velocity PID")) {
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    }
-    /* Turning */
-    if (slot.equals("Turning PID")) {
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, gains.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    }
-
-    /* Motion Magic */
-    if (slot.equals("Motion Magic PID")) {
-      m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
-      m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, gains.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
-    
-      m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-      m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
-    }
+  public void driveWithInput(double move, double steer) {
+    m_driveTrain.arcadeDrive(move, steer);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
   }
 
   /**
-   * Add your docs here.
+   * Runs percent output control on the drive train while using an AUX PID for rotation
+   * @param targetPos The position to drive to in units
+   * @param targetGyro The angle to drive at in units
    */
-  public void driveWithInput(double move, double steer){
-    m_driveTrain.arcadeDrive(move, steer);
-  }
-
   public void driveWithInputAux(double move, double targetGyro) {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
 
     m_rightFrontMotor.set(TalonFXControlMode.PercentOutput, move, DemandType.AuxPID, targetGyro);
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
 
     m_driveTrain.feedWatchdog();
   }
 
   /**
-   * Runs a position PID while driving straight
-   * @param targetPos The position to drive to in units
+   * Runs position PID.
+   * Position is absolute and displacement should be handled on the command side.
+   * @param targetPos  The position to drive to in units
    * @param targetGyro The angle to drive at in units
    */
-  public void runDriveStraightPositionPID(double targetPos, double targetGyro) {
+  public void runDrivePositionPID(double targetPos, double targetGyro) {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
 
     m_rightFrontMotor.set(TalonFXControlMode.Position, targetPos, DemandType.AuxPID, targetGyro);
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
 
-    m_driveTrain.feedWatchdog();
+    //m_driveTrain.feedWatchdog();
   }
 
   /**
-   * Runs velocity PID while driving straight
-   * @param targetVel The velocity to drive at in units
+   * Runs velocity PID
+   * 
+   * @param targetVel  The velocity to drive at in units
    * @param targetGyro The angle to drive at in units
    */
-  public void runDriveStraightVelocityPID(double targetVel, double targetGyro) {
+  public void runDriveVelocityPID(double targetVel, double targetGyro) {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
+
     m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
 
-    m_driveTrain.feedWatchdog();
+    //m_driveTrain.feedWatchdog();
   }
 
   /**
-   * Runs motion magic PID while driving straight (has not been tested)
+   * Runs motion magic PID while driving straight
    * @param targetPos The position to drive to in units
    * @param targetGyro The angle to drive at in units
    */
-  public void runMotionMagicPID(double targetPos, double targetGyro){
+  public void runMotionMagicPID(double targetPos, double targetGyro) {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
 
     m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
+
+    m_driveTrain.feedWatchdog();
+
+  }
+
+  /**
+   * Runs a Turning PID to rotate a to a target angle
+   * 
+   * @param targetAngle target angle in degrees
+   */
+  public void runTurningPID(double targetAngle) {
+    double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+
+    runDriveVelocityPID(0, targetGyro);
+  }
+
+  /**
+   * Controls the left and right sides of the drive with velocity targets.
+   *
+   * @param leftSpeed  the commanded left speed
+   * @param rightSpeed the commanded right speed
+   */
+  public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
+    //DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
+    //ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
+    //double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
+    //double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
+    //double angleVelDeg = Math.toDegrees(angleVelRad);
+
+    //m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
+    //m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle, 
+    //                                          m_currentAngleYaw-(360),
+    //                                          m_currentAngleYaw+(360));
+    //double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    double moveVelLeft = inchesToTicks(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
+    double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
+
+    //SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
+    //SmartDashboard.putNumber("Move Vel Right", moveVelRight);
+
+    //runDriveVelocityPID(moveVel*2, targetGyro);
+
+    m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+    m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+
+    System.err.println(moveVelLeft);
+
+    m_rightBackMotor.set(TalonFXControlMode.Velocity, moveVelRight);
+    m_leftBackMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
+    m_leftFrontMotor.follow(m_leftBackMotor);
+    m_rightFrontMotor.follow(m_rightBackMotor);
 
     m_driveTrain.feedWatchdog();
   }
 
   /**
-   * Runs a Turning PID to rotate a to a target angle
-   * @param targetAngle target angle in degrees
+   * Selects the feedback device for the motors.
+   * @param feedbackDevice The feedback device to set it to, usually SensorDifference or
    */
-  public void runTurningPID(double targetAngle){
-    double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
-    
-    runDriveStraightVelocityPID(0, targetGyro);
+  public void configMotorSensor(FeedbackDevice type) {
+    m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
+                                                    DriveConstants.DRIVE_TIMEOUT_MS);
   }
 
   /**
@@ -404,17 +528,17 @@ public class Drive extends SubsystemBase {
   public double getGyroYaw() {
 
     double[] ypr = new double[3];
-    
+
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[0];
-  }
+  } 
 
   /**
    * Returns the current pitch of the pigeon
    */
   public double getGyroPitch() {
     double[] ypr = new double[3];
-    
+
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[1];
   }
@@ -424,7 +548,7 @@ public class Drive extends SubsystemBase {
    */
   public double getGyroRoll() {
     double[] ypr = new double[3];
-    
+
     m_pigeon.getYawPitchRoll(ypr);
     return ypr[2];
   }
@@ -435,18 +559,170 @@ public class Drive extends SubsystemBase {
   public void resetGyroYaw() {
     m_pigeon.setYaw(0);
     m_pigeon.setAccumZAngle(0);
+    resetGyroAngles();
   }
 
+  /**
+   * Add docs here
+   */
+  public void resetGyroAngles() {
+    m_lastAngleYaw = 0;
+    m_currentAngleYaw = 0;
+    m_kinematicsTargetAngle = 0;
+  }
+//lol
+//sko
+//ridge
+  /**
+//brayden=bad coder
+  * Returns the heading of the robot
+   * @return The robot's heading in degrees, from -180 to 180
+   */
+  public double getHeading() {
+    return Math.IEEEremainder(getGyroYaw(), 360);
+  }
+
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  public double getTurnRate() {
+    double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
+    double turnRate = 1000 * deltaYaw / m_deltaTime;
+    return turnRate;
+  }
+
+  /**
+   * Returns the currently-estimated pose of the robot.
+   * @return The pose.
+   */
+  public Pose2d getPose() {
+    return m_odometry.getPoseMeters();
+  }
+
+  /**
+   * Returns current wheel speeds of robot.
+   * @return The current wheel speeds.
+   */
+  public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+    return new DifferentialDriveWheelSpeeds(  inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)), 
+                                              -inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
+  }
+
+  /**
+   * Resets the encoders for both motors.
+   */
+  public void resetEncoders() {
+    m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
+  }
+
+  /**
+   * Resets the odometry to the specified pose.
+   * 
+   * @param pose The pose to which to set the odometry.
+   */
+  public void setOdometry(Pose2d pose) {
+    resetEncoders();
+    m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
+  }
+
+  /**
+   * Gets the encoder value (position) of a motor
+   * @param falcon The motor to get the position of
+   * @return The position of the motor in inches
+   */
+  public double getDistanceInches(WPI_TalonFX falcon) {
+    return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition());
+  }
+
+  /**
+   * Gets the encoder value (velocity) of a motor
+   * @param falcon The motor to get the velocity of
+   * @return The velocity of the motor in inches per second
+   */
+  public double getVelocityInchesPerSecond(WPI_TalonFX falcon) {
+    return ticksToInches(falcon.getSensorCollection().getIntegratedSensorPosition()/DriveConstants.TICK_TIME_TO_SECONDS);
+  }
+
+  /**
+   * Converts a value in ticks to inches.
+   * @param ticks The value in ticks to convert
+   * @return The converted value in inches
+   */
+  public double  ticksToInches(double ticks) {
+    return ticks * DriveConstants.INCHES_PER_TICK;
+  }
+
+  /**
+   * Converts a value in inches to ticks.
+   * @param inches The value in inches to convert
+   * @return The converted value in ticks
+   */
+  public double inchesToTicks(double inches) {
+    return inches * DriveConstants.TICKS_PER_INCH;
+  }
+
+  /**
+   * Converts a value in inches to meters.
+   * @param inches The value in inches to convert
+   * @return The converted value in meters
+   */
+  public double inchesToMeters(double inches) {
+    return inches * DriveConstants.METERS_PER_INCH;
+  }
+
+  /**
+   * Converts a value in meters to inches.
+   * @param meters The value in meters to convert
+   * @return The converted value in inches
+   */
+  public double metersToInches(double meters) {
+    return meters * DriveConstants.INCHES_PER_METER;
+  }
+  
+  /*
+   * Plays Music!
+   */
+  public void playSong() {
+    m_orchestra.play();
+  }
+
+  /**
+   * Selects a song to play!
+   * @param song The name of the song to be played
+   */
+  public void selectSong(String song) {
+    SmartDashboard.putString("Selected Song", song);
+    m_orchestra.loadMusic(song);
+  }
   /**
    * 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) {
-			speedShift.set(DoubleSolenoid.Value.kForward);
+			m_speedShift.set(DoubleSolenoid.Value.kForward);
 		}
 		if (state == false) {
-			speedShift.set(DoubleSolenoid.Value.kReverse);
+			m_speedShift.set(DoubleSolenoid.Value.kReverse);
 		}
   }
+
+  /**
+   * 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);
+    }
+  }
+
 }

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

@@ -17,16 +17,20 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.Talon;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.LevelerConstants;
+import frc4388.robot.subsystems.*;
 
 public class Leveler extends SubsystemBase {
   CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless);
 
+  private final Climber m_robotClimber = new Climber();
+
   /**
    * Creates a new Leveler.
    */
   public Leveler() {
     m_levelerMotor.restoreFactoryDefaults();
-    m_levelerMotor.setIdleMode(IdleMode.kCoast);
+
+    m_levelerMotor.setIdleMode(IdleMode.kBrake);
     m_levelerMotor.setInverted(false);
   }
 
@@ -40,6 +44,11 @@ public class Leveler extends SubsystemBase {
    * @param input the percent output to run motor at
    */
   public void runLeveler(double input) {
-    m_levelerMotor.set(input);
+    if(m_robotClimber.climberSafety){
+      m_levelerMotor.set(input);
+    }
+    else{
+      m_levelerMotor.set(0);
+    }
   }
 }

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

@@ -11,28 +11,56 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Gains;
-import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Constants.ShooterConstants;
+import frc4388.utility.controller.IHandController;
 
 public class Shooter extends SubsystemBase {
 
   public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
+  public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless);
 
-  public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
+
+  public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS;
+  public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
+  public static Shooter m_shooter;
+  public static IHandController m_controller;
+
+
+  // Configure PID Controllers
+  CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController();
+  CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController();
+
+  CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder();
+  CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder();
 
   double velP;
-  /**
+  double input;
+  
+  /*
    * Creates a new Shooter subsystem.
    */
   public Shooter() {
+    //Testing purposes reseting gyros
+    resetGyroAngleAdj();
+    resetGyroShooterRotate();
+
     m_shooterFalcon.configFactoryDefault();
-
+    m_shooterRotateMotor.setIdleMode(IdleMode.kBrake);
     m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
-
-    m_shooterFalcon.setInverted(true);
+    m_shooterFalcon.setInverted(false);
     
     setShooterGains();
 
@@ -64,10 +92,10 @@ public class Shooter extends SubsystemBase {
    */
   public void setShooterGains() {
     m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_shooterTurretGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
   /**
    * Runs drum shooter velocity PID.
@@ -87,4 +115,55 @@ public class Shooter extends SubsystemBase {
       m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
     }
   }
+
+
+  public void runShooterWithInput(double input) {
+    m_shooterRotateMotor.set(input);
+  }
+
+  /* Angle Adjustment PID Control */
+  public void runAngleAdjustPID(double targetAngle)
+  {
+    // Set PID Coefficients
+    m_angleAdjustPIDController.setP(m_shooterTurretGains.m_kP);
+    m_angleAdjustPIDController.setI(m_shooterTurretGains.m_kI);
+    m_angleAdjustPIDController.setD(m_shooterTurretGains.m_kD);
+    m_angleAdjustPIDController.setIZone(m_shooterTurretGains.m_kIzone);
+    m_angleAdjustPIDController.setFF(m_shooterTurretGains.m_kF);
+    m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
+
+    // Convert input angle in degrees to rotations of the motor
+    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+
+    m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+  /* Rotate Shooter PID Control */
+  public void runshooterRotatePID(double targetAngle)
+  {
+    // Set PID Coefficients
+    m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP);
+    m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI);
+    m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD);
+    m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF);
+    m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone);
+    m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); 
+
+    // Convert input angle in degrees to rotations of the motor
+    targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT;
+
+    m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition);
+  }
+
+  /* For Testing Purposes, reseting gyro for angle adjuster */
+  public void resetGyroAngleAdj()
+  {
+    m_angleEncoder.setPosition(0);
+  }
+
+  /* For Testing Purposes, reseting gyro for shooter rotation */
+  public void resetGyroShooterRotate()
+  {
+    m_shooterRotateEncoder.setPosition(0);
+  }
 }

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

@@ -17,7 +17,6 @@ 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.wpilibj2.command.SubsystemBase;
@@ -61,6 +60,10 @@ public class Storage extends SubsystemBase {
    * @param input the voltage to run motor at
    */
   
+  public void runStorage(final double input) {
+    m_storageMotor.set(input);
+    final boolean beam_on = m_beamSensors[0].get();
+  }
 
   public void resetEncoder()
   {