Commit | Line | Data |
---|---|---|
809609c9 | 1 | package org.usfirst.frc.team3501.robot.commands.shooter; |
2 | ||
dbec800d | 3 | import org.usfirst.frc.team3501.robot.Robot; |
00f515a1 | 4 | import org.usfirst.frc.team3501.robot.subsystems.Shooter; |
cf77d84e | 5 | import org.usfirst.frc.team3501.robot.utils.PIDController; |
dbec800d | 6 | |
809609c9 | 7 | import edu.wpi.first.wpilibj.command.Command; |
8 | ||
9 | /** | |
8c09ecc2 CZ |
10 | * This command runs the fly wheel continuously at a set speed using a PID |
11 | * Controller when OI button managing fly wheel is pressed. The command will run | |
12 | * the fly wheel motor until the button triggering it is released. | |
b6e97560 | 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(...). | |
357feb5c | 18 | * |
8c09ecc2 | 19 | * @author Shaina & Chris |
809609c9 | 20 | */ |
21 | public class RunFlyWheelContinuous extends Command { | |
00f515a1 | 22 | private Shooter shooter = Robot.getShooter(); |
55cbd9ba | 23 | |
cf77d84e CZ |
24 | private PIDController wheelController; |
25 | private double wheelP; | |
26 | private double wheelI; | |
27 | private double wheelD; | |
28 | private double target; | |
29 | ||
00f515a1 | 30 | public RunFlyWheelContinuous() { |
cf77d84e CZ |
31 | // Use requires() here to declare subsystem dependencies |
32 | // eg. requires(chassis); | |
00f515a1 | 33 | requires(shooter); |
cf77d84e CZ |
34 | |
35 | this.wheelP = this.shooter.wheelP; | |
36 | this.wheelI = this.shooter.wheelI; | |
37 | this.wheelD = this.shooter.wheelD; | |
38 | this.wheelController = new PIDController(this.wheelP, this.wheelI, | |
39 | this.wheelD); | |
40 | this.wheelController.setDoneRange(0.5); | |
41 | this.wheelController.setMaxOutput(1.0); | |
42 | this.wheelController.setMinDoneCycles(3); | |
43 | this.target = this.shooter.CURRENT_SHOOTING_SPEED; | |
55cbd9ba | 44 | } |
45 | ||
46 | // Called just before this Command runs the first time | |
55cbd9ba | 47 | protected void initialize() { |
cf77d84e | 48 | this.wheelController.setSetPoint(this.target); |
55cbd9ba | 49 | } |
50 | ||
51 | // Called repeatedly when this Command is scheduled to run | |
55cbd9ba | 52 | protected void execute() { |
cf77d84e | 53 | double shooterSpeed = this.wheelController |
268b0048 | 54 | .calcPID(this.shooter.getShooterRPM()); |
cf77d84e CZ |
55 | |
56 | this.shooter.setFlyWheelMotorVal(shooterSpeed); | |
57 | } | |
58 | ||
59 | // Make this return true when this Command no longer needs to run execute() | |
60 | protected boolean isFinished() { | |
61 | return false; | |
55cbd9ba | 62 | } |
63 | ||
64 | // Called once after isFinished returns true | |
55cbd9ba | 65 | protected void end() { |
cf77d84e | 66 | this.shooter.stopFlyWheel(); |
55cbd9ba | 67 | } |
68 | ||
69 | // Called when another command which requires one or more of the same | |
70 | // subsystems is scheduled to run | |
55cbd9ba | 71 | protected void interrupted() { |
89a76b33 | 72 | end(); |
55cbd9ba | 73 | } |
809609c9 | 74 | } |