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 | /*** | |
1c94f230 | 9 | * This command will drive the drivetrain a certain distance in inches |
33141cdd | 10 | * |
1c94f230 KZ |
11 | * @param distance |
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 | |
33141cdd | 15 | * |
1c94f230 KZ |
16 | * @code |
17 | * Repeatedly updates the driveTrain setpoint | |
18 | * Finishes when the time goes over maxTimeOut or the driveTrain hits the | |
19 | * setpoint | |
20 | * end() disables the PID driveTrain | |
33141cdd KZ |
21 | */ |
22 | public class DriveDistance extends Command { | |
23 | private double maxTimeOut; | |
24 | double distance; | |
25 | int count = 0; | |
26 | ||
27 | public DriveDistance(double distance, double maxTimeOut) { | |
28 | requires(Robot.driveTrain); | |
29 | this.maxTimeOut = maxTimeOut; | |
30 | this.distance = distance; | |
31 | } | |
32 | ||
33 | @Override | |
34 | protected void initialize() { | |
35 | Robot.driveTrain.resetEncoders(); | |
36 | } | |
37 | ||
38 | @Override | |
39 | protected void execute() { | |
40 | Robot.driveTrain.driveDistance(distance, maxTimeOut); | |
41 | ||
42 | } | |
43 | ||
44 | @Override | |
45 | protected boolean isFinished() { | |
46 | if (timeSinceInitialized() >= maxTimeOut | |
47 | || Robot.driveTrain | |
48 | .reachedTarget() || Robot.driveTrain.getError() < 1) { | |
49 | System.out.println("time: " + timeSinceInitialized()); | |
50 | Constants.DriveTrain.time = timeSinceInitialized(); | |
51 | return true; | |
52 | } | |
53 | return false; | |
54 | } | |
55 | ||
56 | @Override | |
57 | protected void end() { | |
58 | Robot.driveTrain.disable(); | |
59 | } | |
60 | ||
61 | @Override | |
62 | protected void interrupted() { | |
33141cdd KZ |
63 | } |
64 | } |