diff --git a/src/Commands/Drive.cpp b/src/Commands/Drive.cpp
index fd136b0..d4dfdbd 100644
--- a/src/Commands/Drive.cpp
+++ b/src/Commands/Drive.cpp
@@ -7,17 +7,22 @@ Drive::Drive() : Command("Drive"){
 void Drive::Initialize(){
 }
 void Drive::Execute(){
-  static float sens=0.7;
-  float x, y, twist;
-  x=DentRobot::oi->GetLeftStick()->GetRawAxis(0);
-  y=DentRobot::oi->GetLeftStick()->GetRawAxis(1);
-  twist=DentRobot::oi->GetLeftStick()->GetRawAxis(2);
-  if(true){
-    x=sens*std::pow(x, 3)+(1.0f-sens)*x;
-    y=sens*std::pow(y, 3)+(1.0f-sens)*y;
+  double x,y,z;
+  //Code to lock the x axis when not holding button 1
+  if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){
+    x = DentRobot::oi->GetLeftStick()->GetRawAxis(0);
+    x /= 1.3;
+  }else{
+    x = 0;
   }
-  //DentRobot::drivetrain->DriveMecanum(DentRobot::oi->GetLeftStick()->GetRawAxis(2), DentRobot::oi->GetLeftStick()->GetRawAxis(1), DentRobot::oi->GetLeftStick()->GetRawAxis(0));
-  DentRobot::drivetrain->DriveMecanum(x, y, twist);
+  z = DentRobot::oi->GetLeftStick()->GetRawAxis(2);
+  y = DentRobot::oi->GetLeftStick()->GetRawAxis(1);
+  if (DentRobot::oi->GetLeftStick()->GetRawAxis(3)<=0.5){
+      y /= 2;
+      z /= 2;
+  }
+  //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro
+  DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0);
 }
 bool Drive::IsFinished(){
   return false;
diff --git a/src/Subsystems/Drivetrain.cpp b/src/Subsystems/Drivetrain.cpp
index cfbf5bf..e9fc379 100644
--- a/src/Subsystems/Drivetrain.cpp
+++ b/src/Subsystems/Drivetrain.cpp
@@ -3,18 +3,21 @@
 #include "../Commands/Drive.h"
 
 Drivetrain::Drivetrain() : Subsystem("Drivetrain"){
-  frontLeft=new CANTalon(40);
-  frontRight=new CANTalon(41);
-  backLeft=new CANTalon(42);
-  backRight=new CANTalon(43);
-  drive=new RobotDrive(frontLeft, frontRight, backLeft, backRight);
+  rightFront = new CANTalon(40);
+  leftFront = new CANTalon(41);
+  rightRear = new CANTalon(42);
+  leftRear = new CANTalon(43);
 }
 void Drivetrain::InitDefaultCommand(){
   SetDefaultCommand(new Drive());
 }
-void Drivetrain::DriveMecanum(float x, float y, float rotation){
-  drive->MecanumDrive_Cartesian(x, y, rotation);
-}
-void Drivetrain::DriveArcade(float x, float y){
-  drive->ArcadeDrive(x, y);
+void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){
+    double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y);
+    double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x);
+    double correctZ = -z *.5;
+    double slowfactor = 2.5;
+    rightFront->Set((-correctX + correctY - correctZ));
+    leftFront->Set((correctX + correctY + correctZ)*-1);
+    rightRear->Set((correctX + correctY - correctZ));
+    leftRear->Set((-correctX + correctY + correctZ)*-1);
 }
diff --git a/src/Subsystems/Drivetrain.h b/src/Subsystems/Drivetrain.h
index d4b1a6c..bdf3f70 100644
--- a/src/Subsystems/Drivetrain.h
+++ b/src/Subsystems/Drivetrain.h
@@ -5,12 +5,12 @@
 class Drivetrain: public Subsystem
 {
   private:
-    CANTalon *frontLeft, *frontRight, *backLeft, *backRight;
+    CANTalon *rightFront, *leftFront, *rightRear, *leftRear;
     RobotDrive *drive;
   public:
     Drivetrain();
     void InitDefaultCommand();
-    void DriveMecanum(float, float, float);
+    void DriveMecanum(float,float,float,float,float);
     void DriveArcade(float, float);
 };
 #endif
diff --git a/src/Subsystems/Elevator.cpp b/src/Subsystems/Elevator.cpp
index 490cc7c..20261b5 100644
--- a/src/Subsystems/Elevator.cpp
+++ b/src/Subsystems/Elevator.cpp
@@ -1,6 +1,6 @@
 #include "Elevator.h"
 #include "../RobotMap.h"
-Elevator::Elevator() : PIDSubsystem("Elevator", kP_real, kI_real, 0.0){
+Elevator::Elevator()/* : PIDSubsystem("Elevator", kP_real, kI_real, 0.0)*/{
   pot=new AnalogPotentiometer(0);
   leftMotor=new Talon(1);
   rightMotor=new Talon(0);
diff --git a/src/Subsystems/Elevator.h b/src/Subsystems/Elevator.h
index d80ddd0..e54d719 100644
--- a/src/Subsystems/Elevator.h
+++ b/src/Subsystems/Elevator.h
@@ -3,7 +3,7 @@
 
 #include "WPILib.h"
 #include "Commands/PIDSubsystem.h"
-class Elevator: public PIDSubsystem{
+class Elevator/*: public PIDSubsystem*/{
   private:
     AnalogPotentiometer *pot;
     Talon *leftMotor, *rightMotor;
diff --git a/src/hhlib b/src/hhlib
new file mode 160000
index 0000000..181fb15
--- /dev/null
+++ b/src/hhlib
@@ -0,0 +1 @@
+Subproject commit 181fb1556d4cc96daa430bfce0fb6ce26c7c90c9