Commit | Line | Data |
---|---|---|
33141cdd KZ |
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 | } |