1 package org
.usfirst
.frc
.team3501
.robot
.commands
.shooter
;
3 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
4 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
5 import org
.usfirst
.frc
.team3501
.robot
.utils
.PIDController
;
7 import edu
.wpi
.first
.wpilibj
.command
.Command
;
10 * This command runs the fly wheel continuously at a set speed using a PID
11 * Controller when OI button managing fly wheel is pressed. The command will run
12 * the fly wheel motor until the button triggering it is released.
14 * Should only be run from the operator interface.
16 * pre-condition: This command must be run by a button in OI, with
17 * button.whileHeld(...).
19 * @author Shaina & Chris
21 public class RunFlyWheelContinuous
extends Command
{
22 private Shooter shooter
= Robot
.getShooter();
24 private PIDController wheelController
;
25 private double wheelP
;
26 private double wheelI
;
27 private double wheelD
;
28 private double target
;
29 private double shooterSpeed
= 0;
31 public RunFlyWheelContinuous() {
32 this.wheelP
= this.shooter
.wheelP
;
33 this.wheelI
= this.shooter
.wheelI
;
34 this.wheelD
= this.shooter
.wheelD
;
35 this.wheelController
= new PIDController(this.wheelP
, this.wheelI
,
37 this.wheelController
.setDoneRange(10);
38 this.wheelController
.setMaxOutput(1.0);
39 this.wheelController
.setMinDoneCycles(3);
40 this.target
= shooter
.getCurrentShootingSpeed();
44 protected void initialize() {
45 this.wheelController
.setSetPoint(this.target
);
49 protected void execute() {
50 double calculatedShooterIncrement
= this.wheelController
51 .calcPID(this.shooter
.getShooterRPM());
52 shooterSpeed
+= calculatedShooterIncrement
;
53 this.shooter
.setFlyWheelMotorVal(shooterSpeed
);
57 protected boolean isFinished() {
62 protected void end() {
63 this.shooter
.stopFlyWheel();
67 protected void interrupted() {