remove unnecessary code from autonInit and autonPeriodic, fix maxTimeOut bug, and...

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index b910b296f74ba3e384a3feb666af963448f1597b..b1e2a8cd1e6a5bdeef572fd88e9147e839a21769 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;
@@ -55,33 +56,6 @@ public class Robot extends IterativeRobot {
   public void autonomousInit() {
     driveTrain.setHighGear();
 
-<<<<<<< HEAD
-    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.GYRO_P_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.GYRO_I_Val, 0);
-    SmartDashboard.putNumber(Constants.DriveTrain.GYRO_D_Val, 0);
-    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.MOTOR_VAL, 0.5);
-
-    SmartDashboard.putNumber(Constants.DriveTrain.DRIVE_MOTOR_VAL, 0.5);
-=======
-    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);
->>>>>>> fix errors
-
     double driveP = SmartDashboard.getNumber(driveTrain.DRIVE_D_Val,
         driveTrain.PID_ERROR);
     double driveI = SmartDashboard.getNumber(driveTrain.DRIVE_I_Val,
@@ -108,6 +82,8 @@ public class Robot extends IterativeRobot {
 
     driveTrain.getGyroController().setConstants(gyroP, gyroI, gyroD);
 
+    Scheduler.getInstance().add(new DriveDistance(Setpoint, Speed));
+
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
index ccbe5fd25c7056e86e15d7f00b42e05fa91107e9..f6e11f4c94268c7190d540edb3045e618f2e5488 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java
@@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.command.Command;
  */
 public class DriveDistance extends Command {
   private DriveTrain driveTrain = Robot.getDriveTrain();
-  private double maxTimeOut;
+  private double maxTimeOut = 10;
   private double target;
   private double zeroAngle;
   private Preferences prefs;
@@ -26,11 +26,10 @@ public class DriveDistance extends Command {
 
   public DriveDistance(double distance, double maxTimeOut) {
     requires(driveTrain);
-    this.maxTimeOut = maxTimeOut;
     this.target = distance;
     this.driveController = driveTrain.getDriveController();
     this.driveController.setDoneRange(0.5);
-    this.driveController.setMaxOutput(1.0);
+    this.driveController.setMaxOutput(motorVal);
     this.driveController.setMinDoneCycles(5);
   }