1 package org
.usfirst
.frc
.team3501
.robot
.commands
.defensearm
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import edu
.wpi
.first
.wpilibj
.command
.Command
;
7 public class MoveIntakeArmToAngle
extends Command
{
8 private double currentAngle
;
9 private double targetAngle
;
10 private double targetSpeed
;
11 private double SENSITIVITY_THRESHOLD
= 0.1;
12 private boolean isDecreasing
= false;
14 public MoveIntakeArmToAngle(double angle
, double speed
) {
15 requires(Robot
.intakeArm
);
22 protected void initialize() {
23 currentAngle
= Robot
.intakeArm
.getArmAngle();
24 double difference
= targetAngle
- currentAngle
;
26 Robot
.intakeArm
.setArmSpeed(targetSpeed
);
29 Robot
.intakeArm
.setArmSpeed(-targetSpeed
);
35 protected void execute() {
40 protected boolean isFinished() {
41 currentAngle
= Robot
.intakeArm
.getArmAngle();
43 if (isDecreasing
== true) {
44 return (currentAngle
<= targetAngle
+ SENSITIVITY_THRESHOLD
);
46 return (currentAngle
>= targetAngle
+ SENSITIVITY_THRESHOLD
);
51 protected void end() {
52 Robot
.intakeArm
.stop();
56 protected void interrupted() {