challenge-bot.com Git - 3501/2015-FRC-Spark/blob - src/org/usfirst/frc3501/RiceCatRobot/commands/DriveForTime.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 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 takes a time in seconds which is how long it should run
  12  *
  13  */
  14 public class DriveForTime extends Command {
  15   private double seconds;
  16   private double speed;
  17   private Timer timer;
  18   private Direction direction;
  19
  20   public DriveForTime(double seconds, double speed, Direction direction) {
  21     // limit speed to the range [0, MOTOR_MAX_VAL]
  22     this.speed = Math.max(speed, -speed);
  23     this.speed = Math.min(speed, DriveTrain.MOTOR_MAX_VAL);
  24
  25     this.seconds = seconds;
  26     this.direction = direction;
  27   }
  28
  29   @Override
  30   protected void initialize() {
  31     timer = new Timer();
  32     timer.reset();
  33     timer.start();
  34   }
  35
  36   @Override
  37   protected void execute() {
  38     if (direction == Direction.FORWARD) {
  39       Robot.driveTrain.setMotorSpeeds(-speed, -speed);
  40     } else if (direction == Direction.BACKWARD) {
  41       Robot.driveTrain.setMotorSpeeds(speed, speed);
  42     }
  43   }
  44
  45   @Override
  46   protected boolean isFinished() {
  47     return timer.get() > seconds;
  48   }
  49
  50   @Override
  51   protected void end() {
  52     Robot.driveTrain.setMotorSpeeds(0, 0);
  53   }
  54
  55   @Override
  56   protected void interrupted() {
  57     end();
  58   }
  59 }