challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java

   1 package org.usfirst.frc.team3501.robot.commands.driving;
   2
   3 import org.usfirst.frc.team3501.robot.Constants.Direction;
   4 import org.usfirst.frc.team3501.robot.Robot;
   5
   6 import edu.wpi.first.wpilibj.Timer;
   7 import edu.wpi.first.wpilibj.command.Command;
   8
   9 public class TurnForTime extends Command {
  10   private final double SPEED = 0.5;
  11   private Direction direction;
  12   private double seconds;
  13   private Timer timer;
  14
  15   public TurnForTime(double seconds, Direction direction) {
  16     this.seconds = seconds;
  17     this.direction = direction;
  18   }
  19
  20   @Override
  21   protected void initialize() {
  22     timer = new Timer();
  23     timer.start();
  24   }
  25
  26   @Override
  27   protected void execute() {
  28
  29     if (direction == Direction.RIGHT) {
  30       Robot.driveTrain.setMotorSpeeds(SPEED, -SPEED);
  31     } else if (direction == Direction.LEFT) {
  32       Robot.driveTrain.setMotorSpeeds(-SPEED, SPEED);
  33     }
  34   }
  35
  36   @Override
  37   protected boolean isFinished() {
  38     if (timer.get() >= seconds)
  39       return true;
  40     return false;
  41   }
  42
  43   @Override
  44   protected void end() {
  45   }
  46
  47   @Override
  48   protected void interrupted() {
  49   }
  50 }