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

   1 package org.usfirst.frc.team3501.robot.commands.driving;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4 import org.usfirst.frc.team3501.robot.Robot;
   5
   6 import edu.wpi.first.wpilibj.command.Command;
   7
   8 /***
   9  * This command will move the robot at the specified speed for the specified
  10  * distance.
  11  *
  12  * post-condition: has driven for the distance and speed specified
  13  *
  14  * @author Meryem and Avi
  15  *
  16  */
  17 public class TurnForAngle extends Command {
  18   private double maxTimeOut;
  19   double angle;
  20   int count = 0;
  21
  22   public TurnForAngle(double angle, double maxTimeOut) {
  23     requires(Robot.driveTrain);
  24     this.maxTimeOut = maxTimeOut;
  25     this.angle = angle;
  26   }
  27
  28   @Override
  29   protected void initialize() {
  30     Robot.driveTrain.resetGyro();
  31     System.out.println(Robot.driveTrain.getMode());
  32   }
  33
  34   @Override
  35   protected void execute() {
  36     Robot.driveTrain.turnAngle(angle);
  37     Robot.driveTrain.printGyroOutput();
  38     Robot.driveTrain.printOutput();
  39     count++;
  40
  41   }
  42
  43   @Override
  44   protected boolean isFinished() {
  45     if (timeSinceInitialized() >= maxTimeOut
  46         || Robot.driveTrain
  47         .reachedTarget() || Robot.driveTrain.getError() < 8) {
  48       System.out.println("time: " + timeSinceInitialized());
  49       Constants.DriveTrain.time = timeSinceInitialized();
  50       return true;
  51     }
  52     return false;
  53   }
  54
  55   @Override
  56   protected void end() {
  57     Robot.driveTrain.disable();
  58   }
  59
  60   @Override
  61   protected void interrupted() {
  62     end();
  63   }
  64 }