challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToHeight.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 MoveIntakeArmToHeight extends Command {
   8         private double targetHeight;
   9         private double currentHeight;
  10         private double targetSpeed;
  11         private double SENSITIVITY_THRESHOLD = 0.1;
  12         private double NO_MOVEMENT_SPEED = 0;
  13
  14         public MoveIntakeArmToHeight(double height, double speed) {
  15                 requires(Robot.intakeArm);
  16                 targetHeight = height;
  17                 targetSpeed = speed;
  18         }
  19
  20         @Override
  21         protected void initialize() {
  22                 currentHeight = Robot.intakeArm.getArmHeight();
  23                 double difference = targetHeight - currentHeight;
  24                 if (difference > 0) {
  25                         Robot.intakeArm.setArmVoltage(targetSpeed);
  26                 } else {
  27                         Robot.intakeArm.setArmVoltage(-targetSpeed);
  28                 }
  29         }
  30
  31         @Override
  32         protected void execute() {
  33
  34         }
  35
  36         @Override
  37         protected boolean isFinished() {
  38                 currentHeight = Robot.intakeArm.getArmHeight();
  39                 double distance = Math.abs(currentHeight - targetHeight);
  40                 if (distance <= SENSITIVITY_THRESHOLD) {
  41                         return true;
  42                 } else {
  43                         return false;
  44                 }
  45
  46         }
  47
  48         @Override
  49         protected void end() {
  50                 Robot.intakeArm.setArmVoltage(NO_MOVEMENT_SPEED);
  51         }
  52
  53         @Override
  54         protected void interrupted() {
  55         }
  56 }