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 static final double ARM_SPEED
= 0.6;
16 public MoveDefenseArmVertical(double horizontalLimit
, double height
) {
17 this.horizontalLimit
= horizontalLimit
;
19 double targetArmAngle
= calculateTargetArmAngle();
20 double targetHandAngle
= calculateTargetHandAngle();
21 double handSpeed
= ARM_SPEED
* targetHandAngle
/ targetArmAngle
;
22 addParallel(new SetArmToAngle(ARM_SPEED
, calculateTargetArmAngle()));
23 addParallel(new SetHandToAngle(handSpeed
, calculateTargetHandAngle()));
26 protected double calculateTargetArmAngle() {
28 armAngle
= square(horizontalLimit
) + square(height
)
29 + square(Constants
.DefenseArm
.ARM_LENGTH
)
30 - square(Constants
.DefenseArm
.HAND_LENGTH
);
31 armAngle
/= 2 * Math
.sqrt(square(horizontalLimit
) + square(height
))
32 * Constants
.DefenseArm
.ARM_LENGTH
;
33 armAngle
= Math
.acos(armAngle
);
34 armAngle
= Math
.atan(height
/ horizontalLimit
) - armAngle
;
38 protected double calculateTargetHandAngle() {
40 handAngle
= square(horizontalLimit
) + square(height
)
41 + square(Constants
.DefenseArm
.HAND_LENGTH
)
42 - square(Constants
.DefenseArm
.ARM_LENGTH
);
43 handAngle
/= 2 * Math
.sqrt(square(horizontalLimit
) + square(height
))
44 * Constants
.DefenseArm
.HAND_LENGTH
;
45 handAngle
= Math
.acos(handAngle
);
46 handAngle
= handAngle
+ 90 - Math
.atan(horizontalLimit
/ height
);
50 public double square(double num
) {