Merge branch 'master' into button

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

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
 import frc4388.utility.LEDPatterns;
 
 /**
@@ -28,12 +29,18 @@ public final class Constants {
 
         /* PID Constants Drive*/
         public static final int DRIVE_TIMEOUT_MS = 30;
-        public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.3);
+        public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3);
-        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.0, 0.1, 0, 1.0);
+        public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05);
-        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.4, 0.0, 0.0, 0.0, 0, 0.5);
+        public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.5);
-        public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
+        //public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0);
-        public static final int DRIVE_CRUISE_VELOCITY = 20000;
+        //public static final int DRIVE_CRUISE_VELOCITY = 20000;
-        public static final int DRIVE_ACCELERATION = 7000;
+        //public static final int DRIVE_ACCELERATION = 7000;
+
+        /* Trajectory Constants */
+        public static final double MAX_SPEED_METERS_PER_SECOND = 3;
+        public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3;
+        public static final double TRACK_WIDTH_METERS = 0.648;
+        public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
  
         /* Remote Sensors */
         public final static int REMOTE_0 = 0;
@@ -50,19 +57,21 @@ public final class Constants {
 	    public final static int SLOT_MOTION_MAGIC = 3;
         
         /* Drive Train Characteristics */
-        public static final double TICKS_PER_MOTOR_REV = 2048*2;
+        public static final double TICKS_PER_MOTOR_REV = 2048;
-        public static final double MOTOR_TO_WHEEL_GEAR_RATIO = 12.5;
+        public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13;
         public static final double WHEEL_DIAMETER_INCHES = 6;
         public static final double TICKS_PER_GYRO_REV = 8192;
         
         /* Ratio Calculation */
         public static final double TICK_TIME_TO_SECONDS = 0.1;
         public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
-        public static final double WHEEL_TO_MOTOR_GEAR_RATIO = 1/MOTOR_TO_WHEEL_GEAR_RATIO;
+        public static final double WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT;
-        public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_TO_WHEEL_GEAR_RATIO;
+        public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT;
         public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
         public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
         public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
+        public static final double INCHES_PER_METER = 39.370;
+        public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
     }
     
     public static final class IntakeConstants {

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

@@ -7,9 +7,11 @@
 
 package frc4388.robot;
 
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -34,6 +36,7 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    SmartDashboard.putString("Auto?", "NAH");
   }
 
   /**
@@ -61,6 +64,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
+    //m_robotContainer.setDriveGearState(true);
   }
 
   @Override
@@ -75,6 +79,10 @@ public class Robot extends TimedRobot {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
+    m_robotContainer.setDriveGearState(true);
+    m_robotContainer.resetOdometry();
+    //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
+
     /*
      * String autoSelected = SmartDashboard.getString("Auto Selector",
      * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
@@ -85,6 +93,7 @@ public class Robot extends TimedRobot {
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
+      System.err.println("Auto Start");
     }
   }
 
@@ -98,6 +107,9 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
+    m_robotContainer.setDriveGearState(true);
+    //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
+    
     // 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
@@ -105,6 +117,8 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+
+    SmartDashboard.putString("Auto?", "NAH");
   }
 
   /**

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

@@ -7,19 +7,34 @@
 
 package frc4388.robot;
 
+import java.util.List;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.controller.RamseteController;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
+import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
+import frc4388.robot.commands.DriveWithJoystickDriveStraight;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
 import frc4388.robot.commands.RunIntakeWithTriggers;
@@ -74,7 +89,7 @@ public class RobotContainer {
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
+        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
         // drives intake with input from triggers on the opperator controller
         m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
         // drives climber with input from triggers on the opperator controller
@@ -98,7 +113,7 @@ public class RobotContainer {
     private void configureButtonBindings() {
         /* Driver Buttons */
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
+            .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
 
         /* Operator Buttons */
         // activates "Lit Mode"
