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
= 2700; // rpm
25 private static final double SHOOTING_SPEED_INCREMENT
= 50;
26 private static final int ACCEPTABLE_SHOOTING_DEVIATION
= 300;
28 private double targetShootingSpeed
= DEFAULT_SHOOTING_SPEED
;
29 private double currentShooterMotorValue
= 0;
31 private final DoubleSolenoid piston
;
34 flyWheel1
= new CANTalon(Constants
.Shooter
.FLY_WHEEL1
);
35 flyWheel2
= new CANTalon(Constants
.Shooter
.FLY_WHEEL2
);
36 indexWheel
= new CANTalon(Constants
.Shooter
.INDEX_WHEEL
);
38 hallEffect
= new HallEffectSensor(Constants
.Shooter
.HALL_EFFECT_PORT
, 1);
40 piston
= new DoubleSolenoid(Constants
.Shooter
.MODULE_NUMBER
,
41 Constants
.Shooter
.PISTON_FORWARD
, Constants
.Shooter
.PISTON_REVERSE
);
44 * piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER,
45 * Constants.Shooter.PISTON);
50 * Returns shooter object
52 * @return Shooter object
54 public static Shooter
getShooter() {
55 if (shooter
== null) {
56 shooter
= new Shooter();
62 * Sets fly wheel motor value to input.
65 * motor value from -1 to 1(fastest forward)
67 public void setFlyWheelMotorVal(double val
) {
68 val
= MathLib
.restrictToRange(val
, 0.0, 1.0);
74 * Stops fly wheel motor.
76 public void stopFlyWheel() {
82 * Sets index wheel motor value to input.
85 * motor value from -1 to 1(fastest forward)
87 public void setIndexWheelMotorVal(double val
) {
88 val
= MathLib
.restrictToRange(val
, -1.0, 1.0);
93 * Stops index wheel motor.
95 public void stopIndexWheel() {
100 protected void initDefaultCommand() {
104 public double getRPMThreshold() {
105 return RPM_THRESHOLD
;
108 public double getShooterRPM() {
109 return hallEffect
.getRPM();
112 public void setTargetShootingSpeed(double Value
) {
113 targetShootingSpeed
= Value
;
116 public void decrementTargetShootingSpeed() {
117 this.targetShootingSpeed
-= this.SHOOTING_SPEED_INCREMENT
;
120 public void incrementTargetShootingSpeed() {
121 this.targetShootingSpeed
+= this.SHOOTING_SPEED_INCREMENT
;
124 public void resetTargetShootingSpeed() {
125 this.targetShootingSpeed
= this.DEFAULT_SHOOTING_SPEED
;
128 public double getTargetShootingSpeed() {
129 return targetShootingSpeed
;
132 public void reverseIndexWheel() {
133 this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE
);
136 public void runIndexWheel() {
137 this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE
);
140 public double calculateShooterSpeed() {
141 this.wheelController
.setSetPoint(targetShootingSpeed
);
142 double calculatedShooterIncrement
= this.wheelController
143 .calcPID(this.getShooterRPM());
144 currentShooterMotorValue
+= calculatedShooterIncrement
;
145 return currentShooterMotorValue
;
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;
157 public Value
getPistonValue() {
161 public void setHighGear() {
162 changeGear(Constants
.Shooter
.HIGH_GEAR
);
165 public void setLowGear() {
166 changeGear(Constants
.Shooter
.LOW_GEAR
);
169 private void changeGear(DoubleSolenoid
.Value gear
) {
173 public boolean isShooterRPMAtTargetSpeed() {
174 return isShooterRPMWithinRangeOfTargetSpeed(ACCEPTABLE_SHOOTING_DEVIATION
);
177 public boolean isShooterRPMWithinRangeOfTargetSpeed(int acceptableRPMError
) {
178 double shooterSpeed
= getShooterRPM();
179 if (shooterSpeed
> DEFAULT_SHOOTING_SPEED
- acceptableRPMError
180 && shooterSpeed
< DEFAULT_SHOOTING_SPEED
+ acceptableRPMError
) {