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
) {
30 this.wheelP
= this.shooter
.wheelP
;
31 this.wheelI
= this.shooter
.wheelI
;
32 this.wheelD
= this.shooter
.wheelD
;
33 this.wheelController
= new PIDController(this.wheelP
, this.wheelI
,
35 this.wheelController
.setDoneRange(0.5);
36 this.wheelController
.setMaxOutput(1.0);
37 this.wheelController
.setMinDoneCycles(3);
38 this.target
= this.shooter
.CURRENT_SHOOTING_SPEED
;
41 // 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
47 protected void execute() {
48 double shooterSpeed
= this.wheelController
49 .calcPID(this.shooter
.getShooterSpeed());
51 this.shooter
.setFlyWheelMotorVal(shooterSpeed
);
54 // Make this return true when this Command no longer needs to run execute()
55 protected boolean isFinished() {
56 return timeSinceInitialized() >= maxTimeOut
;
59 // Called once after isFinished returns true
60 protected void end() {
61 this.shooter
.stopFlyWheel();
64 // Called when another command which requires one or more of the same
65 // subsystems is scheduled to run
66 protected void interrupted() {