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
.command
.Subsystem
;
12 public class Shooter
extends Subsystem
{
13 public double wheelP
= 0.00007, wheelI
= 0, wheelD
= 0.0008;
14 private static Shooter shooter
;
15 private static HallEffectSensor hallEffect
;
16 private final CANTalon flyWheel1
, flyWheel2
, indexWheel
;
18 private PIDController wheelController
;
20 private static final double RPM_THRESHOLD
= 10;
21 public static final double DEFAULT_INDEXING_MOTOR_VALUE
= 0.75;
22 public static final double REVERSE_FLYWHEEL_MOTOR_VALUE
= -0.75;
23 private static final double DEFAULT_SHOOTING_SPEED
= 2875; // rpm
24 private static final double SHOOTING_SPEED_INCREMENT
= 25;
26 private double targetShootingSpeed
= DEFAULT_SHOOTING_SPEED
;
27 private double currentShooterMotorValue
= 0;
30 flyWheel1
= new CANTalon(Constants
.Shooter
.FLY_WHEEL1
);
31 flyWheel2
= new CANTalon(Constants
.Shooter
.FLY_WHEEL2
);
32 indexWheel
= new CANTalon(Constants
.Shooter
.INDEX_WHEEL
);
34 hallEffect
= new HallEffectSensor(Constants
.Shooter
.HALL_EFFECT_PORT
, 1);
38 * Returns shooter object
40 * @return Shooter object
42 public static Shooter
getShooter() {
43 if (shooter
== null) {
44 shooter
= new Shooter();
50 * Sets fly wheel motor value to input.
53 * motor value from -1 to 1(fastest forward)
55 public void setFlyWheelMotorVal(double val
) {
56 val
= MathLib
.restrictToRange(val
, -1.0, 1.0);
62 * Stops fly wheel motor.
64 public void stopFlyWheel() {
70 * Sets index wheel motor value to input.
73 * motor value from -1 to 1(fastest forward)
75 public void setIndexWheelMotorVal(double val
) {
76 val
= MathLib
.restrictToRange(val
, -1.0, 1.0);
81 * Stops index wheel motor.
83 public void stopIndexWheel() {
88 protected void initDefaultCommand() {
92 public double getRPMThreshold() {
96 public double getShooterRPM() {
97 return hallEffect
.getRPM();
100 public void setTargetShootingSpeed(double Value
) {
101 targetShootingSpeed
= Value
;
104 public void decrementTargetShootingSpeed() {
105 this.targetShootingSpeed
-= this.SHOOTING_SPEED_INCREMENT
;
108 public void incrementTargetShootingSpeed() {
109 this.targetShootingSpeed
+= this.SHOOTING_SPEED_INCREMENT
;
112 public void resetTargetShootingSpeed() {
113 this.targetShootingSpeed
= this.DEFAULT_SHOOTING_SPEED
;
116 public double getTargetShootingSpeed() {
117 return targetShootingSpeed
;
120 public void reverseIndexWheel() {
121 this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE
);
124 public void runIndexWheel() {
125 this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE
);
128 public double calculateShooterSpeed() {
129 this.wheelController
.setSetPoint(targetShootingSpeed
);
130 double calculatedShooterIncrement
= this.wheelController
131 .calcPID(this.getShooterRPM());
132 currentShooterMotorValue
+= calculatedShooterIncrement
;
133 return currentShooterMotorValue
;
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;
145 public void reverseFlyWheel() {
146 this.setFlyWheelMotorVal(shooter
.REVERSE_FLYWHEEL_MOTOR_VALUE
);