-package org.usfirst.frc.team3501.robot.commands;
-
-import org.usfirst.frc.team3501.robot.Constants;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-/**
- *
- */
-public class MoveDefenseArmVertical extends CommandGroup {
-
- double horizontalLimit, height;
-
- public static final double ARM_SPEED = 0.6;
-
- public MoveDefenseArmVertical(double horizontalLimit, double height) {
- this.horizontalLimit = horizontalLimit;
- this.height = height;
- double targetArmAngle = calculateTargetArmAngle();
- double targetHandAngle = calculateTargetHandAngle();
- double handSpeed = ARM_SPEED * targetHandAngle / targetArmAngle;
- addParallel(new SetArmToAngle(ARM_SPEED, calculateTargetArmAngle()));
- addParallel(new SetHandToAngle(handSpeed, calculateTargetHandAngle()));
- }
-
- protected double calculateTargetArmAngle() {
- double armAngle;
- armAngle = square(horizontalLimit) + square(height)
- + square(Constants.DefenseArm.ARM_LENGTH)
- - square(Constants.DefenseArm.HAND_LENGTH);
- armAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
- * Constants.DefenseArm.ARM_LENGTH;
- armAngle = Math.acos(armAngle);
- armAngle = Math.atan(height / horizontalLimit) - armAngle;
- return armAngle;
- }
-
- protected double calculateTargetHandAngle() {
- double handAngle;
- handAngle = square(horizontalLimit) + square(height)
- + square(Constants.DefenseArm.HAND_LENGTH)
- - square(Constants.DefenseArm.ARM_LENGTH);
- handAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
- * Constants.DefenseArm.HAND_LENGTH;
- handAngle = Math.acos(handAngle);
- handAngle = handAngle + 90 - Math.atan(horizontalLimit / height);
- return handAngle;
- }
-
- public double square(double num) {
- return num * num;
- }
-}