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 | /** | |
8c09ecc2 CZ |
11 | * This command runs the fly wheel at a specific speed using a PID Controller |
12 | * for accuracy for a given time. The fly wheel is intended to shoot balls fed | |
13 | * by the intake wheel. | |
809609c9 | 14 | * |
cf77d84e | 15 | * @author Shaina & Chris |
809609c9 | 16 | */ |
17 | public class RunFlyWheel extends Command { | |
ad7e6b1e | 18 | private Shooter shooter = Robot.getShooter(); |
cf77d84e CZ |
19 | private double maxTimeOut; |
20 | ||
21 | private PIDController wheelController; | |
22 | private double wheelP; | |
23 | private double wheelI; | |
24 | private double wheelD; | |
25 | private double target; | |
26 | ||
27 | public RunFlyWheel(double maxTimeOut) { | |
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 | |
7cd65a82 | 41 | @Override |
493a4f87 | 42 | protected void initialize() { |
cf77d84e | 43 | this.wheelController.setSetPoint(this.target); |
493a4f87 | 44 | } |
45 | ||
46 | // Called repeatedly when this Command is scheduled to run | |
7cd65a82 | 47 | @Override |
493a4f87 | 48 | protected void execute() { |
cf77d84e | 49 | double shooterSpeed = this.wheelController |
268b0048 | 50 | .calcPID(this.shooter.getShooterRPM()); |
cf77d84e CZ |
51 | |
52 | this.shooter.setFlyWheelMotorVal(shooterSpeed); | |
53 | } | |
54 | ||
55 | // Make this return true when this Command no longer needs to run execute() | |
7cd65a82 | 56 | @Override |
cf77d84e CZ |
57 | protected boolean isFinished() { |
58 | return timeSinceInitialized() >= maxTimeOut; | |
493a4f87 | 59 | } |
60 | ||
61 | // Called once after isFinished returns true | |
7cd65a82 | 62 | @Override |
493a4f87 | 63 | protected void end() { |
cf77d84e | 64 | this.shooter.stopFlyWheel(); |
493a4f87 | 65 | } |
66 | ||
67 | // Called when another command which requires one or more of the same | |
68 | // subsystems is scheduled to run | |
7cd65a82 | 69 | @Override |
493a4f87 | 70 | protected void interrupted() { |
71 | } | |
809609c9 | 72 | } |