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.DeadReckoning;
   4 import org.usfirst.frc.team3501.robot.Constants.Direction;
   5 import org.usfirst.frc.team3501.robot.Robot;
   6
   7 import edu.wpi.first.wpilibj.Timer;
   8 import edu.wpi.first.wpilibj.command.Command;
   9
  10 /***
  11  * This command turns the robot in a specified direction for a specified
  12  * duration in seconds.
  13  *
  14  * pre-condition: robot is on a flat surface
  15  *
  16  * post-condition: robot has turned in the specified direction for the specified
  17  * time
  18  *
  19  * TODO: test for speed/ time constants for specific angles (ex. 30 degrees, 60
  20  * degrees, 90 degrees)
  21  *
  22  * @author Meryem, Avi, and Sarvesh
  23  *
  24  */
  25
  26 public class TurnForTime extends Command {
  27   private Direction direction;
  28   private double seconds;
  29   private Timer timer;
  30   private double speed;
  31
  32   public TurnForTime(double seconds, Direction direction, double speed) {
  33     this.seconds = seconds;
  34     this.direction = direction;
  35     this.speed = speed;
  36   }
  37
  38   public TurnForTime(double seconds, Direction direction) {
  39     this(seconds, direction, DeadReckoning.DEFAULT_SPEED);
  40   }
  41
  42   @Override
  43   protected void initialize() {
  44     timer = new Timer();
  45     timer.start();
  46
  47     if (direction == Direction.RIGHT) {
  48       Robot.driveTrain.drive(speed, -speed);
  49     } else if (direction == Direction.LEFT) {
  50       Robot.driveTrain.drive(-speed, speed);
  51     }
  52   }
  53
  54   @Override
  55   protected void execute() {
  56
  57   }
  58
  59   @Override
  60   protected boolean isFinished() {
  61     return (timer.get() >= seconds);
  62   }
  63
  64   @Override
  65   protected void end() {
  66     Robot.driveTrain.drive(0, 0);
  67   }
  68
  69   @Override
  70   protected void interrupted() {
  71     end();
  72   }
  73 }