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 given speed for a given time. The fly
12 * wheel is intended to shoot balls fed by the intake wheel.
14 * @author Shaina & Chris
16 public class RunFlyWheel
extends Command
{
17 private Shooter shooter
= Robot
.getShooter();
18 private double maxTimeOut
;
20 private PIDController wheelController
;
21 private double wheelP
;
22 private double wheelI
;
23 private double wheelD
;
24 private double target
;
26 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
41 protected void initialize() {
42 this.wheelController
.setSetPoint(this.target
);
45 // Called repeatedly when this Command is scheduled to run
46 protected void execute() {
47 double shooterSpeed
= this.wheelController
48 .calcPID(this.shooter
.getShooterSpeed());
50 this.shooter
.setFlyWheelMotorVal(shooterSpeed
);
53 // Make this return true when this Command no longer needs to run execute()
54 protected boolean isFinished() {
55 return timeSinceInitialized() >= maxTimeOut
;
58 // Called once after isFinished returns true
59 protected void end() {
60 this.shooter
.stopFlyWheel();
63 // Called when another command which requires one or more of the same
64 // subsystems is scheduled to run
65 protected void interrupted() {