1 package org
.usfirst
.frc3501
.RiceCatRobot
.commands
;
3 import org
.usfirst
.frc3501
.RiceCatRobot
.robot
.Robot
;
4 import org
.usfirst
.frc3501
.RiceCatRobot
.robot
.RobotMap
.Direction
;
6 import edu
.wpi
.first
.wpilibj
.Timer
;
7 import edu
.wpi
.first
.wpilibj
.command
.Command
;
10 * This command takes a time in seconds which is how long it should run
13 public class DriveFor
extends Command
{
14 private double seconds
;
16 private Direction direction
;
18 public DriveFor(double seconds
, Direction direction
) {
19 this.seconds
= seconds
;
20 this.direction
= direction
;
25 protected void initialize() {
32 protected void execute() {
33 System
.out
.println(timer
.get());
34 if (direction
== Direction
.FORWARD
) {
35 if (timer
.get() < seconds
* 0.2) { // for the first 20% of time, run
36 // the robot at -.5 speed
37 Robot
.driveTrain
.arcadeDrive(-0.3, 0);
38 } else if (timer
.get() >= seconds
* 0.2 && timer
.get() <= seconds
* 0.8) {
39 Robot
.driveTrain
.arcadeDrive(-0.5, 0);
40 } else if (timer
.get() < seconds
) {
41 Robot
.driveTrain
.arcadeDrive(-0.25, 0);
43 Robot
.driveTrain
.arcadeDrive(0, 0);
45 } else if (direction
== Direction
.BACKWARD
) {
46 if (timer
.get() < seconds
* 0.2) {
47 Robot
.driveTrain
.arcadeDrive(0.3, 0);
48 } else if (timer
.get() >= seconds
* 0.2 && timer
.get() <= seconds
* 0.8) {
49 Robot
.driveTrain
.arcadeDrive(0.5, 0);
50 } else if (timer
.get() < seconds
) {
51 Robot
.driveTrain
.arcadeDrive(0.25, 0);
53 Robot
.driveTrain
.arcadeDrive(0, 0);
59 protected boolean isFinished() {
60 return timer
.get() > seconds
;
64 protected void end() {
65 Robot
.driveTrain
.arcadeDrive(0, 0);
69 protected void interrupted() {