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