Commit | Line | Data |
---|---|---|
809609c9 | 1 | package org.usfirst.frc.team3501.robot.commands.shooter; |
2 | ||
dbec800d | 3 | import org.usfirst.frc.team3501.robot.Robot; |
00f515a1 | 4 | import org.usfirst.frc.team3501.robot.subsystems.Shooter; |
cf77d84e | 5 | import org.usfirst.frc.team3501.robot.utils.PIDController; |
dbec800d | 6 | |
809609c9 | 7 | import edu.wpi.first.wpilibj.command.Command; |
8 | ||
9 | /** | |
8c09ecc2 CZ |
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. | |
b6e97560 | 13 | * |
d4507ede | 14 | * Should only be run from the operator interface. |
15 | * | |
973f0ac4 | 16 | * pre-condition: This command must be run by a button in OI, with |
17 | * button.whileHeld(...). | |
357feb5c | 18 | * |
8c09ecc2 | 19 | * @author Shaina & Chris |
809609c9 | 20 | */ |
21 | public class RunFlyWheelContinuous extends Command { | |
00f515a1 | 22 | private Shooter shooter = Robot.getShooter(); |
55cbd9ba | 23 | |
cf77d84e CZ |
24 | private PIDController wheelController; |
25 | private double wheelP; | |
26 | private double wheelI; | |
27 | private double wheelD; | |
28 | private double target; | |
29 | ||
00f515a1 | 30 | public RunFlyWheelContinuous() { |
6af2bd85 ES |
31 | this.wheelP = this.shooter.getWheelP(); |
32 | this.wheelI = this.shooter.getWheelI(); | |
33 | this.wheelD = this.shooter.getWheelD(); | |
cf77d84e CZ |
34 | this.wheelController = new PIDController(this.wheelP, this.wheelI, |
35 | this.wheelD); | |
36 | this.wheelController.setDoneRange(0.5); | |
37 | this.wheelController.setMaxOutput(1.0); | |
38 | this.wheelController.setMinDoneCycles(3); | |
6af2bd85 | 39 | this.target = this.shooter.getShootingSpeed(); |
55cbd9ba | 40 | } |
41 | ||
7cd65a82 | 42 | @Override |
55cbd9ba | 43 | protected void initialize() { |
cf77d84e | 44 | this.wheelController.setSetPoint(this.target); |
55cbd9ba | 45 | } |
46 | ||
7cd65a82 | 47 | @Override |
55cbd9ba | 48 | protected void execute() { |
135022bc CZ |
49 | // double shooterSpeed = this.wheelController |
50 | // .calcPID(this.shooter.getShooterRPM()); | |
51 | // | |
52 | // this.shooter.setFlyWheelMotorVal(shooterSpeed); | |
53 | System.out.println(shooter.getShooterRPM()); | |
6af2bd85 | 54 | this.shooter.runFlyWheel(); |
cf77d84e CZ |
55 | } |
56 | ||
7cd65a82 | 57 | @Override |
cf77d84e CZ |
58 | protected boolean isFinished() { |
59 | return false; | |
55cbd9ba | 60 | } |
61 | ||
7cd65a82 | 62 | @Override |
55cbd9ba | 63 | protected void end() { |
cf77d84e | 64 | this.shooter.stopFlyWheel(); |
55cbd9ba | 65 | } |
66 | ||
7cd65a82 | 67 | @Override |
55cbd9ba | 68 | protected void interrupted() { |
135022bc | 69 | end(); |
55cbd9ba | 70 | } |
809609c9 | 71 | } |