diff --git a/src/HHRobot.cpp b/src/HHRobot.cpp
index fc70017..5ce416c 100644
--- a/src/HHRobot.cpp
+++ b/src/HHRobot.cpp
@@ -1,20 +1,32 @@
 #include "HHRobot.h"
 #include "HHBase.h"
 HHRobot::HHRobot():
-  //
   hhdrive(new RobotDrive(2,0,3,1)),
+  gyro(new Gyro(1)),
   joystick1(new Extreme3dPro(0)){
     hhdrive->SetExpiration(0.1);
-	hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
-	hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
+    hhdrive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
+    hhdrive->SetInvertedMotor(RobotDrive::kRearLeftMotor,true);
   }
 void HHRobot::Init(){
   printf("Initing\n");
   printf("Code Version: %f\n",0000.1);
+  gyro->Reset();
   //Put table values initally to avoid annoying refreshing
 }
 //Main function used to handle periodic tasks on the robot
 void HHRobot::Handler(){
-	hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
+  const float Kp = 0.3;
+  if(joystick1->GetJoystickButton(1)==1){
+    hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), 0, joystick1->GetJoystickAxis("x"));
+  }else if(joystick1->GetJoystickButton(2)==1){
+    hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), 0);
+  }else if(joystick1->GetJoystickButton(3)==1){
+    hhdrive->Drive(joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("y")*Kp*-gyro->GetAngle());
+  }else{
+    hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
+  }
+  Wait(0.005);
+  // hhdrive->MecanumDrive_Cartesian(joystick1->GetJoystickAxis("z"), joystick1->GetJoystickAxis("y"), joystick1->GetJoystickAxis("x"));
 }
 // vim: ts=2:sw=2:et
diff --git a/src/HHRobot.h b/src/HHRobot.h
index 814029b..398e3de 100644
--- a/src/HHRobot.h
+++ b/src/HHRobot.h
@@ -6,6 +6,7 @@
 class HHRobot{
   private:
     RobotDrive *hhdrive;
+    Gyro *gyro;
     Extreme3dPro *joystick1;
   public:
     HHRobot();