Commit | Line | Data |
---|---|---|
93703c78 SO |
1 | package org.usfirst.frc.team3501.robot.commands; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Robot; | |
4 | ||
5 | import edu.wpi.first.wpilibj.command.Command; | |
6 | ||
7 | public class MoveIntakeArmToHeight extends Command { | |
8 | private double targetHeight; | |
9 | private double currentHeight; | |
10 | private double targetSpeed; | |
11 | private double SENSITIVITY_THRESHOLD = 0.1; | |
12 | private double NO_MOVEMENT_SPEED = 0; | |
13 | ||
14 | public MoveIntakeArmToHeight(double height, double speed) { | |
15 | requires(Robot.intakeArm); | |
16 | targetHeight = height; | |
17 | targetSpeed = speed; | |
18 | } | |
19 | ||
20 | @Override | |
21 | protected void initialize() { | |
22 | currentHeight = Robot.intakeArm.getArmHeight(); | |
23 | double difference = targetHeight - currentHeight; | |
24 | if (difference > 0) { | |
25 | Robot.intakeArm.setArmVoltage(targetSpeed); | |
26 | } else { | |
27 | Robot.intakeArm.setArmVoltage(-targetSpeed); | |
28 | } | |
29 | } | |
30 | ||
31 | @Override | |
32 | protected void execute() { | |
33 | ||
34 | } | |
35 | ||
36 | @Override | |
37 | protected boolean isFinished() { | |
38 | currentHeight = Robot.intakeArm.getArmHeight(); | |
39 | double distance = Math.abs(currentHeight - targetHeight); | |
40 | if (distance <= SENSITIVITY_THRESHOLD) { | |
41 | return true; | |
42 | } else { | |
43 | return false; | |
44 | } | |
45 | ||
46 | } | |
47 | ||
48 | @Override | |
49 | protected void end() { | |
50 | Robot.intakeArm.setArmVoltage(NO_MOVEMENT_SPEED); | |
51 | } | |
52 | ||
53 | @Override | |
54 | protected void interrupted() { | |
55 | } | |
56 | } |