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
;
12 public class StartUpFlyWheel
extends Command
{
13 private Shooter shooter
= Robot
.getShooter();
14 private double maxTimeOut
;
16 private PIDController wheelController
;
17 private double wheelP
;
18 private double wheelI
;
19 private double wheelD
;
20 private double motorVal
;
21 private double target
;
23 public StartUpFlyWheel(double speed
, double maxTimeOut
) {
24 // Use requires() here to declare subsystem dependencies
25 // eg. requires(chassis);
28 this.wheelP
= this.shooter
.wheelP
;
29 this.wheelI
= this.shooter
.wheelI
;
30 this.wheelD
= this.shooter
.wheelD
;
31 this.wheelController
= new PIDController(this.wheelP
, this.wheelI
,
33 this.wheelController
.setDoneRange(0.5);
34 this.wheelController
.setMaxOutput(1.0);
35 this.wheelController
.setMinDoneCycles(3);
39 // Called just before this Command runs the first time
40 protected void initialize() {
41 this.wheelController
.setSetPoint(this.target
);
44 // Called repeatedly when this Command is scheduled to run
45 protected void execute() {
46 double shooterSpeed
= this.wheelController
47 .calcPID(this.shooter
.getShooterSpeed());
49 this.shooter
.setFlyWheelMotorVal(shooterSpeed
);
52 // Make this return true when this Command no longer needs to run execute()
53 protected boolean isFinished() {
54 boolean isDone
= this.wheelController
.isDone();
55 if (timeSinceInitialized() >= maxTimeOut
|| isDone
) {
56 System
.out
.println("time: " + timeSinceInitialized());
62 // 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
69 protected void interrupted() {