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; | |
21 | private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75; | |
b70398a7 | 22 | private static final double DEFAULT_SHOOTING_SPEED = 2800; // rpm |
f625e57a | 23 | private static final double SHOOTING_SPEED_INCREMENT = 25; |
ad7e6b1e | 24 | |
7ba6bc91 CZ |
25 | private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; |
26 | private double currentShooterMotorValue = 0; | |
ad7e6b1e | 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); |
34 | } | |
35 | ||
04227dc0 | 36 | /** |
37 | * Returns shooter object | |
38 | * | |
39 | * @return Shooter object | |
40 | */ | |
079a8cb6 CZ |
41 | public static Shooter getShooter() { |
42 | if (shooter == null) { | |
43 | shooter = new Shooter(); | |
44 | } | |
45 | return shooter; | |
46 | } | |
41dfad94 | 47 | |
079a8cb6 | 48 | /** |
04227dc0 | 49 | * Sets fly wheel motor value to input. |
50 | * | |
51 | * @param val | |
52 | * motor value from -1 to 1(fastest forward) | |
079a8cb6 | 53 | */ |
ac77a7b8 CZ |
54 | public void setFlyWheelMotorVal(double val) { |
55 | val = MathLib.restrictToRange(val, 0.0, 1.0); | |
3a86b1a5 CZ |
56 | flyWheel1.set(val); |
57 | flyWheel2.set(val); | |
079a8cb6 | 58 | } |
41dfad94 | 59 | |
04227dc0 | 60 | /** |
61 | * Stops fly wheel motor. | |
62 | */ | |
63 | public void stopFlyWheel() { | |
3a86b1a5 CZ |
64 | flyWheel1.set(0); |
65 | flyWheel2.set(0); | |
04227dc0 | 66 | } |
41dfad94 | 67 | |
04227dc0 | 68 | /** |
69 | * Sets index wheel motor value to input. | |
70 | * | |
71 | * @param val | |
72 | * motor value from -1 to 1(fastest forward) | |
73 | */ | |
ac77a7b8 CZ |
74 | public void setIndexWheelMotorVal(double val) { |
75 | val = MathLib.restrictToRange(val, -1.0, 1.0); | |
04227dc0 | 76 | indexWheel.set(val); |
079a8cb6 | 77 | } |
41dfad94 | 78 | |
079a8cb6 | 79 | /** |
04227dc0 | 80 | * Stops index wheel motor. |
079a8cb6 CZ |
81 | */ |
82 | public void stopIndexWheel() { | |
04227dc0 | 83 | indexWheel.set(0); |
079a8cb6 | 84 | } |
41dfad94 | 85 | |
f625e57a CZ |
86 | @Override |
87 | protected void initDefaultCommand() { | |
88 | ||
89 | } | |
90 | ||
7ba6bc91 CZ |
91 | public double getRPMThreshold() { |
92 | return RPM_THRESHOLD; | |
93 | } | |
94 | ||
f625e57a CZ |
95 | public double getShooterRPM() { |
96 | return hallEffect.getRPM(); | |
09e509d3 NA |
97 | } |
98 | ||
7ba6bc91 CZ |
99 | public void setTargetShootingSpeed(double Value) { |
100 | targetShootingSpeed = Value; | |
09e509d3 NA |
101 | } |
102 | ||
7ba6bc91 CZ |
103 | public void decrementTargetShootingSpeed() { |
104 | this.targetShootingSpeed -= this.SHOOTING_SPEED_INCREMENT; | |
f625e57a | 105 | } |
079a8cb6 | 106 | |
7ba6bc91 CZ |
107 | public void incrementTargetShootingSpeed() { |
108 | this.targetShootingSpeed += this.SHOOTING_SPEED_INCREMENT; | |
079a8cb6 | 109 | } |
381dad77 | 110 | |
7ba6bc91 CZ |
111 | public void resetTargetShootingSpeed() { |
112 | this.targetShootingSpeed = this.DEFAULT_SHOOTING_SPEED; | |
f625e57a CZ |
113 | } |
114 | ||
7ba6bc91 CZ |
115 | public double getTargetShootingSpeed() { |
116 | return targetShootingSpeed; | |
f625e57a CZ |
117 | } |
118 | ||
119 | public void reverseIndexWheel() { | |
7ba6bc91 | 120 | this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE); |
f625e57a CZ |
121 | } |
122 | ||
123 | public void runIndexWheel() { | |
7ba6bc91 CZ |
124 | this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE); |
125 | } | |
126 | ||
127 | public double calculateShooterSpeed() { | |
128 | this.wheelController.setSetPoint(targetShootingSpeed); | |
129 | double calculatedShooterIncrement = this.wheelController | |
130 | .calcPID(this.getShooterRPM()); | |
131 | currentShooterMotorValue += calculatedShooterIncrement; | |
132 | return currentShooterMotorValue; | |
133 | } | |
134 | ||
135 | public void initializePIDController() { | |
136 | this.wheelController = new PIDController(wheelP, wheelI, wheelD); | |
137 | this.wheelController.setDoneRange(10); | |
138 | this.wheelController.setMaxOutput(1.0); | |
139 | this.wheelController.setMinDoneCycles(3); | |
140 | this.wheelController.setSetPoint(this.targetShootingSpeed); | |
141 | this.currentShooterMotorValue = 0; | |
381dad77 | 142 | } |
79ba119a | 143 | } |