import edu.wpi.first.wpilibj.command.Command;
public class SetHandToAngle extends Command {
+ private static final double THRESHOLD = 0.1;
+ private double speed;
+ private double targetPosition;
+ private double currentPosition;
+ private boolean isDecreasing = false;
- public SetHandToAngle() {
+ public SetHandToAngle(double speed, double targetPosition) {
requires(Robot.defenseArm);
+
+ this.speed = speed;
+ this.targetPosition = targetPosition;
}
@Override
protected void initialize() {
+ currentPosition = Robot.defenseArm.getHandPotAngle();
+
+ if (currentPosition > targetPosition) {
+ Robot.defenseArm.setHandSpeed(-speed);
+ isDecreasing = true;
+ } else {
+ Robot.defenseArm.setHandSpeed(speed);
+ isDecreasing = false;
+ }
}
@Override
protected boolean isFinished() {
- return false;
+ currentPosition = Robot.defenseArm.getHandPotAngle();
+
+ if (isDecreasing == true) {
+ return (currentPosition <= targetPosition + THRESHOLD);
+ } else {
+ return (currentPosition >= targetPosition - THRESHOLD);
+ }
}
@Override
protected void end() {
+ Robot.defenseArm.setHandSpeed(0);
}
@Override