1 package org
.usfirst
.frc
.team3501
.robot
.subsystems
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
;
4 import org
.usfirst
.frc
.team3501
.robot
.MathLib
;
5 import org
.usfirst
.frc
.team3501
.robot
.utils
.HallEffectSensor
;
6 import org
.usfirst
.frc
.team3501
.robot
.utils
.PIDController
;
8 import com
.ctre
.CANTalon
;
10 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
;
11 import edu
.wpi
.first
.wpilibj
.DoubleSolenoid
.Value
;
12 import edu
.wpi
.first
.wpilibj
.command
.Subsystem
;
14 public class Shooter
extends Subsystem
{
15 public double wheelP
= 0.00007, wheelI
= 0, wheelD
= 0.0008;
16 private static Shooter shooter
;
17 private static HallEffectSensor hallEffect
;
18 private final CANTalon flyWheel1
, flyWheel2
, indexWheel
;
20 private PIDController wheelController
;
22 private static final double RPM_THRESHOLD
= 10;
23 private static final double DEFAULT_INDEXING_MOTOR_VALUE
= 0.75;
24 private static final double DEFAULT_SHOOTING_SPEED
= 2800; // rpm
25 private static final double SHOOTING_SPEED_INCREMENT
= 50;
27 private double targetShootingSpeed
= DEFAULT_SHOOTING_SPEED
;
28 private double currentShooterMotorValue
= 0;
30 private final DoubleSolenoid piston
;
33 flyWheel1
= new CANTalon(Constants
.Shooter
.FLY_WHEEL1
);
34 flyWheel2
= new CANTalon(Constants
.Shooter
.FLY_WHEEL2
);
35 indexWheel
= new CANTalon(Constants
.Shooter
.INDEX_WHEEL
);
37 hallEffect
= new HallEffectSensor(Constants
.Shooter
.HALL_EFFECT_PORT
, 1);
39 piston
= new DoubleSolenoid(Constants
.Shooter
.MODULE_NUMBER
,
40 Constants
.Shooter
.PISTON_FORWARD
, Constants
.Shooter
.PISTON_REVERSE
);
44 * Returns shooter object
46 * @return Shooter object
48 public static Shooter
getShooter() {
49 if (shooter
== null) {
50 shooter
= new Shooter();
56 * Sets fly wheel motor value to input.
59 * motor value from -1 to 1(fastest forward)
61 public void setFlyWheelMotorVal(double val
) {
62 val
= MathLib
.restrictToRange(val
, 0.0, 1.0);
68 * Stops fly wheel motor.
70 public void stopFlyWheel() {
76 * Sets index wheel motor value to input.
79 * motor value from -1 to 1(fastest forward)
81 public void setIndexWheelMotorVal(double val
) {
82 val
= MathLib
.restrictToRange(val
, -1.0, 1.0);
87 * Stops index wheel motor.
89 public void stopIndexWheel() {
94 protected void initDefaultCommand() {
98 public double getRPMThreshold() {
102 public double getShooterRPM() {
103 return hallEffect
.getRPM();
106 public void setTargetShootingSpeed(double Value
) {
107 targetShootingSpeed
= Value
;
110 public void decrementTargetShootingSpeed() {
111 this.targetShootingSpeed
-= this.SHOOTING_SPEED_INCREMENT
;
114 public void incrementTargetShootingSpeed() {
115 this.targetShootingSpeed
+= this.SHOOTING_SPEED_INCREMENT
;
118 public void resetTargetShootingSpeed() {
119 this.targetShootingSpeed
= this.DEFAULT_SHOOTING_SPEED
;
122 public double getTargetShootingSpeed() {
123 return targetShootingSpeed
;
126 public void reverseIndexWheel() {
127 this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE
);
130 public void runIndexWheel() {
131 this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE
);
134 public double calculateShooterSpeed() {
135 this.wheelController
.setSetPoint(targetShootingSpeed
);
136 double calculatedShooterIncrement
= this.wheelController
137 .calcPID(this.getShooterRPM());
138 currentShooterMotorValue
+= calculatedShooterIncrement
;
139 return currentShooterMotorValue
;
142 public void initializePIDController() {
143 this.wheelController
= new PIDController(wheelP
, wheelI
, wheelD
);
144 this.wheelController
.setDoneRange(10);
145 this.wheelController
.setMaxOutput(1.0);
146 this.wheelController
.setMinDoneCycles(3);
147 this.wheelController
.setSetPoint(this.targetShootingSpeed
);
148 this.currentShooterMotorValue
= 0;
151 public Value
getPistonValue() {
155 public void setHighGear() {
156 changeGear(Constants
.Shooter
.HIGH_GEAR
);
159 public void setLowGear() {
160 changeGear(Constants
.Shooter
.LOW_GEAR
);
163 private void changeGear(DoubleSolenoid
.Value gear
) {