challenge-bot.com Git - 3501/2017steamworks/blob - src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   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;
   7
   8 import com.ctre.CANTalon;
   9
  10 import edu.wpi.first.wpilibj.command.Subsystem;
  11
  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;
  17
  18   private PIDController wheelController;
  19
  20   private static final double RPM_THRESHOLD = 10;
  21   private static final double DEFAULT_INDEXING_MOTOR_VALUE = 0.75;
  22   private 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;
  25
  26   private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
  27   private double currentShooterMotorValue = 0;
  28
  29   private Shooter() {
  30     flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1);
  31     flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2);
  32     indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL);
  33
  34     hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1);
  35   }
  36
  37   /**
  38    * Returns shooter object
  39    *
  40    * @return Shooter object
  41    */
  42   public static Shooter getShooter() {
  43     if (shooter == null) {
  44       shooter = new Shooter();
  45     }
  46     return shooter;
  47   }
  48
  49   /**
  50    * Sets fly wheel motor value to input.
  51    *
  52    * @param val
  53    *          motor value from -1 to 1(fastest forward)
  54    */
  55   public void setFlyWheelMotorVal(double val) {
  56     val = MathLib.restrictToRange(val, -1.0, 1.0);
  57     flyWheel1.set(val);
  58     flyWheel2.set(val);
  59   }
  60
  61   /**
  62    * Stops fly wheel motor.
  63    */
  64   public void stopFlyWheel() {
  65     flyWheel1.set(0);
  66     flyWheel2.set(0);
  67   }
  68
  69   /**
  70    * Sets index wheel motor value to input.
  71    *
  72    * @param val
  73    *          motor value from -1 to 1(fastest forward)
  74    */
  75   public void setIndexWheelMotorVal(double val) {
  76     val = MathLib.restrictToRange(val, -1.0, 1.0);
  77     indexWheel.set(val);
  78   }
  79
  80   /**
  81    * Stops index wheel motor.
  82    */
  83   public void stopIndexWheel() {
  84     indexWheel.set(0);
  85   }
  86
  87   @Override
  88   protected void initDefaultCommand() {
  89
  90   }
  91
  92   public double getRPMThreshold() {
  93     return RPM_THRESHOLD;
  94   }
  95
  96   public double getShooterRPM() {
  97     return hallEffect.getRPM();
  98   }
  99
 100   public void setTargetShootingSpeed(double Value) {
 101     targetShootingSpeed = Value;
 102   }
 103
 104   public void decrementTargetShootingSpeed() {
 105     this.targetShootingSpeed -= this.SHOOTING_SPEED_INCREMENT;
 106   }
 107
 108   public void incrementTargetShootingSpeed() {
 109     this.targetShootingSpeed += this.SHOOTING_SPEED_INCREMENT;
 110   }
 111
 112   public void resetTargetShootingSpeed() {
 113     this.targetShootingSpeed = this.DEFAULT_SHOOTING_SPEED;
 114   }
 115
 116   public double getTargetShootingSpeed() {
 117     return targetShootingSpeed;
 118   }
 119
 120   public void reverseIndexWheel() {
 121     this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE);
 122   }
 123
 124   public void runIndexWheel() {
 125     this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE);
 126   }
 127
 128   public double calculateShooterSpeed() {
 129     this.wheelController.setSetPoint(targetShootingSpeed);
 130     double calculatedShooterIncrement = this.wheelController
 131         .calcPID(this.getShooterRPM());
 132     currentShooterMotorValue += calculatedShooterIncrement;
 133     return currentShooterMotorValue;
 134   }
 135
 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;
 143   }
 144
 145   public void reverseFlyWheel() {
 146     this.setFlyWheelMotorVal(shooter.REVERSE_FLYWHEEL_MOTOR_VALUE);
 147   }
 148 }