fix alot of unnecessary/not used code and init gyroController

[3501/2017steamworks] / src / org / usfirst / frc / team3501 / robot / Robot.java

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index cde55dc61d13cf2c33e969d72b2d510d2803e2a5..039f5d65a78a0becbf3672e4417b10e20331985b 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -62,6 +62,7 @@ public class Robot extends IterativeRobot {
         Constants.DriveTrain.PID_ERROR);
     double driveD = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
         Constants.DriveTrain.PID_ERROR);
+
     double gyroP = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_P_Val,
         Constants.DriveTrain.PID_ERROR);
     double gyroI = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_I_Val,
@@ -72,15 +73,12 @@ public class Robot extends IterativeRobot {
     DriveTrain.getDriveTrain().getDriveController().setConstants(driveP, driveI,
         driveD);
 
-    DriveTrain.getDriveTrain().getGyroController().setConstants(driveP, driveI,
-        driveD);
+    DriveTrain.getDriveTrain().getGyroController().setConstants(gyroP, gyroI,
+        gyroD);
 
     // new DriveDistance(SETPOINT, SPEED).start();
-    double Setpoint = SmartDashboard.getNumber(
-        Constants.DriveTrain.DRIVE_TARGET_DIST, Constants.DriveTrain.PID_ERROR);
-    double Speed = SmartDashboard.getNumber(
-        Constants.DriveTrain.DRIVE_MOTOR_VAL, Constants.DriveTrain.PID_ERROR);
 
+    // new TurnForAngle(0, Direction.FORWARD, 5).start();
   }
 
   @Override