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