dd4289748525c80ab058b4e13ddd96e69b08f686
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 SetHandToLevel
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 SetHandToLevel(double speed
, double targetPosition
) {
15 requires(Robot
.defenseArm
);
18 this.targetPosition
= targetPosition
;
22 protected void initialize() {
23 currentPosition
= Robot
.defenseArm
.getHandPotAngle();
25 if (currentPosition
> targetPosition
) {
26 Robot
.defenseArm
.setHandSpeed(-speed
);
29 Robot
.defenseArm
.setHandSpeed(speed
);
36 protected void execute() {
40 protected boolean isFinished() {
41 currentPosition
= Robot
.defenseArm
.getHandPotAngle();
43 if (isDecreasing
== true) {
44 return (currentPosition
<= targetPosition
+ THRESHOLD
);
46 return (currentPosition
>= targetPosition
- THRESHOLD
);
51 protected void end() {
52 Robot
.defenseArm
.setHandSpeed(0);
56 protected void interrupted() {