Commit | Line | Data |
---|---|---|
9b93bff0 CZ |
1 | package org.usfirst.frc.team3501.robot.commands; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Constants; | |
4 | ||
c5f13b9e | 5 | import edu.wpi.first.wpilibj.command.CommandGroup; |
9b93bff0 CZ |
6 | |
7 | /** | |
8 | * | |
9 | */ | |
c5f13b9e | 10 | public class MoveDefenseArmVertical extends CommandGroup { |
9b93bff0 CZ |
11 | |
12 | double horizontalLimit, height; | |
13 | ||
14 | public MoveDefenseArmVertical(double horizontalLimit, double height) { | |
9b93bff0 CZ |
15 | this.horizontalLimit = horizontalLimit; |
16 | this.height = height; | |
17 | } | |
18 | ||
c5f13b9e | 19 | protected double calculateTargetArmAngle() { |
9b93bff0 CZ |
20 | double armAngle; |
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; | |
c5f13b9e CZ |
28 | return armAngle; |
29 | } | |
9b93bff0 | 30 | |
c5f13b9e | 31 | protected double calculateTargetHandAngle() { |
9b93bff0 CZ |
32 | double handAngle; |
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); | |
c5f13b9e | 40 | return handAngle; |
9b93bff0 CZ |
41 | } |
42 | ||
43 | // Make this return true when this Command no longer needs to run execute() | |
44 | @Override | |
45 | protected boolean isFinished() { | |
46 | return false; | |
47 | } | |
48 | ||
49 | // Called once after isFinished returns true | |
50 | @Override | |
51 | protected void end() { | |
52 | } | |
53 | ||
54 | public double square(double num) { | |
55 | return num * num; | |
56 | } | |
57 | ||
58 | // Called when another command which requires one or more of the same | |
59 | // subsystems is scheduled to run | |
60 | @Override | |
61 | protected void interrupted() { | |
62 | } | |
63 | } |