+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands;
-
-import org.usfirst.frc.team3501.robot.Robot;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class SetHandToLevel extends Command {
- private static final double THRESHOLD = 0.1;
- private double speed;
- private double targetPosition;
- private double currentPosition;
-
- public SetHandToLevel(double speed, int level) {
- requires(Robot.defenseArm);
-
- this.speed = speed;
- this.targetPosition = Robot.defenseArm.getAngleForHandLocation(level);
- }
-
- @Override
- protected void initialize() {
- currentPosition = Robot.defenseArm.getHandPotAngle();
-
- if (currentPosition > targetPosition) {
- Robot.defenseArm.setHandSpeed(-speed);
- } else {
- Robot.defenseArm.setHandSpeed(speed);
- }
-
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- currentPosition = Robot.defenseArm.getHandPotAngle();
-
- double difference = Math.abs(currentPosition - targetPosition);
- return (difference <= THRESHOLD);
- }
-
- @Override
- protected void end() {
- Robot.defenseArm.setHandSpeed(0);
- }
-
- @Override
- protected void interrupted() {
- end();
- }
-
-}