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