1 package org
.usfirst
.frc
.team3501
.robot
.commands
.driving
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
4 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
6 import edu
.wpi
.first
.wpilibj
.command
.Command
;
9 * This command will drive the drivetrain a certain distance in inches
12 * is the distance we want to drive
13 * maxTimeOut is a catch just in case the robot malfunctions and never
14 * gets to the setpoint
17 * Repeatedly updates the driveTrain setpoint
18 * Finishes when the time goes over maxTimeOut or the driveTrain hits the
20 * end() disables the PID driveTrain
22 public class DriveDistance
extends Command
{
23 private double maxTimeOut
;
27 public DriveDistance(double distance
, double maxTimeOut
) {
28 requires(Robot
.driveTrain
);
29 this.maxTimeOut
= maxTimeOut
;
30 this.distance
= distance
;
34 protected void initialize() {
35 Robot
.driveTrain
.resetEncoders();
39 protected void execute() {
40 Robot
.driveTrain
.driveDistance(distance
, maxTimeOut
);
45 protected boolean isFinished() {
46 if (timeSinceInitialized() >= maxTimeOut
48 .reachedTarget() || Robot
.driveTrain
.getError() < 1) {
49 System
.out
.println("time: " + timeSinceInitialized());
50 Constants
.DriveTrain
.time
= timeSinceInitialized();
57 protected void end() {
58 Robot
.driveTrain
.disable();
62 protected void interrupted() {