package org.usfirst.frc3501.RiceCatRobot.commands;
-import org.usfirst.frc3501.RiceCatRobot.Robot;
-import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
+import org.usfirst.frc3501.RiceCatRobot.robot.Robot;
+import org.usfirst.frc3501.RiceCatRobot.robot.RobotMap.Direction;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Command;
*
*/
public class DriveFor extends Command {
- private double seconds;
- private Timer timer;
- private Direction direction;
+ private double seconds;
+ private Timer timer;
+ private Direction direction;
- public DriveFor(double seconds, Direction direction) {
- this.seconds = seconds;
- this.direction = direction;
+ public DriveFor(double seconds, Direction direction) {
+ this.seconds = seconds;
+ this.direction = direction;
- }
+ }
- @Override
- protected void initialize() {
- timer = new Timer();
- timer.reset();
- timer.start();
- }
+ @Override
+ protected void initialize() {
+ timer = new Timer();
+ timer.reset();
+ timer.start();
+ }
- @Override
- protected void execute() {
- System.out.println(timer.get());
- if (direction == Direction.FORWARD) {
- if (timer.get() < seconds * 0.2) { // for the first 20% of time, run
- // the robot at -.5 speed
- Robot.driveTrain.arcadeDrive(-0.3, 0);
- } else if (timer.get() >= seconds * 0.2
- && timer.get() <= seconds * 0.8) { // for the +20% - 75%
- // time, move the robot
- // at -.3 speed.
- Robot.driveTrain.arcadeDrive(-0.5, 0);
- } else if (timer.get() < seconds) {
- Robot.driveTrain.arcadeDrive(-0.25, 0);
- } else {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
- } else if (direction == Direction.BACKWARD) {
- if (timer.get() < seconds * 0.2) {
- Robot.driveTrain.arcadeDrive(0.3, 0);
- } else if (timer.get() >= seconds * 0.2
- && timer.get() <= seconds * 0.8) {
- Robot.driveTrain.arcadeDrive(0.5, 0);
- } else if (timer.get() < seconds) {
- Robot.driveTrain.arcadeDrive(0.25, 0);
- } else {
- Robot.driveTrain.arcadeDrive(0, 0);
- }
- }
- }
-
- @Override
- protected boolean isFinished() {
- return timer.get() > seconds;
- }
-
- @Override
- protected void end() {
+ @Override
+ protected void execute() {
+ System.out.println(timer.get());
+ if (direction == Direction.FORWARD) {
+ if (timer.get() < seconds * 0.2) { // for the first 20% of time, run
+ // the robot at -.5 speed
+ Robot.driveTrain.arcadeDrive(-0.3, 0);
+ } else if (timer.get() >= seconds * 0.2 && timer.get() <= seconds * 0.8) {
+ Robot.driveTrain.arcadeDrive(-0.5, 0);
+ } else if (timer.get() < seconds) {
+ Robot.driveTrain.arcadeDrive(-0.25, 0);
+ } else {
+ Robot.driveTrain.arcadeDrive(0, 0);
+ }
+ } else if (direction == Direction.BACKWARD) {
+ if (timer.get() < seconds * 0.2) {
+ Robot.driveTrain.arcadeDrive(0.3, 0);
+ } else if (timer.get() >= seconds * 0.2 && timer.get() <= seconds * 0.8) {
+ Robot.driveTrain.arcadeDrive(0.5, 0);
+ } else if (timer.get() < seconds) {
+ Robot.driveTrain.arcadeDrive(0.25, 0);
+ } else {
Robot.driveTrain.arcadeDrive(0, 0);
+ }
}
+ }
- @Override
- protected void interrupted() {
- end();
- }
+ @Override
+ protected boolean isFinished() {
+ return timer.get() > seconds;
+ }
+
+ @Override
+ protected void end() {
+ Robot.driveTrain.arcadeDrive(0, 0);
+ }
-}
\ No newline at end of file
+ @Override
+ protected void interrupted() {
+ end();
+ }
+}