update code

[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 e9b1d4f8d8b23078b0ab6cefb960b2247373362e..003e138eae22de8f0a6640ad13110891832b8813 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,6 +1,5 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.subsystems.Intake;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
@@ -51,6 +50,12 @@ public class Robot extends IterativeRobot {
     SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_TARGET_DIST, 50);
     SmartDashboard.putNumber(Constants.DriveTrain.MOTOR_VAL, 0.5);
 
+    SmartDashboard.putNumber(Constants.DriveTrain.P_Val, 0);
+    SmartDashboard.putNumber(Constants.DriveTrain.I_Val, 0);
+    SmartDashboard.putNumber(Constants.DriveTrain.D_Val, 0);
+    SmartDashboard.putNumber(Constants.DriveTrain.TARGET_DIST, 50);
+    SmartDashboard.putNumber(Constants.DriveTrain.MOTOR_VAL, 0.5);
+
   }
 
   @Override
@@ -63,6 +68,18 @@ public class Robot extends IterativeRobot {
         Constants.DriveTrain.PID_ERROR);
     double D = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
         Constants.DriveTrain.PID_ERROR);
+    double P = SmartDashboard.getNumber(Constants.DriveTrain.P_Val,
+        Constants.DriveTrain.PID_ERROR);
+    double I = SmartDashboard.getNumber(Constants.DriveTrain.I_Val,
+        Constants.DriveTrain.PID_ERROR);
+    double D = SmartDashboard.getNumber(Constants.DriveTrain.D_Val,
+        Constants.DriveTrain.PID_ERROR);
+
+    double SPEED = SmartDashboard.getNumber(Constants.DriveTrain.MOTOR_VAL, 0);
+
+    double SETPOINT = SmartDashboard.getNumber(Constants.DriveTrain.TARGET_DIST,
+        Constants.DriveTrain.TARGET_DISTANCE);
+
     double SPEED = SmartDashboard.getNumber(Constants.DriveTrain.MOTOR_VAL, 0);
 
     double SETPOINT = SmartDashboard.getNumber(
@@ -71,7 +88,7 @@ public class Robot extends IterativeRobot {
 
     DriveTrain.getDriveTrain().getDriveController().setConstants(P, I, D);
 
-    new DriveDistance(SETPOINT, SPEED).start();
+    // new DriveDistance(SETPOINT, SPEED).start();
   }
 
   @Override