| 1 | package org.usfirst.frc.team3501.robot.commands; |
| 2 | |
| 3 | import org.usfirst.frc.team3501.robot.Constants; |
| 4 | |
| 5 | import edu.wpi.first.wpilibj.command.CommandGroup; |
| 6 | |
| 7 | /** |
| 8 | * Given input target x and y positions of the tip of the defense arm apparatus: |
| 9 | * first, calculates what the angle of the arm and hand corresponding to the |
| 10 | * given x-y coordinate is |
| 11 | * next, with speed of arm set to a constant, determine what the speed of hand |
| 12 | * should be if proportional to speed of arm (with the ratio equal to the ratio |
| 13 | * between arm angle to be adjusted and hand angle to be adjusted |
| 14 | */ |
| 15 | public class MoveDefenseArm extends CommandGroup { |
| 16 | |
| 17 | double horizontalLimit, height; |
| 18 | |
| 19 | public static final double ARM_SPEED = 0.6; |
| 20 | |
| 21 | public MoveDefenseArm(double horizontalLimit, double height) { |
| 22 | this.horizontalLimit = horizontalLimit; |
| 23 | this.height = height; |
| 24 | double targetArmAngle = calculateTargetArmAngle(); |
| 25 | double targetHandAngle = calculateTargetHandAngle(); |
| 26 | double handSpeed = ARM_SPEED * targetHandAngle / targetArmAngle; |
| 27 | addParallel(new SetArmToAngle(ARM_SPEED, calculateTargetArmAngle())); |
| 28 | addParallel(new SetHandToAngle(handSpeed, calculateTargetHandAngle())); |
| 29 | } |
| 30 | |
| 31 | protected double calculateTargetArmAngle() { |
| 32 | double armAngle; |
| 33 | armAngle = square(horizontalLimit) + square(height) |
| 34 | + square(Constants.DefenseArm.ARM_LENGTH) |
| 35 | - square(Constants.DefenseArm.HAND_LENGTH); |
| 36 | armAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height)) |
| 37 | * Constants.DefenseArm.ARM_LENGTH; |
| 38 | armAngle = Math.acos(armAngle); |
| 39 | armAngle = Math.atan(height / horizontalLimit) - armAngle; |
| 40 | return armAngle; |
| 41 | } |
| 42 | |
| 43 | protected double calculateTargetHandAngle() { |
| 44 | double handAngle; |
| 45 | handAngle = square(horizontalLimit) + square(height) |
| 46 | + square(Constants.DefenseArm.HAND_LENGTH) |
| 47 | - square(Constants.DefenseArm.ARM_LENGTH); |
| 48 | handAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height)) |
| 49 | * Constants.DefenseArm.HAND_LENGTH; |
| 50 | handAngle = Math.acos(handAngle); |
| 51 | handAngle = handAngle + 90 - Math.atan(horizontalLimit / height); |
| 52 | return handAngle; |
| 53 | } |
| 54 | |
| 55 | public double square(double num) { |
| 56 | return num * num; |
| 57 | } |
| 58 | } |