Commit | Line | Data |
---|---|---|
afc49207 T |
1 | package org.usfirst.frc.team3501.robot.commands.shooter; |
2 | ||
5ba0f018 CZ |
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; | |
6 | ||
afc49207 T |
7 | import edu.wpi.first.wpilibj.command.Command; |
8 | ||
9 | /** | |
10 | * | |
11 | */ | |
12 | public class StartUpFlyWheel extends Command { | |
5ba0f018 | 13 | private Shooter shooter = Robot.getShooter(); |
381dad77 CZ |
14 | private double maxTimeOut; |
15 | ||
5ba0f018 CZ |
16 | private PIDController wheelController; |
17 | private double wheelP; | |
18 | private double wheelI; | |
19 | private double wheelD; | |
20 | private double motorVal; | |
21 | private double target; | |
381dad77 CZ |
22 | |
23 | public StartUpFlyWheel(double speed, double maxTimeOut) { | |
5ba0f018 CZ |
24 | // Use requires() here to declare subsystem dependencies |
25 | // eg. requires(chassis); | |
26 | requires(shooter); | |
381dad77 CZ |
27 | |
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, | |
32 | this.wheelD); | |
33 | this.wheelController.setDoneRange(0.5); | |
34 | this.wheelController.setMaxOutput(1.0); | |
35 | this.wheelController.setMinDoneCycles(3); | |
36 | this.target = speed; | |
5ba0f018 CZ |
37 | } |
38 | ||
39 | // Called just before this Command runs the first time | |
40 | protected void initialize() { | |
381dad77 | 41 | this.wheelController.setSetPoint(this.target); |
5ba0f018 CZ |
42 | } |
43 | ||
44 | // Called repeatedly when this Command is scheduled to run | |
45 | protected void execute() { | |
381dad77 CZ |
46 | double shooterSpeed = this.wheelController |
47 | .calcPID(this.shooter.getShooterSpeed()); | |
48 | ||
49 | this.shooter.setFlyWheelMotorVal(shooterSpeed); | |
5ba0f018 CZ |
50 | } |
51 | ||
52 | // Make this return true when this Command no longer needs to run execute() | |
53 | protected boolean isFinished() { | |
381dad77 CZ |
54 | boolean isDone = this.wheelController.isDone(); |
55 | if (timeSinceInitialized() >= maxTimeOut || isDone) { | |
56 | System.out.println("time: " + timeSinceInitialized()); | |
57 | return true; | |
58 | } | |
5ba0f018 CZ |
59 | return false; |
60 | } | |
61 | ||
62 | // Called once after isFinished returns true | |
63 | protected void end() { | |
381dad77 | 64 | this.shooter.stopFlyWheel(); |
5ba0f018 CZ |
65 | } |
66 | ||
67 | // Called when another command which requires one or more of the same | |
68 | // subsystems is scheduled to run | |
69 | protected void interrupted() { | |
381dad77 | 70 | end(); |
5ba0f018 | 71 | } |
afc49207 | 72 | } |