1 package org
.usfirst
.frc
.team3501
.robot
.commands
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
5 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
10 public class MoveDefenseArmVertical
extends CommandGroup
{
12 double horizontalLimit
, height
;
14 public MoveDefenseArmVertical(double horizontalLimit
, double height
) {
15 this.horizontalLimit
= horizontalLimit
;
19 protected double calculateTargetArmAngle() {
21 armAngle
= square(horizontalLimit
) + square(height
)
22 + square(Constants
.DefenseArm
.ARM_LENGTH
)
23 - square(Constants
.DefenseArm
.HAND_LENGTH
);
24 armAngle
/= 2 * Math
.sqrt(square(horizontalLimit
) + square(height
))
25 * Constants
.DefenseArm
.ARM_LENGTH
;
26 armAngle
= Math
.acos(armAngle
);
27 armAngle
= Math
.atan(height
/ horizontalLimit
) - armAngle
;
31 protected double calculateTargetHandAngle() {
33 handAngle
= square(horizontalLimit
) + square(height
)
34 + square(Constants
.DefenseArm
.HAND_LENGTH
)
35 - square(Constants
.DefenseArm
.ARM_LENGTH
);
36 handAngle
/= 2 * Math
.sqrt(square(horizontalLimit
) + square(height
))
37 * Constants
.DefenseArm
.HAND_LENGTH
;
38 handAngle
= Math
.acos(handAngle
);
39 handAngle
= handAngle
+ 90 - Math
.atan(horizontalLimit
/ height
);
43 // Make this return true when this Command no longer needs to run execute()
45 protected boolean isFinished() {
49 // Called once after isFinished returns true
51 protected void end() {
54 public double square(double num
) {
58 // Called when another command which requires one or more of the same
59 // subsystems is scheduled to run
61 protected void interrupted() {