Commit | Line | Data |
---|---|---|
cf77d84e | 1 | |
809609c9 | 2 | package org.usfirst.frc.team3501.robot.commands.shooter; |
3 | ||
414d5638 | 4 | import org.usfirst.frc.team3501.robot.Robot; |
ad7e6b1e | 5 | import org.usfirst.frc.team3501.robot.subsystems.Shooter; |
cf77d84e | 6 | import org.usfirst.frc.team3501.robot.utils.PIDController; |
414d5638 | 7 | |
809609c9 | 8 | import edu.wpi.first.wpilibj.command.Command; |
9 | ||
10 | /** | |
973f0ac4 | 11 | * This command runs the fly wheel at a given speed for a given time. The fly |
12 | * wheel is intended to shoot balls fed by the intake wheel. | |
809609c9 | 13 | * |
cf77d84e | 14 | * @author Shaina & Chris |
809609c9 | 15 | */ |
16 | public class RunFlyWheel extends Command { | |
ad7e6b1e | 17 | private Shooter shooter = Robot.getShooter(); |
cf77d84e CZ |
18 | private double maxTimeOut; |
19 | ||
20 | private PIDController wheelController; | |
21 | private double wheelP; | |
22 | private double wheelI; | |
23 | private double wheelD; | |
24 | private double target; | |
25 | ||
26 | public RunFlyWheel(double maxTimeOut) { | |
ad7e6b1e | 27 | requires(shooter); |
cf77d84e CZ |
28 | |
29 | this.wheelP = this.shooter.wheelP; | |
30 | this.wheelI = this.shooter.wheelI; | |
31 | this.wheelD = this.shooter.wheelD; | |
32 | this.wheelController = new PIDController(this.wheelP, this.wheelI, | |
33 | this.wheelD); | |
34 | this.wheelController.setDoneRange(0.5); | |
35 | this.wheelController.setMaxOutput(1.0); | |
36 | this.wheelController.setMinDoneCycles(3); | |
37 | this.target = this.shooter.CURRENT_SHOOTING_SPEED; | |
493a4f87 | 38 | } |
39 | ||
40 | // Called just before this Command runs the first time | |
493a4f87 | 41 | protected void initialize() { |
cf77d84e | 42 | this.wheelController.setSetPoint(this.target); |
493a4f87 | 43 | } |
44 | ||
45 | // Called repeatedly when this Command is scheduled to run | |
493a4f87 | 46 | protected void execute() { |
cf77d84e CZ |
47 | double shooterSpeed = this.wheelController |
48 | .calcPID(this.shooter.getShooterSpeed()); | |
49 | ||
50 | this.shooter.setFlyWheelMotorVal(shooterSpeed); | |
51 | } | |
52 | ||
53 | // Make this return true when this Command no longer needs to run execute() | |
54 | protected boolean isFinished() { | |
55 | return timeSinceInitialized() >= maxTimeOut; | |
493a4f87 | 56 | } |
57 | ||
58 | // Called once after isFinished returns true | |
493a4f87 | 59 | protected void end() { |
cf77d84e | 60 | this.shooter.stopFlyWheel(); |
493a4f87 | 61 | } |
62 | ||
63 | // Called when another command which requires one or more of the same | |
64 | // subsystems is scheduled to run | |
493a4f87 | 65 | protected void interrupted() { |
414d5638 | 66 | end(); |
493a4f87 | 67 | } |
809609c9 | 68 | } |