98f306a57e29f892e669c4bc8fed3ac40cb304f6
1 package org
.usfirst
.frc
.team3501
.robot
.commands
.doublejointeddefensearm
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import edu
.wpi
.first
.wpilibj
.command
.Command
;
8 * This command moves the defense arm to a input angle position.
9 * Requires input of a targetPosition and a speed.
14 public class SetHandToAngle
extends Command
{
15 private static final double THRESHOLD
= 0.1;
17 private double targetPosition
;
18 private double currentPosition
;
19 private boolean isDecreasing
= false;
21 public SetHandToAngle(double speed
, double targetPosition
) {
22 requires(Robot
.defenseArm
);
25 this.targetPosition
= targetPosition
;
29 public void initialize() {
30 currentPosition
= Robot
.defenseArm
.getHandPotAngle();
32 if (currentPosition
> targetPosition
) {
33 Robot
.defenseArm
.setArmSpeed(-speed
);
36 Robot
.defenseArm
.setArmSpeed(speed
);
42 public void execute() {
47 public boolean isFinished() {
48 currentPosition
= Robot
.defenseArm
.getHandPotAngle();
50 if (isDecreasing
== true) {
51 return (currentPosition
<= targetPosition
+ THRESHOLD
);
53 return (currentPosition
>= targetPosition
- THRESHOLD
);
59 Robot
.defenseArm
.setArmSpeed(0);
63 protected void interrupted() {