1 package org
.usfirst
.frc
.team3501
.robot
.commands
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import edu
.wpi
.first
.wpilibj
.command
.Command
;
7 public class SetArmToAngle
extends Command
{
8 private static final double THRESHOLD
= 0.1;
10 private double targetPosition
;
11 private double currentPosition
;
12 private boolean isDecreasing
= false;
14 public SetArmToAngle(double speed
, double targetPosition
) {
15 requires(Robot
.defenseArm
);
18 this.targetPosition
= targetPosition
;
22 public void initialize() {
23 currentPosition
= Robot
.defenseArm
.getArmPotAngle();
25 if (currentPosition
> targetPosition
) {
26 Robot
.defenseArm
.setArmSpeed(-speed
);
29 Robot
.defenseArm
.setArmSpeed(speed
);
35 public void execute() {
40 public boolean isFinished() {
41 currentPosition
= Robot
.defenseArm
.getArmPotAngle();
43 if (isDecreasing
== true) {
44 return (currentPosition
<= targetPosition
+ THRESHOLD
);
46 return (currentPosition
>= targetPosition
- THRESHOLD
);
52 Robot
.defenseArm
.setArmSpeed(0);
56 protected void interrupted() {