From b4eeeb65de8cfa2eae42ce71dbb79fad07b60765 Mon Sep 17 00:00:00 2001 From: Shivani Oghanta Date: Thu, 11 Feb 2016 19:53:54 -0800 Subject: [PATCH] Add command MoveIntakeArmToAngle --- .../robot/commands/MoveIntakeArmToAngle.java | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java new file mode 100644 index 00000000..092f2d41 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToAngle.java @@ -0,0 +1,59 @@ +package org.usfirst.frc.team3501.robot.commands; + +import org.usfirst.frc.team3501.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class MoveIntakeArmToAngle extends Command { + private double currentAngle; + private double targetAngle; + private double targetSpeed; + private double SENSITIVITY_THRESHOLD = 0.1; + private boolean isDecreasing = false; + + public MoveIntakeArmToAngle(double angle, double speed) { + requires(Robot.intakeArm); + targetAngle = angle; + targetSpeed = speed; + + } + + @Override + protected void initialize() { + currentAngle = Robot.intakeArm.getArmAngle(); + double difference = targetAngle - currentAngle; + if (difference > 0) { + Robot.intakeArm.setArmSpeed(targetSpeed); + isDecreasing = true; + } else { + Robot.intakeArm.setArmSpeed(-targetSpeed); + isDecreasing = false; + } + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + currentAngle = Robot.intakeArm.getArmAngle(); + + if (isDecreasing == true) { + return (currentAngle <= targetAngle + SENSITIVITY_THRESHOLD); + } else { + return (currentAngle >= targetAngle + SENSITIVITY_THRESHOLD); + } + } + + @Override + protected void end() { + Robot.intakeArm.stop(); + } + + @Override + protected void interrupted() { + } + +} -- 2.30.2