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 041097e490f3e5dccc574cac9d8b4757f8717ef7..44c513d5085fe4fda891a4ab086a838106454f50 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,5 +1,6 @@
 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;
@@ -20,6 +21,17 @@ public class Robot extends IterativeRobot {
     oi = OI.getOI();
     shooter = Shooter.getShooter();
     intake = Intake.getIntake();
+
+    SmartDashboard.putNumber(driveTrain.DRIVE_P_Val, 0);
+    SmartDashboard.putNumber(driveTrain.DRIVE_I_Val, 0);
+    SmartDashboard.putNumber(driveTrain.DRIVE_D_Val, 0);
+    SmartDashboard.putNumber(driveTrain.DRIVE_TARGET_DIST, 50);
+    SmartDashboard.putNumber(driveTrain.DRIVE_MOTOR_VAL, 0.5);
+
+    SmartDashboard.putNumber(driveTrain.GYRO_P_Val, 0);
+    SmartDashboard.putNumber(driveTrain.GYRO_I_Val, 0);
+    SmartDashboard.putNumber(driveTrain.GYRO_D_Val, 0);
+
   }
 
   public static DriveTrain getDriveTrain() {
@@ -44,35 +56,24 @@ public class Robot extends IterativeRobot {
   public void autonomousInit() {
     driveTrain.setHighGear();
 
-<<<<<<< 7361ba4ac28673251bd93f8a926927ca81a1eec6
-    SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_P_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_I_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_D_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_TARGET_DIST, 50);
-    SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_MOTOR_VAL, 0.5);
-
-    SmartDashboard.putNumber(Constants.DriveTrain.GYRO_P_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.GYRO_I_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.GYRO_D_Val, 0);
-
-    double driveP = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
-        Constants.DriveTrain.PID_ERROR);
-    double driveI = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_I_Val,
-        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,
-        Constants.DriveTrain.PID_ERROR);
-    double gyroD = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_D_Val,
-        Constants.DriveTrain.PID_ERROR);
-
-    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);
+    double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_D_Val,
+        driveTrain.PID_ERROR);
+    double driveI = SmartDashboard.getNumber(driveTrain.DRIVE_I_Val,
+        driveTrain.PID_ERROR);
+    double driveD = SmartDashboard.getNumber(driveTrain.DRIVE_D_Val,
+        driveTrain.PID_ERROR);
+
+    double gyroP = SmartDashboard.getNumber(driveTrain.GYRO_P_Val,
+        driveTrain.PID_ERROR);
+    double gyroI = SmartDashboard.getNumber(driveTrain.GYRO_I_Val,
+        driveTrain.PID_ERROR);
+    double gyroD = SmartDashboard.getNumber(driveTrain.GYRO_D_Val,
+        driveTrain.PID_ERROR);
+
+    double Setpoint = SmartDashboard.getNumber(driveTrain.DRIVE_TARGET_DIST,
+        driveTrain.PID_ERROR);
+    double Speed = SmartDashboard.getNumber(driveTrain.DRIVE_MOTOR_VAL,
+        driveTrain.PID_ERROR);
 
     driveTrain.getDriveController().setName("Drive");
     driveTrain.getGyroController().setName("Gyro");
@@ -81,36 +82,20 @@ public class Robot extends IterativeRobot {
 
     driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
 
+    Scheduler.getInstance().add(new DriveDistance(Setpoint, Speed));
+
+    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
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
 
-    double driveP = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_D_Val,
-        Constants.DriveTrain.PID_ERROR);
-    double driveI = SmartDashboard.getNumber(Constants.DriveTrain.DRIVE_I_Val,
-        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,
-        Constants.DriveTrain.PID_ERROR);
-    double gyroD = SmartDashboard.getNumber(Constants.DriveTrain.GYRO_D_Val,
-        Constants.DriveTrain.PID_ERROR);
-
-    driveTrain.getDriveController().setName("Drive");
-    driveTrain.getGyroController().setName("Gyro");
-
-    driveTrain.getDriveController().setConstants(driveP, driveI, driveD);
-
-    driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
-
-    // new DriveDistance(SETPOINT, SPEED).start();
-
-    // new TurnForAngle(0, Direction.FORWARD, 5).start();
   }
 
   @Override