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
;
13 public SetHandToLevel(double speed
, int level
) {
14 requires(Robot
.defenseArm
);
17 this.targetPosition
= Robot
.defenseArm
.getAngleForHandLocation(level
);
21 protected void initialize() {
22 currentPosition
= Robot
.defenseArm
.getHandPotAngle();
24 if (currentPosition
> targetPosition
) {
25 Robot
.defenseArm
.setHandSpeed(-speed
);
27 Robot
.defenseArm
.setHandSpeed(speed
);
33 protected void execute() {
37 protected boolean isFinished() {
38 currentPosition
= Robot
.defenseArm
.getHandPotAngle();
40 double difference
= Math
.abs(currentPosition
- targetPosition
);
41 return (difference
<= THRESHOLD
);
45 protected void end() {
46 Robot
.defenseArm
.setHandSpeed(0);
50 protected void interrupted() {