Commit | Line | Data |
---|---|---|
79ba119a | 1 | package org.usfirst.frc.team3501.robot.subsystems; |
2 | ||
04227dc0 | 3 | import org.usfirst.frc.team3501.robot.Constants; |
ac77a7b8 | 4 | import org.usfirst.frc.team3501.robot.MathLib; |
268b0048 | 5 | import org.usfirst.frc.team3501.robot.utils.HallEffectSensor; |
7ba6bc91 | 6 | import org.usfirst.frc.team3501.robot.utils.PIDController; |
04227dc0 | 7 | |
8 | import com.ctre.CANTalon; | |
9 | ||
10 | import edu.wpi.first.wpilibj.command.Subsystem; | |
11 | ||
12 | public class Shooter extends Subsystem { | |
7ba6bc91 | 13 | public double wheelP = 0.00007, wheelI = 0, wheelD = 0.0008; |
079a8cb6 | 14 | private static Shooter shooter; |
268b0048 | 15 | private static HallEffectSensor hallEffect; |
3a86b1a5 | 16 | private final CANTalon flyWheel1, flyWheel2, indexWheel; |
79ba119a | 17 | |
7ba6bc91 CZ |
18 | private PIDController wheelController; |
19 | ||
20 | private static final double RPM_THRESHOLD = 10; | |
f74d236d CZ |
21 | public static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75; |
22 | public static final double REVERSE_FLYWHEEL_MOTOR_VALUE = -0.75; | |
150f450f CZ |
23 | private static final double DEFAULT_SHOOTING_SPEED = 2875; // rpm |
24 | private static final double SHOOTING_SPEED_INCREMENT = 25; | |
ad7e6b1e | 25 | |
7ba6bc91 CZ |
26 | private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; |
27 | private double currentShooterMotorValue = 0; | |
ad7e6b1e | 28 | |
079a8cb6 | 29 | private Shooter() { |
3a86b1a5 CZ |
30 | flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1); |
31 | flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2); | |
04227dc0 | 32 | indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL); |
ad7e6b1e | 33 | |
268b0048 | 34 | hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1); |
35 | } | |
36 | ||
04227dc0 | 37 | /** |
38 | * Returns shooter object | |
39 | * | |
40 | * @return Shooter object | |
41 | */ | |
079a8cb6 CZ |
42 | public static Shooter getShooter() { |
43 | if (shooter == null) { | |
44 | shooter = new Shooter(); | |
45 | } | |
46 | return shooter; | |
47 | } | |
41dfad94 | 48 | |
079a8cb6 | 49 | /** |
04227dc0 | 50 | * Sets fly wheel motor value to input. |
51 | * | |
52 | * @param val | |
53 | * motor value from -1 to 1(fastest forward) | |
079a8cb6 | 54 | */ |
ac77a7b8 | 55 | public void setFlyWheelMotorVal(double val) { |
9ca89e45 | 56 | val = MathLib.restrictToRange(val, -1.0, 1.0); |
3a86b1a5 CZ |
57 | flyWheel1.set(val); |
58 | flyWheel2.set(val); | |
079a8cb6 | 59 | } |
41dfad94 | 60 | |
04227dc0 | 61 | /** |
62 | * Stops fly wheel motor. | |
63 | */ | |
64 | public void stopFlyWheel() { | |
3a86b1a5 CZ |
65 | flyWheel1.set(0); |
66 | flyWheel2.set(0); | |
04227dc0 | 67 | } |
41dfad94 | 68 | |
04227dc0 | 69 | /** |
70 | * Sets index wheel motor value to input. | |
71 | * | |
72 | * @param val | |
73 | * motor value from -1 to 1(fastest forward) | |
74 | */ | |
ac77a7b8 CZ |
75 | public void setIndexWheelMotorVal(double val) { |
76 | val = MathLib.restrictToRange(val, -1.0, 1.0); | |
04227dc0 | 77 | indexWheel.set(val); |
079a8cb6 | 78 | } |
41dfad94 | 79 | |
079a8cb6 | 80 | /** |
04227dc0 | 81 | * Stops index wheel motor. |
079a8cb6 CZ |
82 | */ |
83 | public void stopIndexWheel() { | |
04227dc0 | 84 | indexWheel.set(0); |
079a8cb6 | 85 | } |
41dfad94 | 86 | |
f625e57a CZ |
87 | @Override |
88 | protected void initDefaultCommand() { | |
89 | ||
90 | } | |
91 | ||
7ba6bc91 CZ |
92 | public double getRPMThreshold() { |
93 | return RPM_THRESHOLD; | |
94 | } | |
95 | ||
f625e57a CZ |
96 | public double getShooterRPM() { |
97 | return hallEffect.getRPM(); | |
09e509d3 NA |
98 | } |
99 | ||
7ba6bc91 CZ |
100 | public void setTargetShootingSpeed(double Value) { |
101 | targetShootingSpeed = Value; | |
09e509d3 NA |
102 | } |
103 | ||
7ba6bc91 CZ |
104 | public void decrementTargetShootingSpeed() { |
105 | this.targetShootingSpeed -= this.SHOOTING_SPEED_INCREMENT; | |
f625e57a | 106 | } |
079a8cb6 | 107 | |
7ba6bc91 CZ |
108 | public void incrementTargetShootingSpeed() { |
109 | this.targetShootingSpeed += this.SHOOTING_SPEED_INCREMENT; | |
079a8cb6 | 110 | } |
381dad77 | 111 | |
7ba6bc91 CZ |
112 | public void resetTargetShootingSpeed() { |
113 | this.targetShootingSpeed = this.DEFAULT_SHOOTING_SPEED; | |
f625e57a CZ |
114 | } |
115 | ||
7ba6bc91 CZ |
116 | public double getTargetShootingSpeed() { |
117 | return targetShootingSpeed; | |
f625e57a CZ |
118 | } |
119 | ||
120 | public void reverseIndexWheel() { | |
7ba6bc91 | 121 | this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE); |
f625e57a CZ |
122 | } |
123 | ||
124 | public void runIndexWheel() { | |
7ba6bc91 CZ |
125 | this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE); |
126 | } | |
127 | ||
128 | public double calculateShooterSpeed() { | |
129 | this.wheelController.setSetPoint(targetShootingSpeed); | |
130 | double calculatedShooterIncrement = this.wheelController | |
131 | .calcPID(this.getShooterRPM()); | |
132 | currentShooterMotorValue += calculatedShooterIncrement; | |
133 | return currentShooterMotorValue; | |
134 | } | |
135 | ||
136 | public void initializePIDController() { | |
137 | this.wheelController = new PIDController(wheelP, wheelI, wheelD); | |
138 | this.wheelController.setDoneRange(10); | |
139 | this.wheelController.setMaxOutput(1.0); | |
140 | this.wheelController.setMinDoneCycles(3); | |
141 | this.wheelController.setSetPoint(this.targetShootingSpeed); | |
142 | this.currentShooterMotorValue = 0; | |
381dad77 | 143 | } |
185e0c8d | 144 | |
fc01fb0f CZ |
145 | public void reverseFlyWheel() { |
146 | this.setFlyWheelMotorVal(shooter.REVERSE_FLYWHEEL_MOTOR_VALUE); | |
147 | } | |
79ba119a | 148 | } |