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 private static final double DEFAULT_INDEXING_MOTOR_VALUE
= 0.75;
22 private static final double DEFAULT_SHOOTING_SPEED
= 2800; // rpm
23 private static final double SHOOTING_SPEED_INCREMENT
= 50;
25 private double targetShootingSpeed
= DEFAULT_SHOOTING_SPEED
;
26 private double currentShooterMotorValue
= 0;
29 flyWheel1
= new CANTalon(Constants
.Shooter
.FLY_WHEEL1
);
30 flyWheel2
= new CANTalon(Constants
.Shooter
.FLY_WHEEL2
);
31 indexWheel
= new CANTalon(Constants
.Shooter
.INDEX_WHEEL
);
33 hallEffect
= new HallEffectSensor(Constants
.Shooter
.HALL_EFFECT_PORT
, 1);
37 * Returns shooter object
39 * @return Shooter object
41 public static Shooter
getShooter() {
42 if (shooter
== null) {
43 shooter
= new Shooter();
49 * Sets fly wheel motor value to input.
52 * motor value from -1 to 1(fastest forward)
54 public void setFlyWheelMotorVal(double val
) {
55 val
= MathLib
.restrictToRange(val
, 0.0, 1.0);
61 * Stops fly wheel motor.
63 public void stopFlyWheel() {
69 * Sets index wheel motor value to input.
72 * motor value from -1 to 1(fastest forward)
74 public void setIndexWheelMotorVal(double val
) {
75 val
= MathLib
.restrictToRange(val
, -1.0, 1.0);
80 * Stops index wheel motor.
82 public void stopIndexWheel() {
87 protected void initDefaultCommand() {
91 public double getRPMThreshold() {
95 public double getShooterRPM() {
96 return hallEffect
.getRPM();
99 public void setTargetShootingSpeed(double Value
) {
100 targetShootingSpeed
= Value
;
103 public void decrementTargetShootingSpeed() {
104 this.targetShootingSpeed
-= this.SHOOTING_SPEED_INCREMENT
;
107 public void incrementTargetShootingSpeed() {
108 this.targetShootingSpeed
+= this.SHOOTING_SPEED_INCREMENT
;
111 public void resetTargetShootingSpeed() {
112 this.targetShootingSpeed
= this.DEFAULT_SHOOTING_SPEED
;
115 public double getTargetShootingSpeed() {
116 return targetShootingSpeed
;
119 public void reverseIndexWheel() {
120 this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE
);
123 public void runIndexWheel() {
124 this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE
);
127 public double calculateShooterSpeed() {
128 this.wheelController
.setSetPoint(targetShootingSpeed
);
129 double calculatedShooterIncrement
= this.wheelController
130 .calcPID(this.getShooterRPM());
131 currentShooterMotorValue
+= calculatedShooterIncrement
;
132 return currentShooterMotorValue
;
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;