@@ -130,11 +145,11 @@ public class RobotContainer {
       
         // resets the yaw of the pigeon
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
+            .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
       
         // turn 45 degrees
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
+            .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
 
 
         // sets solenoids into high gear
@@ -167,13 +182,59 @@ public class RobotContainer {
         m_robotDrive.setDriveTrainNeutralMode(mode);
     }
 
+    /**
+     * Sets the gear of the drivetrain
+     * @param state the gearing of the gearbox (true is high, false is low)
+     */
+    public void setDriveGearState(boolean state) {
+        m_robotDrive.setShiftState(state);
+    }
+
+    public void configDriveTrainSensors(FeedbackDevice type) {
+        m_robotDrive.configMotorSensor(type);
+    } 
+
+    public void resetOdometry() {
+        m_robotDrive.resetGyroAngles();
+        m_robotDrive.setOdometry(new Pose2d());
+    }
+
     /**
      * 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
+
+        // Create config for trajectory
+        /*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND,
+                                                        DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
+                                                        // Add kinematics to ensure max speed is actually obeyed
+                                                        .setKinematics(DriveConstants.kDriveKinematics);
+        
+        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+            // Start at the origin facing the +X direction
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(
+                new Translation2d(10, 0)
+            ),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(20, 20, new Rotation2d(0)),
+            // Pass config
+            config);
+            // 10 = 20, 20 = 35, 30 = 53.5
+            // (0,10) = (8,22)
+        RamseteCommand ramseteCommand = new RamseteCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose,
+            new RamseteController(), 
+            DriveConstants.kDriveKinematics,
+            m_robotDrive::tankDriveVelocity,
+            m_robotDrive);
+
+        // Run path following command, then stop at the end.
+        return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
         return new InstantCommand();
     }
 

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

@@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   @Override
   public void execute() {
     //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
-    m_drive.runDriveStraightVelocityPID(-m_targetVel, m_targetGyro);
+    m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -47,7 +47,7 @@ public class DriveStraightToPositionPID extends CommandBase {
     //System.err.println("| \n Sensor Pos \n" + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
     //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
     //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
-    m_drive.runDriveStraightPositionPID(m_targetPosOut, m_targetGyro);
+    m_drive.runDrivePositionPID(m_targetPosOut, m_targetGyro);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -38,7 +38,7 @@ public class DriveWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double moveInput = m_controller.getLeftYAxis();
+    double moveInput = -m_controller.getLeftYAxis();
     double steerInput = m_controller.getRightXAxis();
     double moveOutput = 0;
     double steerOutput = 0;

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

@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+import frc4388.utility.controller.IHandController;
+
+public class DriveWithJoystickDriveStraight extends CommandBase {
+  Drive m_drive;
+  double m_targetGyro, m_currentGyro;
+  double m_stopPos;
+  long m_currTime, m_deltaTime;
+  long m_deadTimeSteer, m_deadTimeMove;
+  long m_deadTimeout = 100;
+  IHandController m_controller;
+
+  /**
+   * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
+   * Applies a curved ramp to the input from the controllers to make the robot less "touchy".
+   * Also uses PIDs to keep the robot on course when given a "dead" or 0 input.
+   * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the
+   * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in
+   * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
+   */
+  public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_controller = controller;
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currTime = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1);
+    double moveInput = -m_controller.getLeftYAxis();
+    double steerInput = m_controller.getRightXAxis();
+    double moveOutput = 0;
+    m_deltaTime = System.currentTimeMillis() - m_currTime;
+    m_currTime = System.currentTimeMillis();
+
+    /* If steer stick is being used */
+    if (steerInput != 0) {
+      m_deadTimeSteer = m_currTime;
+    }
+
+    /* Curves the moveInput to be slightly more gradual at first */
+    if (moveInput >= 0) {
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
+
+    /* If steer stick has not been used for less than 1 sec */
+    if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
+      runDriveWithInput(moveOutput, steerInput);
+      resetGyroTarget();
+    }
+    /* If steer stick has not been used for 1 sec */
+    else {
+      runDriveStraight(moveOutput);
+    }
+  }
+
+  private void runDriveWithInput(double move, double steer) {
+    double cosMultiplier = .45;
+    double steerOutput = 0;
+    double deadzone = .2;
+    /* Curves the steer output to be similarily gradual */
+    if (steer > 0){
+      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
+    } else {
+      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    }
+    m_drive.driveWithInput(move, steerOutput);
+    System.out.println("Driving With Input");
+  }
+
+  private void runDriveStraight(double move) {
+    m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
+    System.out.println("Driving Straight with Target: " + m_targetGyro);
+  }
+
+  /**
+   * set target angle to current angle (prevents buildup of gyro error).
+   */
+  private void resetGyroTarget() {
+    //m_targetGyro = m_currentGyro;
+    m_targetGyro =  m_currentGyro
+                    + m_drive.getTurnRate();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -15,10 +15,12 @@ import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   Drive m_drive;
-  double m_targetGyro;
+  double m_targetGyro, m_currentGyro;
-  long lastTime;
+  double m_stopPos;
+  long m_currTime, m_deltaTime;
+  long m_deadTimeSteer, m_deadTimeMove;
+  long m_deadTimeout = 100;
   IHandController m_controller;
-  boolean isAuxPIDEnabled = false;
 
   /**
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
@@ -39,35 +41,32 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    lastTime = System.currentTimeMillis();
+    m_currTime = System.currentTimeMillis();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
-    double moveInput = m_controller.getLeftYAxis();
+    double moveInput = -m_controller.getLeftYAxis();
     double steerInput = m_controller.getRightXAxis();
     double moveOutput = 0;
-    double steerOutput = 0;
+    m_deltaTime = System.currentTimeMillis() - m_currTime;
-    long deltaTime = System.currentTimeMillis() - lastTime;
+    m_currTime = System.currentTimeMillis();
-    lastTime = System.currentTimeMillis();
-
-    /* If AuxPID is enabled, then update using the steer input */
-    if (isAuxPIDEnabled) {
-      m_targetGyro += 2 * steerInput * deltaTime;
-  
-      m_targetGyro = MathUtil.clamp(  m_targetGyro, 
-                                      currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
-                                      currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
-    }
-    /* Otherwise set target angle to current angle (prevents buildup of gyro error) */
-    else {
-      m_targetGyro = currentGyro;
-    }
 
     /* If move stick is being used */
     if (moveInput != 0) {
+      m_deadTimeMove = m_currTime;
+      m_stopPos = m_drive.m_rightFrontMotor.getSelectedSensorPosition()
+                  + (m_drive.m_rightFrontMotor.getSelectedSensorVelocity());
+    }
+    /* If steer stick is being used */
+    if (steerInput != 0) {
+      m_deadTimeSteer = m_currTime;
+    }
+
+    /* If move stick has been pressed within 1 sec */
+    if (m_currTime - m_deadTimeMove < m_deadTimeout) {
       /* Curves the moveInput to be slightly more gradual at first */
       if (moveInput >= 0) {
         moveOutput = -Math.cos(1.571*moveInput)+1;
@@ -75,32 +74,66 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
         moveOutput = Math.cos(1.571*moveInput)-1;
       }
 
-      /* If steer stick is being used. */
+      /* If steer stick has not been used for less than 1 sec */
-      if (steerInput != 0) {
+      if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
-        double cosMultiplier = .45;
+        runDriveWithInput(moveOutput, steerInput);
-        double deadzone = .2;
+        resetGyroTarget();
-        /* Curves the steer output to be similarily gradual */
-        if (steerInput > 0){
-          steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
-        } else {
-          steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
-        }
-        m_drive.driveWithInput(moveOutput, steerOutput);
-        isAuxPIDEnabled = false;
       }
-      /* If only the move stick is being used */
+      /* If steer stick has not been used for 1 sec */
       else {
-        m_drive.driveWithInputAux(moveOutput, m_targetGyro);
+        runDriveStraight(moveOutput);
-        isAuxPIDEnabled = true;
       }
     }
-    /* If the move stick is not being used */
+    /* If the move stick has not been used for 1 sec */
     else {
-      m_drive.runDriveStraightVelocityPID(0, m_targetGyro);
+      runStoppedTurn(steerInput);
-      isAuxPIDEnabled = true;
     }
   }
 
+  private void runDriveWithInput(double move, double steer) {
+    double cosMultiplier = .45;
+    double steerOutput = 0;
+    double deadzone = .2;
+    /* Curves the steer output to be similarily gradual */
+    if (steer > 0){
+      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
+    } else {
+      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    }
+    m_drive.driveWithInput(move, steerOutput);
+    System.out.println("Driving With Input");
+  }
+
+  private void runDriveStraight(double move) {
+    m_drive.driveWithInputAux(move * 3/4, m_targetGyro);
+    System.out.println("Driving Straight with Target: " + m_targetGyro);
+  }
+
+  private void runStoppedTurn(double steer) {
+    updateGyroTarget(steer);
+    m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    System.out.println("Turning with Target: " + m_targetGyro);
+  }
+
+  /**
+   * If AuxPID is enabled, then update using the steer input
+   */
+  private void updateGyroTarget(double steerInput) {
+    m_targetGyro -= 5 * steerInput * m_deltaTime;
+    m_targetGyro = MathUtil.clamp(  m_targetGyro,
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
+  }
+
+  /**
+   * set target angle to current angle (prevents buildup of gyro error).
+   */
+  private void resetGyroTarget() {
+    m_targetGyro = m_currentGyro;
+    m_targetGyro =  m_currentGyro
+                    + m_drive.getTurnRate();
+  }
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {

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

@@ -36,11 +36,17 @@ 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;
 
@@ -58,17 +64,29 @@ public class Drive extends SubsystemBase {
   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);
 
   SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
   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;
 
   SendableChooser<String> m_songChooser = new SendableChooser<String>();
 
-  public DoubleSolenoid speedShift;
+  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.
@@ -82,18 +100,18 @@ 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);
 
-    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
-
     /* flip input so forward becomes back, etc */
     m_leftFrontMotor.setInverted(false);
     m_rightFrontMotor.setInverted(true);
@@ -101,12 +119,21 @@ public class Drive extends SubsystemBase {
     m_leftBackMotor.setInverted(InvertType.FollowMaster);
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
 
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
+    setDriveTrainNeutralMode(NeutralMode.Coast);
-    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);
+    /* deadbands */
-    m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
-    m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
+    m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
-    m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
+    //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
+
+    /* 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);
@@ -120,41 +147,59 @@ public class Drive extends SubsystemBase {
     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.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_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_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_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.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.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
+    //m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionSCurveStrength(0, 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);
+    resetEncoders();
-    m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
 
     /* Configure the left Talon's selected sensor as local QuadEncoder */
-		m_leftFrontMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor,    // Local Feedback Source
+    m_leftFrontMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor, // Local Feedback Source
-                                                    DriveConstants.PID_PRIMARY,				  // PID Index for Source [0, 1]
+                                                    DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1]
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);	  // Configuration Timeout
+                                                    DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
 
