diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index a0385ee..6ba3ae9 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -24,6 +24,17 @@ public final class Constants {
 	    public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
         public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
         public static final int PIGEON_ID = 6;
+
+        /* PID Constants */
+        public static final int SLOT_IDX = 0;
+
+        public static final int PID_LOOP_IDX = 0;
+
+        public static final int TIMEOUT_MS = 30;
+
+        static final Gains kGains = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);    
+        
+        public static final double ENCODER_TICKS_PER_REV = 2048;
     }
 
     public static final class LEDConstants {
diff --git a/src/main/java/frc4388/robot/Gains.java b/src/main/java/frc4388/robot/Gains.java
new file mode 100644
index 0000000..14ede74
--- /dev/null
+++ b/src/main/java/frc4388/robot/Gains.java
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+/**
+ * Add your docs here.
+ */
+public class Gains {
+    public final double kP;
+    public final double kI;
+    public final double kD;
+    public final double kF;
+    public final int kIzone;
+    public final double kPeakOutput;
+
+    public Gains(double _kP, double _kI, double _kD, double _kF, int _kIzone, double _kPeakOutput)
+    {
+        kP = _kP;
+        kI = _kI;
+        kD = _kD;
+        kF = _kF;
+        kIzone = _kIzone;
+        kPeakOutput = _kPeakOutput;
+    }
+}
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index ad837e9..72d42fe 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -69,6 +69,9 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
             .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+
+        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
+            .whenPressed(() -> m_robotDrive.goToTargetPos());
     }
     
     /**
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index c8d4d1e..fe12acd 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/src/main/java/frc4388/robot/subsystems/Drive.java
@@ -7,6 +7,8 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
@@ -22,11 +24,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.utility.controller.XboxController;
 
 /**
  * Add your docs here.
  */
-public class Drive extends ProfiledPIDSubsystem {
+public class Drive extends SubsystemBase {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
@@ -42,9 +45,6 @@ public class Drive extends ProfiledPIDSubsystem {
    * Add your docs here.
    */
   public Drive() {
-    /*  */
-    super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0);
-
     /* factory default values */
     m_leftFrontMotor.configFactoryDefault();
     m_rightFrontMotor.configFactoryDefault();
@@ -65,6 +65,10 @@ public class Drive extends ProfiledPIDSubsystem {
     m_rightFrontMotor.setInverted(false);
     m_leftBackMotor.setInverted(InvertType.FollowMaster);
     m_rightBackMotor.setInverted(InvertType.FollowMaster);
+
+    /* Motor Encoders */
+    m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS);
+    m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.PID_LOOP_IDX, DriveConstants.TIMEOUT_MS);
   }
 
   @Override
@@ -121,15 +125,12 @@ public class Drive extends ProfiledPIDSubsystem {
     m_pigeon.setAccumZAngle(0);
   }
 
-  @Override
-  protected void useOutput(double output, TrapezoidProfile.State setpoint) {
-    // TODO Auto-generated method stub
-    
-  }
+  // Motion Magic Testing
+  public void goToTargetPos()
+  {
+    double targetPos = XboxController.RIGHT_Y_AXIS * DriveConstants.ENCODER_TICKS_PER_REV * 10.0;
 
-  @Override
-  protected double getMeasurement() {
-    // TODO Auto-generated method stub
-    return 0;
+    m_leftFrontMotor.set(ControlMode.MotionMagic, targetPos);
+    m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos);
   }
 }