From 506f70cc138bee10bfd4643b64f30b74f2ed2d7c Mon Sep 17 00:00:00 2001 From: Shivani Oghanta Date: Sat, 6 Feb 2016 17:44:23 -0800 Subject: [PATCH] Fill out MoveIntakeArmToBallIntakeHeight command --- .../MoveIntakeArmToBallIntakeHeight.java | 53 ++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java index 805bd786..deda77b4 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java +++ b/src/org/usfirst/frc/team3501/robot/commands/MoveIntakeArmToBallIntakeHeight.java @@ -1,5 +1,56 @@ package org.usfirst.frc.team3501.robot.commands; -public class MoveIntakeArmToBallIntakeHeight { +import org.usfirst.frc.team3501.robot.Robot; +import edu.wpi.first.wpilibj.command.Command; + +public class MoveIntakeArmToBallIntakeHeight extends Command { + private double targetHeight; + private double currentHeight; + private double targetSpeed; + private double SENSITIVITY_THRESHOLD = 0.1; + private double NO_MOVEMENT_SPEED = 0; + + public MoveIntakeArmToBallIntakeHeight(double height, double speed) { + requires(Robot.intakeArm); + targetHeight = height; + targetSpeed = speed; + } + + @Override + protected void initialize() { + currentHeight = Robot.intakeArm.getArmAngle(); + double difference = targetHeight - currentHeight; + if (difference > 0) { + Robot.intakeArm.setArmVoltage(targetSpeed); + } else { + Robot.intakeArm.setArmVoltage(-targetSpeed); + } + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + currentHeight = Robot.intakeArm.getArmAngle(); + double distance = Math.abs(currentHeight - targetHeight); + if (distance <= SENSITIVITY_THRESHOLD) { + return true; + } else { + return false; + } + + } + + @Override + protected void end() { + Robot.intakeArm.setArmVoltage(NO_MOVEMENT_SPEED); + } + + @Override + protected void interrupted() { + } } -- 2.30.2