diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java
index 5b4d80f..282c2d4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java
@@ -74,7 +74,7 @@ public void init(HardwareMap ahwMap){
         servos[2] = ahwMap.get(Servo.class, "intake_right_bottom");
         servos[3] = ahwMap.get(Servo.class, "intake_left_bottom");
 
-        lift=new GlyphIntake2(servos);
+        lift=new GlyphIntake2(ahwMap);
         sensorColor = hwMap.get(ColorSensor.class, "colorSensor");
         limitSwitch = hwMap.get(DigitalChannel.class, "limit_switch_1");
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java
index 2af92c0..1951d27 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java
@@ -1,10 +1,10 @@
 package org.firstinspires.ftc.teamcode;
 
+import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DigitalChannel;
-import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.teamcode.Components.DriveTrain;
 import org.firstinspires.ftc.teamcode.Components.GlyphIntake2;
@@ -15,54 +15,56 @@
  * */
 @TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL")
 public class CompTeleop extends OpMode {
-    public DriveTrain driveTrain;
-    DcMotor frontRight, backRight, frontLeft, backLeft;
-    Servo[] servos;
-
+    DriveTrain driveTrain;
     Gyro gyro;
 
-
     GlyphIntake2 intake;
     DcMotor liftA, liftB;
     DigitalChannel limitSwitch;
 
     @Override
     public void init() {
-        gyro = new Gyro(hardwareMap);
-        this.driveTrain = new DriveTrain(hardwareMap, gyro);
+        BNO055IMU imu = hardwareMap.get(BNO055IMU.class, Name.SENSOR_GYRO);
+        this.gyro = new Gyro(imu);
 
-        liftA = hardwareMap.get(DcMotor.class,"lift_left");
-        liftB = hardwareMap.get(DcMotor.class,"lift_right");
+        this.driveTrain = new DriveTrain(hardwareMap);
 
-        servos = new Servo[4];
-        servos[0] = hardwareMap.get(Servo.class, "intake_right_top");
-        servos[1] = hardwareMap.get(Servo.class, "intake_left_top");
-        servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom");
-        servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom");
+        liftA = hardwareMap.get(DcMotor.class, Name.LIFT_L);
+        liftB = hardwareMap.get(DcMotor.class, Name.LIFT_R);
 
-        intake = new GlyphIntake2(servos);
-        gyro = new Gyro(hardwareMap);
+        intake = new GlyphIntake2(hardwareMap);
         limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1");
     }
 
     @Override
     public void loop() {
-        driveTrain.driveMechanum(gamepad1);
+        gyro.update();
+        driveTrain.halfPower(gamepad1.left_bumper);
+        driveTrain.driveMechanum(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.right_stick_x);
 
-        if (gamepad2.a)
+        if (gamepad2.a) {
             intake.openBottom();
-        else
+        } else {
             intake.closeBottom();
+        }
 
-        if (gamepad2.b)
+        if (gamepad2.b) {
             intake.openTop();
-        else
+        } else {
             intake.closeTop();
+        }
 
         liftA.setPower(gamepad2.left_stick_y);
         liftB.setPower(-gamepad2.left_stick_y);
 
-        telemetry.addData("Orientation", gyro.x + "°");
+        telemetry.addData("Gyroscope", gyro);
         telemetry.addData("Limit Switch", limitSwitch);
+        telemetry.update();
+    }
+
+
+    @Override
+    public void stop() {
+        driveTrain.stop();
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java
index f23a199..962e698 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java
@@ -1,30 +1,38 @@
 package org.firstinspires.ftc.teamcode.Components;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.GyroLock;
+import org.firstinspires.ftc.teamcode.Name;
 import org.firstinspires.ftc.teamcode.PID;
-import org.firstinspires.ftc.teamcode.Gyro;
 
 /**
- * Created by Joshua Rapoport on 11/20/17.
+ * Created by Montclair Robotics on 11/27/17
+ * @author Joshua Rapoport
+ * @version 12/2/17.
  */
 
 public class DriveTrain {
-    public static final double TURN_ERROR = 0.1;
+    public static final double TURN_ERROR = 0.05;
 
     GyroLock lock;
+//    GyroBalance balance;
+
+    private double power;
     DcMotor frontLeft, frontRight, backLeft, backRight;
 
-    public DriveTrain(HardwareMap map, Gyro gyro) {
-        lock = new GyroLock(new PID(0.01, 0, 0.005), gyro);
+    public DriveTrain(HardwareMap map) {
+        this.lock = new GyroLock(new PID(0.04, 0, 0.02)); // TODO: Calibrate GyroLock
+
+//        this.balance = new GyroBalance(new PID(0, 0, 0), new PID(0, 0, 0)); // TODO: Calibrate GyroBalance
 
-        frontRight = map.get(DcMotor.class, "right_front");
-        backRight = map.get(DcMotor.class, "right_back");
-        backLeft = map.get(DcMotor.class, "left_back");
-        frontLeft = map.get(DcMotor.class, "left_front");
+        this.power = 0;
+
+        frontRight = map.get(DcMotor.class, Name.DRIVETRAIN_FR);
+        backRight = map.get(DcMotor.class, Name.DRIVETRAIN_BR);
+        backLeft = map.get(DcMotor.class, Name.DRIVETRAIN_BL);
+        frontLeft = map.get(DcMotor.class, Name.DRIVETRAIN_FL);
 
         frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
@@ -33,31 +41,38 @@ public DriveTrain(HardwareMap map, Gyro gyro) {
     }
 
     public void stop() {
-        frontLeft.setPower(0);
-        frontRight.setPower(0);
-        backLeft.setPower(0);
-        backRight.setPower(0);
+        driveMechanum(0, 0, 0);
     }
 
-    public void driveMechanum(Gamepad g) {
-        double pow = 1.0;
+    public void halfPower(boolean h) {
+        power = 0.8;
 
-        if (g.left_bumper) {
-            pow = 0.5;
+        if (h) {
+            power = 0.4;
         }
+    }
 
-        double x = g.left_stick_x * pow;
-        double y = -g.left_stick_y * pow;
-        double turn = g.right_stick_x * pow;
+    /**
+     * A single loop.
+     * @param x going along x-axis, from -1 to 1.
+     * @param y going along x-axis, from -1 to 1.
+     * @param turn rotating clockwise, from -1 to 1.
+     */
+    public void driveMechanum(double x, double y, double turn) {
+        if (Math.abs(turn) < TURN_ERROR) {
+            if (lock.init()) {
+                lock.setTarget();
+            }
 
-        if (turn < TURN_ERROR) {
-            lock.teleopLoop(g);
+            lock.setInput();
             turn = lock.correction();
+        } else {
+            lock.stop();
         }
 
-        frontRight.setPower(x - y + turn);
-        backRight.setPower(-x - y + turn);
-        backLeft.setPower(-x + y + turn);
-        frontLeft.setPower(x + y + turn);
+        frontRight.setPower(power * (x + y + turn));
+        backRight.setPower(power * (-x + y + turn));
+        backLeft.setPower(power * (-x - y + turn));
+        frontLeft.setPower(power * (x - y + turn));
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java
index 0b9cb8a..5280e27 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java
@@ -1,12 +1,14 @@
 package org.firstinspires.ftc.teamcode.Components;
 
+import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
+import org.firstinspires.ftc.teamcode.Name;
 import org.firstinspires.ftc.teamcode.Robot;
 
 /**
  * Created by Montclair Robotics on 11/13/17.
- * @Author:Rafi
+ * @author Rafi
  * */
 
 public class GlyphIntake2 {
@@ -19,8 +21,14 @@ public class GlyphIntake2 {
 
     private Servo[] servos;
 
-    public GlyphIntake2(Servo... servos) {
-        this.servos = servos;
+    public GlyphIntake2(HardwareMap map) {
+        this.servos = new Servo[] {
+                map.get(Servo.class, Name.INTAKE_TR),
+                map.get(Servo.class, Name.INTAKE_TL),
+                map.get(Servo.class, Name.INTAKE_BR),
+                map.get(Servo.class, Name.INTAKE_BL)
+        };
+
         servoOpen=new double[4];
         servoClose=new double[4];
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java
index 53f6ca5..34d98e8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java
@@ -2,15 +2,12 @@
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.montclairrobotics.sprocket.core.IMotor;
 import org.montclairrobotics.sprocket.ftc.FTCMotor;
 import org.montclairrobotics.sprocket.motors.Module;
-import org.montclairrobotics.sprocket.motors.SEncoder;
-import org.montclairrobotics.sprocket.utils.PID;
 
 /**
  * Created by Montclair Robotics on 11/13/17.
- * @Author:Garrett
+ * @author Garrett
  * */
 
 public class GlyphLift extends Module{
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java
index 07c66d1..9331e8c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java
@@ -1,28 +1,21 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.montclairrobotics.sprocket.loop.Priority;
-import org.montclairrobotics.sprocket.loop.Updatable;
-import org.montclairrobotics.sprocket.loop.Updater;
-import org.montclairrobotics.sprocket.utils.Input;
 
 /**
  * Created by Montclair Robotics on 11/13/17.
- * @Author:Jack
+ * @author Jack
  * */
 
-public class Gyro implements Input<Double>, Updatable {
-    double x, y, z;
+public class Gyro {
+    public static Gyro current;
 
     private RRQuaternion quat; // An angle object to store the gyro angles
     private BNO055IMU imu; // Gyroscope
 
-    public Gyro(HardwareMap map) {
-        x = y = z = 0;
-
-        imu = map.get(BNO055IMU.class, "gyro");
+    public Gyro(BNO055IMU imu) {
+        this.imu = imu;
+        this.quat = new RRQuaternion();
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro
         parameters.angleUnit            = BNO055IMU.AngleUnit.DEGREES; // set the angle unit parameter to
@@ -31,22 +24,25 @@ public Gyro(HardwareMap map) {
         parameters.loggingTag           = "IMU"; // set the logging tag
 
         imu.initialize(parameters);
-        imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6);
 
-        update();
-        Updater.add(this, Priority.INPUT);
+        current = this;
     }
 
-    @Override
     public void update() {
         quat = new RRQuaternion(imu.getQuaternionOrientation());
-        x = (double) ((int) (10*quat.getX())) / 10;
-        y = (double) ((int) (10*quat.getY())) / 10;
-        z = (double) ((int) (10*quat.getZ())) / 10;
     }
 
-    @Override
-    public Double get() {
-        return quat.getX();
+    public Double getX() {
+        return -quat.getX();
+    }
+    public Double getY() {
+        return quat.getY();
+    }
+    public Double getZ() {
+        return quat.getZ();
+    }
+
+    public String toString() {
+        return "{x: " + getX().intValue() + "°,\ty: " + getY().intValue() + "°,\tz: " + getZ().intValue() + "°}";
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java
new file mode 100644
index 0000000..6bd7c30
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java
@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode;
+
+/**
+ * @author Joshua Rapoport
+ * @version 11/27/17.
+ */
+
+public class GyroBalance {
+    private Gyro gyro;
+    private PID xPID, yPID;
+
+    public GyroBalance(PID x, PID y) {
+        this.gyro = Gyro.current;
+        this.xPID = x;
+        this.yPID = y;
+
+        xPID.setTarget(0.0);
+        yPID.setTarget(0.0);
+
+        xPID.setInputRange(-45, 45);
+        yPID.setInputRange(-45, 45);
+
+        xPID.setOutputRange(-1.0, 1.0);
+        yPID.setOutputRange(-1.0, 1.0);
+    }
+
+    public double correctionX() { return xPID.getOutput(); }
+    public double correctionY() { return yPID.getOutput(); }
+
+    public void update() {
+        xPID.setInput(gyro.getY()); // TODO: double check please
+        yPID.setInput(gyro.getZ()); // TODO: double check please
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java
index 1d19d78..fda5788 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java
@@ -1,12 +1,8 @@
 package org.firstinspires.ftc.teamcode;
 
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.teamcode.Components.DriveTrain;
-import org.montclairrobotics.sprocket.utils.Debug;
-
 /**
- * Created by Joshua Rapoport on 11/16/17.
+ * @author Joshua Rapoport
+ * @version 11/27/17.
  */
 
 public class GyroLock {
@@ -14,11 +10,13 @@ public class GyroLock {
     private PID pid;
     private boolean active;
 
-    public GyroLock(PID pid, Gyro gyro) {
-        this.gyro = gyro;
+    public GyroLock(PID pid) {
+        this.gyro = Gyro.current;
         this.pid = pid;
         this.active = false;
 
+        pid.setTarget(gyro.getX());
+
         pid.setInputRange(-180, 180);
         pid.setOutputRange(-1.0, 1.0);
     }
@@ -27,36 +25,34 @@ public double correction() {
         return pid.getOutput();
     }
 
-    public void teleopLoop(Gamepad g) {
-        if (Math.abs(g.right_stick_x) < DriveTrain.TURN_ERROR) {
-            // If the gyro-lock wasn't active before...
-            if (!active) {
-                // Reset the error record.
-                pid.error.reset();
-                // Set the target to the current trajectory.
-                pid.setTarget(gyro.get());
-            }
-
-            // It sure is active now.
+    public void setTarget() {
+        setTarget(gyro.getX());
+    }
+
+    public void setTarget(double t) {
+        pid.setTarget(t);
+    }
+
+    public void setInput() {
+        pid.setInput(gyro.getX());
+    }
+
+    public boolean init() {
+        if (!active) {
+            pid.error.reset();
             active = true;
-            // Run update loop.
-            loop();
-        } else {
-            // DENIED!!!
-            active = false;
+            return true;
         }
-    }
 
-    public void autoLoop() {
-        //...
+        return false;
     }
 
-    public void loop() {
-        gyro.update();
-        pid.setInput(gyro.get());
-        pid.update();
+    public boolean stop() {
+        if (active) {
+            active = false;
+            return true;
+        }
 
-        Debug.msg("PID Input", pid.getInput().intValue() + "°");
-        Debug.msg("PID Output", (int) (100 * pid.getOutput()) + "%");
+        return false;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java
new file mode 100644
index 0000000..30e60fd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Name.java
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode;
+
+/**
+ * Created by Montclair Robotics on 12/2/17.
+ * @author Joshua Rapoport
+ */
+
+public final class Name {
+    public static final String DRIVETRAIN_FL = "left_front";
+    public static final String DRIVETRAIN_FR = "right_front";
+    public static final String DRIVETRAIN_BL = "left_back";
+    public static final String DRIVETRAIN_BR = "right_back";
+
+    public static final String LIFT_L = "lift_left";
+    public static final String LIFT_R = "lift_right";
+
+    public static final String INTAKE_TL = "intake_left_top";
+    public static final String INTAKE_TR = "intake_right_top";
+    public static final String INTAKE_BL = "intake_left_bottom";
+    public static final String INTAKE_BR = "intake_right_bottom";
+
+    public static final String SENSOR_COLOR = "sensor_color";
+    public static final String SENSOR_GYRO = "gyro";
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java
index b9c7848..8a30a80 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java
@@ -1,46 +1,43 @@
 package org.firstinspires.ftc.teamcode;
 
 /**
- * Created by Joshua Rapoport on 11/14/17.
+ * Created by Montclair Robotics on 11/10/2017
+ * @author Joshua Rapoport
+ * @version 12/2/2017
  */
 
 public class PID {
-    public class Range {
+    class Range {
         double min, max;
 
-        public Range() {
+        Range() {
             this.min = this.max = 0;
         }
-        public Range(double a, double b) {
+        Range(double a, double b) {
             this.min = a;
             this.max = b;
         }
 
-        public double distance() {
+        double distance() {
             return Math.abs(max - min);
         }
-        public boolean contains(double x) {
-            return x >= min && x <= max;
-        }
-        public int compareTo(double x) {
-            if (contains(x))
-                return 0;
-            else if (x < min)
+        int compareTo(double x) {
+            if (x < min)
                 return -1;
-            else
+            else if (x > max)
                 return 1;
+            else
+                return 0;
         }
     }
 
-    public class Error {
-        double current, total;
-        public Error() {
+    class Error {
+        double current, rate, total;
+
+        Error() {
             reset();
         }
-        public void update() {
-            total += current * dTime();
-        }
-        public void reset() { current = total = 0; }
+        void reset() { current = rate = total = 0; }
     }
 
     private double P, I, D;
@@ -48,6 +45,7 @@ public void update() {
     protected Range inRange;
     /** The acceptable range of output. */
     protected Range outRange;
+
     /** An object that handles error due to input-target difference. */
     protected Error error;
 
@@ -55,6 +53,7 @@ public void update() {
     private Double input;
     /** The robot's target, to be compared with the input. */
     private double target;
+    /** The calculated output to correct the error. */
     private double output;
 
     private long lastUpdateTime;
@@ -76,72 +75,39 @@ public PID(double p, double i, double d) {
 
         this.input = this.target = this.output = 0.0;
 
-        update();
+        this.lastUpdateTime = System.currentTimeMillis() / 1000;
     }
 
-    public double getOutput() {
-        return output;
+    /** Use this method to get a new output value. */
+    public void setInput(double i) {
+        this.updateOutput(i);
+        this.input = i;
     }
-    private double setOutput(double newInput) {
-        error.current = target - newInput;
-        double dInput = newInput - input;
-
-        double diff = inRange.distance();
-        if (diff != 0) {
-            error.current = ((error.current - inRange.min) % diff + diff) % diff + inRange.min;
-            dInput = ((dInput - inRange.min) % diff + diff) % diff + inRange.min;
-        }
 
-        error.update();
+    private void updateOutput(double i) {
+        double d = inRange.distance();
+        error.current = ((target - i - d / 2) % d + d) % d + d / 2;
+        error.rate = (dTime() > 0) ? ((target - i) - error.current) / dTime() : 0;
 
         if (I != 0) {
-            double potentialI = (error.current + error.total) * I;
+            double potentialI = I * error.total;
+
             if (outRange.compareTo(potentialI) > 0)
                 error.total = outRange.max / I;
             else if (outRange.compareTo(potentialI) < 0)
                 error.total = outRange.min / I;
             else
-                error.total += error.current;
+                error.total += error.current * dTime();
         }
 
-        double out = (P * error.current * dTime()) + (I * error.total) + (D * -dInput / dTime());
-
-        setInput(newInput);
-
-        if (outRange.compareTo(out) > 0)
-            out = outRange.max;
-        else if (outRange.compareTo(out) < 0)
-            out = outRange.min;
-
-        return out;
-    }
-
-    /** @return a reference to a different PID object with identical properties */
-    public PID copy() {
-        PID c = new PID(P, I, D);
-        c.setInputRange(inRange.min, inRange.max);
-        c.setOutputRange(outRange.min, outRange.max);
-        c.setTarget(target);
+        lastUpdateTime = System.currentTimeMillis();
 
-        return c;
-    }
-
-    public void setPID(double p, double i, double d) {
-        P = p;
-        I = i;
-        D = d;
-    }
-    /** @return P, the proportional constant. */
-    public double getP() {
-        return P;
-    }
-    /** @return I, the integral constant. */
-    public double getI() {
-        return I;
-    }
-    /** @return D, the derivative constant. */
-    public double getD() {
-        return D;
+        if (outRange.compareTo(output) > 0)
+            this.output = outRange.max;
+        else if (outRange.compareTo(output) < 0)
+            this.output = outRange.min;
+        else
+            this.output = (P * error.current) + (I * error.total) + (D * error.rate);
     }
 
     public void setInputRange(double a, double b) {
@@ -154,24 +120,14 @@ public void setOutputRange(double a, double b) {
     public Double getInput() {
         return input;
     }
-
-    /** Use this method to get a new output value. */
-    public void setInput(double i) {
-        this.output = setOutput(i);
-        this.input = i;
+    public double getOutput() {
+        return output;
     }
 
-    public double getTarget() {
-        return target;
-    }
-    public void setTarget(double t) { this.target = target; }
+    public void setTarget(double t) { this.target = t; }
 
     /** @return the time difference from the last update. */
     public double dTime() {
-        return System.currentTimeMillis() - lastUpdateTime;
-    }
-
-    public void update() {
-        lastUpdateTime = System.currentTimeMillis();
+        return (System.currentTimeMillis()) - lastUpdateTime;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java
index 6ffcdd6..ff3fb4b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RRQuaternion.java
@@ -26,6 +26,10 @@
 public class RRQuaternion {
     public double w, x, y, z;
 
+    public RRQuaternion() {
+        w = x = y = z = 0.0;
+    }
+
     public RRQuaternion(double w, double x, double y, double z) {
         this.w = w;
         this.x = x;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
index 90ebf88..c4a89ba 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
@@ -66,19 +66,13 @@ public void setup() {
         frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
         limitSwitch       = hardwareMap.get(DigitalChannel.class, "limit_switch_1");
 
-        servos = new Servo[4];
-        servos[0] = hardwareMap.get(Servo.class, "intake_right_top");
-        servos[1] = hardwareMap.get(Servo.class, "intake_left_top");
-        servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom");
-        servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom");
-
         liftLeft = new FTCMotor("lift_left");
         liftRight = new FTCMotor("lift_right");
         lift = new GlyphLift(liftLeft, liftRight);
-        intake=new GlyphIntake2(servos);
+        intake=new GlyphIntake2(hardwareMap);
         limitSwitch.setMode(DigitalChannel.Mode.INPUT);
 
-        gyro = new Gyro(hardwareMap);
+//        gyro = new Gyro(hardwareMap);
         final DriveModule[] modules = new DriveModule[4];
 
         //Mecanum
@@ -285,9 +279,9 @@ public void disable() {
 
     @Override
     public void update() {
-        Debug.msg("X-angle", gyro.x);
-        Debug.msg("Y-angle", gyro.y);
-        Debug.msg("Z-angle", gyro.z);
+        Debug.msg("X-angle", gyro.getX());
+        Debug.msg("Y-angle", gyro.getY());
+        Debug.msg("Z-angle", gyro.getZ());
 
         //Debug.msg("Servo",intakeRightTop.getPosition());
         Debug.msg("Switch Value:", limitSwitch.getState());