Commit | Line | Data |
---|---|---|
59cf03f0 D |
1 | package org.usfirst.frc3501.RiceCatRobot.commands; |
2 | ||
3 | import org.usfirst.frc3501.RiceCatRobot.Robot; | |
4 | import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction; | |
5 | import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain; | |
6 | ||
7 | import edu.wpi.first.wpilibj.Timer; | |
8 | import edu.wpi.first.wpilibj.command.Command; | |
9 | ||
10 | /** | |
11 | * This command will drive the specified distance at specified speed. | |
12 | * | |
13 | */ | |
14 | public class DriveDistance extends Command { | |
15 | private double distance; // in units of cm | |
16 | private double speed; | |
17 | ||
18 | public DriveDistance(double distance, double speed) { | |
19 | // Since negative values in setMotorSpeeds(...) are forwards, we reverse | |
20 | // speed here so positive input values will move the robot forwards. | |
21 | this.speed = -speed; | |
79e4711d D |
22 | |
23 | // ensure distance is positive | |
24 | this.distance = Math.max(distance, -distance); | |
59cf03f0 D |
25 | } |
26 | ||
27 | @Override | |
28 | protected void initialize() { | |
29 | Robot.driveTrain.resetEncoders(); | |
30 | Robot.driveTrain.setMotorSpeeds(speed, speed); | |
31 | } | |
32 | ||
33 | @Override | |
34 | protected void execute() { | |
35 | // nothing to do since motor speeds already set | |
36 | } | |
37 | ||
38 | @Override | |
39 | protected boolean isFinished() { | |
79e4711d D |
40 | return Math.abs(Robot.driveTrain.getLeftDistance()) >= distance |
41 | && Math.abs(Robot.driveTrain.getRightDistance()) >= distance; | |
59cf03f0 D |
42 | } |
43 | ||
44 | @Override | |
45 | protected void end() { | |
46 | Robot.driveTrain.stop(); | |
47 | } | |
48 | ||
49 | @Override | |
50 | protected void interrupted() { | |
51 | end(); | |
52 | } | |
53 | } |