75ae227b9e8bfb6602e779e00956ecddbc36b580
1 package org
.usfirst
.frc3501
.RiceCatRobot
.commands
;
3 import org
.usfirst
.frc3501
.RiceCatRobot
.Robot
;
4 import org
.usfirst
.frc3501
.RiceCatRobot
.RobotMap
.Direction
;
5 import org
.usfirst
.frc3501
.RiceCatRobot
.subsystems
.DriveTrain
;
7 import edu
.wpi
.first
.wpilibj
.Timer
;
8 import edu
.wpi
.first
.wpilibj
.command
.Command
;
11 * This command will drive the specified distance at specified speed.
14 public class DriveDistance
extends Command
{
15 private double distance
; // in units of cm
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.
22 this.distance
= distance
;
26 protected void initialize() {
27 Robot
.driveTrain
.resetEncoders();
28 Robot
.driveTrain
.setMotorSpeeds(speed
, speed
);
32 protected void execute() {
33 // nothing to do since motor speeds already set
37 protected boolean isFinished() {
38 return Robot
.driveTrain
.getLeftDistance() >= distance
39 && Robot
.driveTrain
.getRightDistance() >= distance
;
43 protected void end() {
44 Robot
.driveTrain
.stop();
48 protected void interrupted() {