From d06f436f11f5dd4da3d93521abf8df3c13e48477 Mon Sep 17 00:00:00 2001
From: Austen Adler <stonewareslord@gmail.com>
Date: Fri, 13 Mar 2015 19:47:37 -0400
Subject: [PATCH] Changeable gyro kP (untested)

---
 DentRobot.cpp             | 3 ++-
 Subsystems/Drivetrain.cpp | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/DentRobot.cpp b/DentRobot.cpp
index eb45120..bb790e8 100644
--- a/DentRobot.cpp
+++ b/DentRobot.cpp
@@ -35,7 +35,6 @@ void DentRobot::RobotInit(){
   SmartDashboard::PutNumber("Auto Tote Distance", 0.5);
   SmartDashboard::PutNumber("Auto Bin Distance", 0.25);
   SmartDashboard::PutNumber("TurnAmount", 1.8);
-
   // Elevators
   SmartDashboard::PutBoolean("Bin Elevator Bottom", false);
   SmartDashboard::PutBoolean("Bin Elevator Top", false);
@@ -43,6 +42,8 @@ void DentRobot::RobotInit(){
   SmartDashboard::PutBoolean("Elevator Top", false);
   //Drive speed
   SmartDashboard::PutNumber("DriveSpeedReductionThresh", 2.0);
+  //Gyro
+  SmartDashboard::PutNumber("Gyro kP", 0.02);
 }
 void DentRobot::DisabledPeriodic(){
   Scheduler::GetInstance()->Run();
diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp
index 4587e8c..4e368f7 100644
--- a/Subsystems/Drivetrain.cpp
+++ b/Subsystems/Drivetrain.cpp
@@ -14,7 +14,7 @@ void Drivetrain::InitDefaultCommand(){
 }
 void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, bool driveStraight){
   //TODO: Find the correct value for kP
-  double kP=0.02;
+  double kP=SmartDashboard::GetNumber("Gyro kP");
   double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x);
   double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y);
   double correctZ;