2 package org
.usfirst
.frc
.team3501
.robot
.commands
.shooter
;
4 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
6 import org
.usfirst
.frc
.team3501
.robot
.utils
.PIDController
;
8 import edu
.wpi
.first
.wpilibj
.command
.Command
;
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.
15 * @author Shaina & Chris
17 public class RunFlyWheel
extends Command
{
18 private Shooter shooter
= Robot
.getShooter();
19 private double maxTimeOut
;
21 private PIDController wheelController
;
22 private double wheelP
;
23 private double wheelI
;
24 private double wheelD
;
25 private double target
;
27 public RunFlyWheel(double maxTimeOut
) {
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
,
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
;
40 // Called just before this Command runs the first time
42 protected void initialize() {
43 this.wheelController
.setSetPoint(this.target
);
46 // Called repeatedly when this Command is scheduled to run
48 protected void execute() {
49 double shooterSpeed
= this.wheelController
50 .calcPID(this.shooter
.getShooterRPM());
52 this.shooter
.setFlyWheelMotorVal(shooterSpeed
);
55 // Make this return true when this Command no longer needs to run execute()
57 protected boolean isFinished() {
58 return timeSinceInitialized() >= maxTimeOut
;
61 // Called once after isFinished returns true
63 protected void end() {
64 this.shooter
.stopFlyWheel();
67 // Called when another command which requires one or more of the same
68 // subsystems is scheduled to run
70 protected void interrupted() {