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); | |
cf77d84e CZ |
33 | |
34 | this.wheelP = this.shooter.wheelP; | |
35 | this.wheelI = this.shooter.wheelI; | |
36 | this.wheelD = this.shooter.wheelD; | |
37 | this.wheelController = new PIDController(this.wheelP, this.wheelI, | |
38 | this.wheelD); | |
39 | this.wheelController.setDoneRange(0.5); | |
40 | this.wheelController.setMaxOutput(1.0); | |
41 | this.wheelController.setMinDoneCycles(3); | |
42 | this.target = this.shooter.CURRENT_SHOOTING_SPEED; | |
55cbd9ba | 43 | } |
44 | ||
45 | // Called just before this Command runs the first time | |
7cd65a82 | 46 | @Override |
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 | |
7cd65a82 | 52 | @Override |
55cbd9ba | 53 | protected void execute() { |
cf77d84e | 54 | double shooterSpeed = this.wheelController |
268b0048 | 55 | .calcPID(this.shooter.getShooterRPM()); |
cf77d84e CZ |
56 | |
57 | this.shooter.setFlyWheelMotorVal(shooterSpeed); | |
58 | } | |
59 | ||
60 | // Make this return true when this Command no longer needs to run execute() | |
7cd65a82 | 61 | @Override |
cf77d84e CZ |
62 | protected boolean isFinished() { |
63 | return false; | |
55cbd9ba | 64 | } |
65 | ||
66 | // Called once after isFinished returns true | |
7cd65a82 | 67 | @Override |
55cbd9ba | 68 | protected void end() { |
cf77d84e | 69 | this.shooter.stopFlyWheel(); |
55cbd9ba | 70 | } |
71 | ||
72 | // Called when another command which requires one or more of the same | |
73 | // subsystems is scheduled to run | |
7cd65a82 | 74 | @Override |
55cbd9ba | 75 | protected void interrupted() { |
7cd65a82 | 76 | |
55cbd9ba | 77 | } |
809609c9 | 78 | } |