challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/SetHandToLevel.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 public class SetHandToLevel extends Command {
   8   private static final double THRESHOLD = 0.1;
   9   private double speed;
  10   private double targetPosition;
  11   private double currentPosition;
  12   private boolean isDecreasing = false;
  13
  14   public SetHandToLevel(double speed, int level) {
  15     requires(Robot.defenseArm);
  16
  17     this.speed = speed;
  18     this.targetPosition = Robot.defenseArm.getAngleForLocation(level);
  19   }
  20
  21   @Override
  22   protected void initialize() {
  23     currentPosition = Robot.defenseArm.getHandPotAngle();
  24
  25     if (currentPosition > targetPosition) {
  26       Robot.defenseArm.setHandSpeed(-speed);
  27       isDecreasing = true;
  28     } else {
  29       Robot.defenseArm.setHandSpeed(speed);
  30       isDecreasing = false;
  31     }
  32
  33   }
  34
  35   @Override
  36   protected void execute() {
  37   }
  38
  39   @Override
  40   protected boolean isFinished() {
  41     currentPosition = Robot.defenseArm.getHandPotAngle();
  42
  43     if (isDecreasing == true) {
  44       return (currentPosition <= targetPosition + THRESHOLD);
  45     } else {
  46       return (currentPosition >= targetPosition - THRESHOLD);
  47     }
  48   }
  49
  50   @Override
  51   protected void end() {
  52     Robot.defenseArm.setHandSpeed(0);
  53   }
  54
  55   @Override
  56   protected void interrupted() {
  57     end();
  58   }
  59
  60 }