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