1 package org
.usfirst
.frc
.team3501
.robot
.commands
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
5 import edu
.wpi
.first
.wpilibj
.command
.Command
;
10 public class MoveDefenseArmVertical
extends Command
{
12 double horizontalLimit
, height
;
14 public MoveDefenseArmVertical(double horizontalLimit
, double height
) {
16 this.horizontalLimit
= horizontalLimit
;
20 // Called just before this Command runs the first time
22 protected void initialize() {
25 // Called repeatedly when this Command is scheduled to run
27 protected void execute() {
29 armAngle
= square(horizontalLimit
) + square(height
)
30 + square(Constants
.DefenseArm
.ARM_LENGTH
)
31 - square(Constants
.DefenseArm
.HAND_LENGTH
);
32 armAngle
/= 2 * Math
.sqrt(square(horizontalLimit
) + square(height
))
33 * Constants
.DefenseArm
.ARM_LENGTH
;
34 armAngle
= Math
.acos(armAngle
);
35 armAngle
= Math
.atan(height
/ horizontalLimit
) - armAngle
;
38 handAngle
= square(horizontalLimit
) + square(height
)
39 + square(Constants
.DefenseArm
.HAND_LENGTH
)
40 - square(Constants
.DefenseArm
.ARM_LENGTH
);
41 handAngle
/= 2 * Math
.sqrt(square(horizontalLimit
) + square(height
))
42 * Constants
.DefenseArm
.HAND_LENGTH
;
43 handAngle
= Math
.acos(handAngle
);
44 handAngle
= handAngle
+ 90 - Math
.atan(horizontalLimit
/ height
);
47 // Make this return true when this Command no longer needs to run execute()
49 protected boolean isFinished() {
53 // Called once after isFinished returns true
55 protected void end() {
58 public double square(double num
) {
62 // Called when another command which requires one or more of the same
63 // subsystems is scheduled to run
65 protected void interrupted() {