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

   1 package org.usfirst.frc.team3501.robot.commands;
   2
   3 import org.usfirst.frc.team3501.robot.Robot;
   4
   5 import edu.wpi.first.wpilibj.command.Command;
   6
   7 /***
   8  * This command moves the defense arm to a input angle position.
   9  * Requires input of a targetPosition and a speed.
  10  *
  11  * @author shaina
  12  *
  13  */
  14 public class SetHandToAngle extends Command {
  15   private static final double THRESHOLD = 0.1;
  16   private double speed;
  17   private double targetPosition;
  18   private double currentPosition;
  19   private boolean isDecreasing = false;
  20
  21   public SetHandToAngle(double speed, double targetPosition) {
  22     requires(Robot.defenseArm);
  23
  24     this.speed = speed;
  25     this.targetPosition = targetPosition;
  26   }
  27
  28   @Override
  29   public void initialize() {
  30     currentPosition = Robot.defenseArm.getHandPotAngle();
  31
  32     if (currentPosition > targetPosition) {
  33       Robot.defenseArm.setArmSpeed(-speed);
  34       isDecreasing = true;
  35     } else {
  36       Robot.defenseArm.setArmSpeed(speed);
  37       isDecreasing = false;
  38     }
  39   }
  40
  41   @Override
  42   public void execute() {
  43
  44   }
  45
  46   @Override
  47   public boolean isFinished() {
  48     currentPosition = Robot.defenseArm.getHandPotAngle();
  49
  50     if (isDecreasing == true) {
  51       return (currentPosition <= targetPosition + THRESHOLD);
  52     } else {
  53       return (currentPosition >= targetPosition - THRESHOLD);
  54     }
  55   }
  56
  57   @Override
  58   public void end() {
  59     Robot.defenseArm.setArmSpeed(0);
  60   }
  61
  62   @Override
  63   protected void interrupted() {
  64     end();
  65   }
  66
  67 }