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  * @param angle
  10  *          is the setpoint we want to turn for
  11  *          maxTimeOut catch just in case robot malfunctions and never reaches
  12  *          setpoint
  13  * @initialize resets the Gyro and prints the current mode
  14  * @code repeatedly sets a new setpoint angle to the motor
  15  * @end ends when the setpoint is reached.
  16  */
  17 public class TurnForAngle extends Command {
  18   private double maxTimeOut;
  19   double angle;
  20
  21   public TurnForAngle(double angle, double maxTimeOut) {
  22     requires(Robot.driveTrain);
  23     this.maxTimeOut = maxTimeOut;
  24     this.angle = angle;
  25   }
  26
  27   @Override
  28   protected void initialize() {
  29     Robot.driveTrain.resetGyro();
  30     System.out.println(Robot.driveTrain.getMode());
  31   }
  32
  33   @Override
  34   protected void execute() {
  35     Robot.driveTrain.turnAngle(angle);
  36     Robot.driveTrain.printGyroOutput();
  37     Robot.driveTrain.printOutput();
  38
  39   }
  40
  41   @Override
  42   protected boolean isFinished() {
  43     if (timeSinceInitialized() >= maxTimeOut
  44         || Robot.driveTrain
  45         .reachedTarget() || Robot.driveTrain.getError() < 8) {
  46       System.out.println("time: " + timeSinceInitialized());
  47       Constants.DriveTrain.time = timeSinceInitialized();
  48       return true;
  49     }
  50     return false;
  51   }
  52
  53   @Override
  54   protected void end() {
  55     Robot.driveTrain.disable();
  56   }
  57
  58   @Override
  59   protected void interrupted() {
  60     end();
  61   }
  62 }