1 package org
.usfirst
.frc
.team3501
.robot
.commands
.intakearm
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import edu
.wpi
.first
.wpilibj
.command
.Command
;
7 public class MoveIntakeArmToAngle
extends Command
{
8 private double currentAngle
;
9 private double targetAngle
;
10 private double targetSpeed
;
11 private double calculatedSpeed
;
12 private double SENSITIVITY_THRESHOLD
= 0.1;
13 private double MIN_SPEED
= .3;
15 public MoveIntakeArmToAngle(double angle
, double speed
) {
16 requires(Robot
.intakeArm
);
23 protected void initialize() {
25 // set the arm speed to the calculated angle
26 double speed
= getCalculatedSpeed();
28 if (speed
< MIN_SPEED
&& speed
> 0)
29 speed
= Math
.signum(getCalculatedSpeed()) * MIN_SPEED
;
30 Robot
.intakeArm
.setArmSpeed(speed
);
34 protected void execute() {
35 // set the arm speed to the calculated angle
36 Robot
.intakeArm
.setArmSpeed(calculatedSpeed
);
39 private double getError() {
40 // targetAngle - currentAngle = error
41 return Robot
.intakeArm
.getArmAngle() - targetAngle
;
44 private double getCalculatedSpeed() {
45 // if the arm has to move up -- the speed will be +
46 // if the arm has to move down -- the speed will be -
48 return (getError() / targetAngle
) * targetSpeed
;
52 protected boolean isFinished() {
53 // if the arm's angle is close enough to the target value return true
55 return (Math
.abs(getError()) <= SENSITIVITY_THRESHOLD
);
59 protected void end() {
60 Robot
.intakeArm
.stop();
64 protected void interrupted() {