Commit | Line | Data |
---|---|---|
809609c9 | 1 | package org.usfirst.frc.team3501.robot.commands.shooter; |
2 | ||
eedddcb4 | 3 | import org.usfirst.frc.team3501.robot.Constants; |
210ebccb | 4 | import org.usfirst.frc.team3501.robot.Robot; |
ad7e6b1e | 5 | import org.usfirst.frc.team3501.robot.subsystems.Shooter; |
210ebccb | 6 | |
eedddcb4 | 7 | import edu.wpi.first.wpilibj.Timer; |
809609c9 | 8 | import edu.wpi.first.wpilibj.command.Command; |
9 | ||
10 | /** | |
d4507ede | 11 | * This command runs index wheel continuously when OI button managing index |
12 | * wheel is pressed. The command will run the index wheel motor until the button | |
89a76b33 | 13 | * triggering it is released. |
357feb5c | 14 | * |
d4507ede | 15 | * Should only be run from the operator interface. |
16 | * | |
973f0ac4 | 17 | * pre-condition: This command must be run by a button in OI with |
18 | * button.whileHeld(...). | |
19 | * | |
27d2386f | 20 | * @author Shaina |
809609c9 | 21 | */ |
22 | public class RunIndexWheelContinuous extends Command { | |
ad7e6b1e | 23 | private Shooter shooter = Robot.getShooter(); |
eedddcb4 | 24 | private Timer t = new Timer(); |
4f2fa45c | 25 | |
b7ef589a | 26 | /** |
27 | * See JavaDoc comment in class for details | |
b7ef589a | 28 | */ |
ad7e6b1e | 29 | public RunIndexWheelContinuous() { |
00f515a1 | 30 | requires(shooter); |
4f2fa45c | 31 | } |
32 | ||
4f2fa45c | 33 | @Override |
34 | protected void initialize() { | |
eedddcb4 | 35 | t.reset(); |
4f2fa45c | 36 | } |
37 | ||
4f2fa45c | 38 | @Override |
39 | protected void execute() { | |
eedddcb4 RR |
40 | if (t.get() % 1 == 0) { |
41 | if (Shooter.getShooter().getPistonValue() == Constants.Shooter.LOW_GEAR) { | |
42 | Shooter.getShooter().setHighGear(); | |
43 | } else { | |
44 | Shooter.getShooter().setLowGear(); | |
45 | } | |
46 | } | |
47 | ||
d7042bec | 48 | double shooterSpeed = shooter.getShooterRPM(); |
7ba6bc91 CZ |
49 | double targetShooterSpeed = shooter.getTargetShootingSpeed(); |
50 | double threshold = shooter.getRPMThreshold(); | |
b70398a7 CZ |
51 | if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold) |
52 | shooter.runIndexWheel(); | |
4f2fa45c | 53 | } |
54 | ||
4f2fa45c | 55 | @Override |
56 | protected void end() { | |
135022bc | 57 | shooter.stopIndexWheel(); |
4f2fa45c | 58 | } |
59 | ||
4f2fa45c | 60 | @Override |
61 | protected void interrupted() { | |
210ebccb | 62 | end(); |
4f2fa45c | 63 | } |
64 | ||
65 | @Override | |
66 | protected boolean isFinished() { | |
2a9dabb1 | 67 | return false; |
4f2fa45c | 68 | } |
809609c9 | 69 | |
70 | } |