package org.usfirst.frc.team3501.robot.commands;
-import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Robot;
-import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.CommandGroup;
/**
- *
+ * Given input verticalDisplacement, which represents how far (and in what
+ * direction, depending on the sign) the user wants the arm to be moved linearly
+ * & vertically, this commandGroup calls the MoveDefenseArm command, which
+ * will then move the tip of the arm to the target position based on input
+ * verticalDisplacement
*/
-public class MoveDefenseArmVertical extends Command {
+public class MoveDefenseArmVertical extends CommandGroup {
- double horizontalLimit, height;
+ public MoveDefenseArmVertical(double verticalDisplacement) {
+ double x = Robot.defenseArm.getArmHorizontalDisplacement();
+ double y = Robot.defenseArm.getArmVerticalDisplacement();
+ addSequential(new MoveDefenseArm(x, y + verticalDisplacement));
- public MoveDefenseArmVertical(double horizontalLimit, double height) {
- // requires();
- this.horizontalLimit = horizontalLimit;
- this.height = height;
- }
-
- // Called just before this Command runs the first time
- @Override
- protected void initialize() {
- }
-
- // Called repeatedly when this Command is scheduled to run
- @Override
- protected void execute() {
- 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;
-
- 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);
- }
-
- // Make this return true when this Command no longer needs to run execute()
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- // Called once after isFinished returns true
- @Override
- protected void end() {
- }
-
- public double square(double num) {
- return num * num;
- }
-
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
- @Override
- protected void interrupted() {
}
}