From: Cindy Zhang Date: Sat, 13 Feb 2016 21:00:59 +0000 (-0800) Subject: rename MoveDefenseArmVertical to MoveDefenseArm because previously MoveDefenseArmVert... X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=6d20dad3185d000ff698c06822b8237425fe4235;p=3501%2Fstronghold-2016 rename MoveDefenseArmVertical to MoveDefenseArm because previously MoveDefenseArmVertical is general enough to rename MoveDefenseArm --- diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArm.java b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArm.java new file mode 100644 index 00000000..5466f58c --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArm.java @@ -0,0 +1,53 @@ +package org.usfirst.frc.team3501.robot.commands; + +import org.usfirst.frc.team3501.robot.Constants; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * + */ +public class MoveDefenseArm extends CommandGroup { + + double horizontalLimit, height; + + public static final double ARM_SPEED = 0.6; + + public MoveDefenseArm(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; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java deleted file mode 100644 index 72ad433c..00000000 --- a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmVertical.java +++ /dev/null @@ -1,53 +0,0 @@ -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; - } -}