-    /*m_rightFrontMotor.configSelectedFeedbackSensor(  FeedbackDevice.IntegratedSensor, // Local Feedback Source
+    /* Configure the left back Talon's selected sensor as local QuadEncoder */
-                                                    DriveConstants.PID_PRIMARY,				  // PID Index for Source [0, 1]
+    m_leftBackMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);*/	// Configuration Timeout
+                                                  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
+    m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
-                                                  RemoteSensorSource.TalonSRX_SelectedSensor,
+                                                  RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
-                                                  DriveConstants.REMOTE_0,							        // Source number [0, 1]
+                                                  DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
-                                                  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(),
+    m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
-                                                  RemoteSensorSource.Pigeon_Yaw,
+                                                  DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
-                                                  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);
@@ -165,94 +210,81 @@ 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,
+    configMotorSensor(FeedbackDevice.SensorDifference);
-                                                    DriveConstants.PID_PRIMARY,
-                                                    DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
+    /* 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 
+    m_rightFrontMotor.configSelectedFeedbackCoefficient(  1, // Coefficient
-                                                          DriveConstants.DRIVE_TIMEOUT_MS);   // Configuration Timeout
+                                                          DriveConstants.PID_PRIMARY, // PID Slot of Source
+                                                          DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
+    */
 
-    m_rightFrontMotor.configSelectedFeedbackSensor(	FeedbackDevice.RemoteSensor1, 
+    m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
                                                     DriveConstants.PID_TURN,
                                                     DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
+    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
-    m_rightFrontMotor.configSelectedFeedbackCoefficient(  1,
+    //m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
-                                                          DriveConstants.PID_TURN,
-                                                          DriveConstants.DRIVE_TIMEOUT_MS);
 
-    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */
+    /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
-	  m_leftFrontMotor.configSelectedFeedbackCoefficient( 1,
+    //m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
-                                                        DriveConstants.PID_PRIMARY,
-                                                        DriveConstants.DRIVE_TIMEOUT_MS);
 
-   /* Set status frame periods to ensure we don't have stale data */
+    /* 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_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_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
-		m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 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 */
+    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);
-      /* 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);
 
     /**
-		 * 1ms per loop.  PID loop can be slowed down if need be.
+     * Max out the peak output (for all modes). However you can limit the output of
-		 * For example,
+     * a given PID object with configClosedLoopPeakOutput().
-		 * - if sensor updates are too slow
+     */
-		 * - sensor deltas are very small per update, so derivative error never gets large enough to be useful.
+    m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS);
-		 * - sensor movement is very slow causing the derivative error to be near zero.
+    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_PRIMARY, 
-    m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
+                                              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)
+     * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
-		 * false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
+     * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
-		 * true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
+     * 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);
@@ -270,30 +302,59 @@ public class Drive extends SubsystemBase {
   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 Pitch", getGyroPitch());
-      SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
+      //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
-
+      SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
-      SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
+      SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
-      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("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("Right Motor Temp", m_rightFrontMotor.getTemperature());
-      SmartDashboard.putNumber("Left Front Motor Current", m_leftFrontMotor.getSupplyCurrent());
+      SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
-      SmartDashboard.putNumber("Right Back Motor Current", m_rightFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("Left Back Motor Current", m_leftFrontMotor.getSupplyCurrent());
 
-      SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
+      //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
+      //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
+      //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
-      SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
+      //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
-      SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
+
-      SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
+      //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();
@@ -302,12 +363,17 @@ public class Drive extends SubsystemBase {
       }
     } 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) {
@@ -317,59 +383,14 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.setNeutralMode(mode);
   }
 
-  /**
-   * 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
-   */
-  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);
-    }
-  }
-
   /**
    * 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 driveWithInput(double move, double steer){
+  public void driveWithInput(double move, double steer) {
-    //m_driveTrain.arcadeDrive(move, steer);
+    m_driveTrain.arcadeDrive(move, steer);
+    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightBackMotor.follow(m_rightFrontMotor);
   }
 
   /**
@@ -382,35 +403,44 @@ public class Drive extends SubsystemBase {
 
     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
+   * Runs position PID.
-   * @param targetPos The position to drive to in units
+   * 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();
   }
 
   /**
-   * Runs velocity PID while driving straight
+   * Runs velocity PID
-   * @param targetVel The velocity to drive at in units
+   * 
+   * @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();
   }
@@ -420,12 +450,14 @@ public class Drive extends SubsystemBase {
    * @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();
 
@@ -433,12 +465,61 @@ public class Drive extends SubsystemBase {
 
   /**
    * Runs a Turning PID to rotate a to a target angle
+   * 
    * @param targetAngle target angle in degrees
    */
-  public void runTurningPID(double targetAngle){
+  public void runTurningPID(double targetAngle) {
-    double targetGyro = (targetAngle/360)*DriveConstants.TICKS_PER_GYRO_REV;
+    double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
 
-    runDriveStraightVelocityPID(0, targetGyro);
+    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();
+  }
+
+  /**
+   * Selects the feedback device for the motors.
+   * @param feedbackDevice The feedback device to set it to, usually SensorDifference or
+   */
+  public void configMotorSensor(FeedbackDevice type) {
+    m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
+                                                    DriveConstants.DRIVE_TIMEOUT_MS);
   }
 
   /**
@@ -477,9 +558,132 @@ 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() {
@@ -500,10 +704,24 @@ public class Drive extends SubsystemBase {
    */
   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/Storage.java

@@ -58,7 +58,6 @@ public class Storage extends SubsystemBase {
   public void runStorage(final double input) {
     m_storageMotor.set(input);
     final boolean beam_on = m_beamSensors[0].get();
-
   }
 
   public void resetEncoder()