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 | ||
5ab65eab RR |
10 | import edu.wpi.first.wpilibj.DoubleSolenoid; |
11 | import edu.wpi.first.wpilibj.DoubleSolenoid.Value; | |
04227dc0 | 12 | import edu.wpi.first.wpilibj.command.Subsystem; |
13 | ||
14 | public class Shooter extends Subsystem { | |
7ba6bc91 | 15 | public double wheelP = 0.00007, wheelI = 0, wheelD = 0.0008; |
079a8cb6 | 16 | private static Shooter shooter; |
268b0048 | 17 | private static HallEffectSensor hallEffect; |
3a86b1a5 | 18 | private final CANTalon flyWheel1, flyWheel2, indexWheel; |
79ba119a | 19 | |
7ba6bc91 CZ |
20 | private PIDController wheelController; |
21 | ||
22 | private static final double RPM_THRESHOLD = 10; | |
cb8e86cb | 23 | private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75; |
f2139d95 | 24 | private static final double DEFAULT_SHOOTING_SPEED = 2700; // rpm |
f56e6ebf | 25 | private static final double SHOOTING_SPEED_INCREMENT = 50; |
dd4a8793 | 26 | private static final int ACCEPTABLE_SHOOTING_DEVIATION = 300; |
ad7e6b1e | 27 | |
7ba6bc91 CZ |
28 | private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; |
29 | private double currentShooterMotorValue = 0; | |
ad7e6b1e | 30 | |
e4082bee | 31 | private final DoubleSolenoid piston; |
307be8c9 | 32 | |
079a8cb6 | 33 | private Shooter() { |
3a86b1a5 CZ |
34 | flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1); |
35 | flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2); | |
04227dc0 | 36 | indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL); |
ad7e6b1e | 37 | |
268b0048 | 38 | hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1); |
307be8c9 | 39 | |
82fa994f | 40 | piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER, |
307be8c9 | 41 | Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE); |
f2139d95 RR |
42 | |
43 | /* | |
44 | * piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER, | |
45 | * Constants.Shooter.PISTON); | |
46 | */ | |
268b0048 | 47 | } |
48 | ||
04227dc0 | 49 | /** |
50 | * Returns shooter object | |
51 | * | |
52 | * @return Shooter object | |
53 | */ | |
079a8cb6 CZ |
54 | public static Shooter getShooter() { |
55 | if (shooter == null) { | |
56 | shooter = new Shooter(); | |
57 | } | |
58 | return shooter; | |
59 | } | |
41dfad94 | 60 | |
079a8cb6 | 61 | /** |
04227dc0 | 62 | * Sets fly wheel motor value to input. |
63 | * | |
64 | * @param val | |
65 | * motor value from -1 to 1(fastest forward) | |
079a8cb6 | 66 | */ |
ac77a7b8 CZ |
67 | public void setFlyWheelMotorVal(double val) { |
68 | val = MathLib.restrictToRange(val, 0.0, 1.0); | |
3a86b1a5 CZ |
69 | flyWheel1.set(val); |
70 | flyWheel2.set(val); | |
079a8cb6 | 71 | } |
41dfad94 | 72 | |
04227dc0 | 73 | /** |
74 | * Stops fly wheel motor. | |
75 | */ | |
76 | public void stopFlyWheel() { | |
3a86b1a5 CZ |
77 | flyWheel1.set(0); |
78 | flyWheel2.set(0); | |
04227dc0 | 79 | } |
41dfad94 | 80 | |
04227dc0 | 81 | /** |
82 | * Sets index wheel motor value to input. | |
83 | * | |
84 | * @param val | |
85 | * motor value from -1 to 1(fastest forward) | |
86 | */ | |
ac77a7b8 CZ |
87 | public void setIndexWheelMotorVal(double val) { |
88 | val = MathLib.restrictToRange(val, -1.0, 1.0); | |
04227dc0 | 89 | indexWheel.set(val); |
079a8cb6 | 90 | } |
41dfad94 | 91 | |
079a8cb6 | 92 | /** |
04227dc0 | 93 | * Stops index wheel motor. |
079a8cb6 CZ |
94 | */ |
95 | public void stopIndexWheel() { | |
04227dc0 | 96 | indexWheel.set(0); |
079a8cb6 | 97 | } |
41dfad94 | 98 | |
f625e57a CZ |
99 | @Override |
100 | protected void initDefaultCommand() { | |
101 | ||
102 | } | |
103 | ||
7ba6bc91 CZ |
104 | public double getRPMThreshold() { |
105 | return RPM_THRESHOLD; | |
106 | } | |
107 | ||
f625e57a CZ |
108 | public double getShooterRPM() { |
109 | return hallEffect.getRPM(); | |
09e509d3 NA |
110 | } |
111 | ||
7ba6bc91 CZ |
112 | public void setTargetShootingSpeed(double Value) { |
113 | targetShootingSpeed = Value; | |
09e509d3 NA |
114 | } |
115 | ||
7ba6bc91 CZ |
116 | public void decrementTargetShootingSpeed() { |
117 | this.targetShootingSpeed -= this.SHOOTING_SPEED_INCREMENT; | |
f625e57a | 118 | } |
079a8cb6 | 119 | |
7ba6bc91 CZ |
120 | public void incrementTargetShootingSpeed() { |
121 | this.targetShootingSpeed += this.SHOOTING_SPEED_INCREMENT; | |
079a8cb6 | 122 | } |
381dad77 | 123 | |
7ba6bc91 CZ |
124 | public void resetTargetShootingSpeed() { |
125 | this.targetShootingSpeed = this.DEFAULT_SHOOTING_SPEED; | |
f625e57a CZ |
126 | } |
127 | ||
7ba6bc91 CZ |
128 | public double getTargetShootingSpeed() { |
129 | return targetShootingSpeed; | |
f625e57a CZ |
130 | } |
131 | ||
132 | public void reverseIndexWheel() { | |
7ba6bc91 | 133 | this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE); |
f625e57a CZ |
134 | } |
135 | ||
136 | public void runIndexWheel() { | |
7ba6bc91 CZ |
137 | this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE); |
138 | } | |
139 | ||
140 | public double calculateShooterSpeed() { | |
141 | this.wheelController.setSetPoint(targetShootingSpeed); | |
142 | double calculatedShooterIncrement = this.wheelController | |
143 | .calcPID(this.getShooterRPM()); | |
144 | currentShooterMotorValue += calculatedShooterIncrement; | |
145 | return currentShooterMotorValue; | |
146 | } | |
147 | ||
148 | public void initializePIDController() { | |
149 | this.wheelController = new PIDController(wheelP, wheelI, wheelD); | |
150 | this.wheelController.setDoneRange(10); | |
151 | this.wheelController.setMaxOutput(1.0); | |
152 | this.wheelController.setMinDoneCycles(3); | |
153 | this.wheelController.setSetPoint(this.targetShootingSpeed); | |
154 | this.currentShooterMotorValue = 0; | |
381dad77 | 155 | } |
5ab65eab RR |
156 | |
157 | public Value getPistonValue() { | |
158 | return piston.get(); | |
159 | } | |
160 | ||
161 | public void setHighGear() { | |
162 | changeGear(Constants.Shooter.HIGH_GEAR); | |
163 | } | |
164 | ||
165 | public void setLowGear() { | |
166 | changeGear(Constants.Shooter.LOW_GEAR); | |
167 | } | |
168 | ||
169 | private void changeGear(DoubleSolenoid.Value gear) { | |
170 | piston.set(gear); | |
171 | } | |
dd4a8793 RR |
172 | |
173 | public boolean isShooterRPMAtTargetSpeed() { | |
174 | return isShooterRPMWithinRangeOfTargetSpeed(ACCEPTABLE_SHOOTING_DEVIATION); | |
175 | } | |
176 | ||
177 | public boolean isShooterRPMWithinRangeOfTargetSpeed(int acceptableRPMError) { | |
178 | double shooterSpeed = getShooterRPM(); | |
179 | if (shooterSpeed > DEFAULT_SHOOTING_SPEED - acceptableRPMError | |
180 | && shooterSpeed < DEFAULT_SHOOTING_SPEED + acceptableRPMError) { | |
181 | return true; | |
182 | } | |
183 | return false; | |
184 | } | |
79ba119a | 185 | } |