challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java

   1 package org.usfirst.frc.team3501.robot.commands.driving;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4 import org.usfirst.frc.team3501.robot.Robot;
   5
   6 import edu.wpi.first.wpilibj.command.Command;
   7
   8 /***
   9  * This command will move the robot at the specified speed for the specified
  10  * distance.
  11  *
  12  * post-condition: has driven for the distance and speed specified
  13  *
  14  * @author Meryem and Avi
  15  *
  16  */
  17 public class DriveDistance extends Command {
  18   private double maxTimeOut;
  19   double distance;
  20   int count = 0;
  21
  22   public DriveDistance(double distance, double maxTimeOut) {
  23     requires(Robot.driveTrain);
  24     this.maxTimeOut = maxTimeOut;
  25     this.distance = distance;
  26   }
  27
  28   @Override
  29   protected void initialize() {
  30     Robot.driveTrain.resetEncoders();
  31   }
  32
  33   @Override
  34   protected void execute() {
  35     Robot.driveTrain.driveDistance(distance, maxTimeOut);
  36
  37   }
  38
  39   @Override
  40   protected boolean isFinished() {
  41     if (timeSinceInitialized() >= maxTimeOut
  42         || Robot.driveTrain
  43             .reachedTarget() || Robot.driveTrain.getError() < 1) {
  44       System.out.println("time: " + timeSinceInitialized());
  45       Constants.DriveTrain.time = timeSinceInitialized();
  46       return true;
  47     }
  48     return false;
  49   }
  50
  51   @Override
  52   protected void end() {
  53     Robot.driveTrain.disable();
  54   }
  55
  56   @Override
  57   protected void interrupted() {
  58     end();
  59   }
  60 }