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
  13   public SetHandToLevel(double speed, int level) {
  14     requires(Robot.defenseArm);
  15
  16     this.speed = speed;
  17     this.targetPosition = Robot.defenseArm.getAngleForHandLocation(level);
  18   }
  19
  20   @Override
  21   protected void initialize() {
  22     currentPosition = Robot.defenseArm.getHandPotAngle();
  23
  24     if (currentPosition > targetPosition) {
  25       Robot.defenseArm.setHandSpeed(-speed);
  26     } else {
  27       Robot.defenseArm.setHandSpeed(speed);
  28     }
  29
  30   }
  31
  32   @Override
  33   protected void execute() {
  34   }
  35
  36   @Override
  37   protected boolean isFinished() {
  38     currentPosition = Robot.defenseArm.getHandPotAngle();
  39
  40     double difference = Math.abs(currentPosition - targetPosition);
  41     return (difference <= THRESHOLD);
  42   }
  43
  44   @Override
  45   protected void end() {
  46     Robot.defenseArm.setHandSpeed(0);
  47   }
  48
  49   @Override
  50   protected void interrupted() {
  51     end();
  52   }
  53
  54 }