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
;
26 private double shooterSpeed
= 0;
28 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
.getCurrentShootingSpeed();
41 // Called just before this Command runs the first time
43 protected void initialize() {
44 this.wheelController
.setSetPoint(this.target
);
47 // Called repeatedly when this Command is scheduled to run
49 protected void execute() {
50 double calculatedShooterIncrement
= this.wheelController
51 .calcPID(this.shooter
.getShooterRPM());
52 shooterSpeed
+= calculatedShooterIncrement
;
53 this.shooter
.setFlyWheelMotorVal(shooterSpeed
);
56 // Make this return true when this Command no longer needs to run execute()
58 protected boolean isFinished() {
59 return timeSinceInitialized() >= maxTimeOut
;
62 // Called once after isFinished returns true
64 protected void end() {
65 this.shooter
.stopFlyWheel();
68 // Called when another command which requires one or more of the same
69 // subsystems is scheduled to run
71 protected void interrupted() {