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; |
04227dc0 | 6 | |
7 | import com.ctre.CANTalon; | |
8 | ||
5ab65eab RR |
9 | import edu.wpi.first.wpilibj.DoubleSolenoid; |
10 | import edu.wpi.first.wpilibj.DoubleSolenoid.Value; | |
04227dc0 | 11 | import edu.wpi.first.wpilibj.command.Subsystem; |
12 | ||
13 | public class Shooter extends Subsystem { | |
1782cbad | 14 | public double wheelP = 0.0001, wheelI = 0, wheelD = 0.0004; |
079a8cb6 | 15 | private static Shooter shooter; |
268b0048 | 16 | private static HallEffectSensor hallEffect; |
3a86b1a5 | 17 | private final CANTalon flyWheel1, flyWheel2, indexWheel; |
79ba119a | 18 | |
f625e57a | 19 | private static final double DEFAULT_INDEXING_SPEED = -0.75; |
005d6bf0 | 20 | private static final double DEFAULT_SHOOTING_SPEED = 2800; // rpm |
f625e57a | 21 | private static final double SHOOTING_SPEED_INCREMENT = 25; |
dd4a8793 | 22 | private static final int ACCEPTABLE_SHOOTING_DEVIATION = 300; |
ad7e6b1e | 23 | |
f625e57a | 24 | private double currentShootingSpeed = DEFAULT_SHOOTING_SPEED; |
ad7e6b1e | 25 | |
e4082bee | 26 | private final DoubleSolenoid piston; |
307be8c9 | 27 | |
079a8cb6 | 28 | private Shooter() { |
3a86b1a5 CZ |
29 | flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1); |
30 | flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2); | |
04227dc0 | 31 | indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL); |
ad7e6b1e | 32 | |
268b0048 | 33 | hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1); |
307be8c9 RR |
34 | |
35 | piston = new DoubleSolenoid(Constants.Shooter.PISTON_MODULE, | |
36 | Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE); | |
268b0048 | 37 | } |
38 | ||
04227dc0 | 39 | /** |
40 | * Returns shooter object | |
41 | * | |
42 | * @return Shooter object | |
43 | */ | |
079a8cb6 CZ |
44 | public static Shooter getShooter() { |
45 | if (shooter == null) { | |
46 | shooter = new Shooter(); | |
47 | } | |
48 | return shooter; | |
49 | } | |
41dfad94 | 50 | |
079a8cb6 | 51 | /** |
04227dc0 | 52 | * Sets fly wheel motor value to input. |
53 | * | |
54 | * @param val | |
55 | * motor value from -1 to 1(fastest forward) | |
079a8cb6 | 56 | */ |
ac77a7b8 CZ |
57 | public void setFlyWheelMotorVal(double val) { |
58 | val = MathLib.restrictToRange(val, 0.0, 1.0); | |
3a86b1a5 CZ |
59 | flyWheel1.set(val); |
60 | flyWheel2.set(val); | |
079a8cb6 | 61 | } |
41dfad94 | 62 | |
04227dc0 | 63 | /** |
64 | * Stops fly wheel motor. | |
65 | */ | |
66 | public void stopFlyWheel() { | |
3a86b1a5 CZ |
67 | flyWheel1.set(0); |
68 | flyWheel2.set(0); | |
04227dc0 | 69 | } |
41dfad94 | 70 | |
04227dc0 | 71 | /** |
72 | * Sets index wheel motor value to input. | |
73 | * | |
74 | * @param val | |
75 | * motor value from -1 to 1(fastest forward) | |
76 | */ | |
ac77a7b8 CZ |
77 | public void setIndexWheelMotorVal(double val) { |
78 | val = MathLib.restrictToRange(val, -1.0, 1.0); | |
04227dc0 | 79 | indexWheel.set(val); |
079a8cb6 | 80 | } |
41dfad94 | 81 | |
079a8cb6 | 82 | /** |
04227dc0 | 83 | * Stops index wheel motor. |
079a8cb6 CZ |
84 | */ |
85 | public void stopIndexWheel() { | |
04227dc0 | 86 | indexWheel.set(0); |
079a8cb6 | 87 | } |
41dfad94 | 88 | |
f625e57a CZ |
89 | @Override |
90 | protected void initDefaultCommand() { | |
91 | ||
92 | } | |
93 | ||
94 | public double getShooterRPM() { | |
95 | return hallEffect.getRPM(); | |
09e509d3 NA |
96 | } |
97 | ||
98 | public void setCurrentShootingSpeed(double Value) { | |
f625e57a | 99 | currentShootingSpeed = Value; |
09e509d3 NA |
100 | } |
101 | ||
f625e57a CZ |
102 | public void decrementCurrentShootingSpeed() { |
103 | this.currentShootingSpeed -= this.SHOOTING_SPEED_INCREMENT; | |
104 | } | |
079a8cb6 | 105 | |
f625e57a CZ |
106 | public void incrementCurrentShootingSpeed() { |
107 | this.currentShootingSpeed += this.SHOOTING_SPEED_INCREMENT; | |
079a8cb6 | 108 | } |
381dad77 | 109 | |
f625e57a CZ |
110 | public void resetCurrentShootingSpeed() { |
111 | this.currentShootingSpeed = this.DEFAULT_SHOOTING_SPEED; | |
112 | } | |
113 | ||
114 | public double getCurrentShootingSpeed() { | |
115 | return currentShootingSpeed; | |
116 | } | |
117 | ||
118 | public void reverseIndexWheel() { | |
119 | this.setIndexWheelMotorVal(-DEFAULT_INDEXING_SPEED); | |
120 | } | |
121 | ||
122 | public void runIndexWheel() { | |
123 | this.setIndexWheelMotorVal(DEFAULT_INDEXING_SPEED); | |
381dad77 | 124 | } |
5ab65eab RR |
125 | |
126 | public Value getPistonValue() { | |
127 | return piston.get(); | |
128 | } | |
129 | ||
130 | public void setHighGear() { | |
131 | changeGear(Constants.Shooter.HIGH_GEAR); | |
132 | } | |
133 | ||
134 | public void setLowGear() { | |
135 | changeGear(Constants.Shooter.LOW_GEAR); | |
136 | } | |
137 | ||
138 | private void changeGear(DoubleSolenoid.Value gear) { | |
139 | piston.set(gear); | |
140 | } | |
dd4a8793 RR |
141 | |
142 | public boolean isShooterRPMAtTargetSpeed() { | |
143 | return isShooterRPMWithinRangeOfTargetSpeed(ACCEPTABLE_SHOOTING_DEVIATION); | |
144 | } | |
145 | ||
146 | public boolean isShooterRPMWithinRangeOfTargetSpeed(int acceptableRPMError) { | |
147 | double shooterSpeed = getShooterRPM(); | |
148 | if (shooterSpeed > DEFAULT_SHOOTING_SPEED - acceptableRPMError | |
149 | && shooterSpeed < DEFAULT_SHOOTING_SPEED + acceptableRPMError) { | |
150 | return true; | |
151 | } | |
152 | return false; | |
153 | } | |
79ba119a | 154 | } |