challenge-bot.com Git - 3501/2015-FRC-Spark/blob - src/org/usfirst/frc3501/RiceCatRobot/commands/DriveFor.java

   1 package org.usfirst.frc3501.RiceCatRobot.commands;
   2
   3 import org.usfirst.frc3501.RiceCatRobot.Robot;
   4 import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
   5
   6 import edu.wpi.first.wpilibj.Timer;
   7 import edu.wpi.first.wpilibj.command.Command;
   8
   9 /**
  10  * This command takes a time in seconds which is how long it should run
  11  *
  12  */
  13 public class DriveFor extends Command {
  14   private double seconds;
  15   private Timer timer;
  16   private Direction direction;
  17
  18   public DriveFor(double seconds, Direction direction) {
  19     this.seconds = seconds;
  20     this.direction = direction;
  21
  22   }
  23
  24   @Override
  25   protected void initialize() {
  26     timer = new Timer();
  27     timer.reset();
  28     timer.start();
  29   }
  30
  31   @Override
  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);
  42       } else {
  43         Robot.driveTrain.arcadeDrive(0, 0);
  44       }
  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);
  52       } else {
  53         Robot.driveTrain.arcadeDrive(0, 0);
  54       }
  55     }
  56   }
  57
  58   @Override
  59   protected boolean isFinished() {
  60     return timer.get() > seconds;
  61   }
  62
  63   @Override
  64   protected void end() {
  65     Robot.driveTrain.arcadeDrive(0, 0);
  66   }
  67
  68   @Override
  69   protected void interrupted() {
  70     end();
  71   }
  72 }