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.DoubleSolenoid;
  11 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  12 import edu.wpi.first.wpilibj.command.Subsystem;
  13
  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;
  19
  20   private PIDController wheelController;
  21
  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 = 3100; // rpm
  25   private static final double SHOOTING_SPEED_INCREMENT = 50;
  26
  27   private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED;
  28   private double currentShooterMotorValue = 0;
  29
  30   private final DoubleSolenoid piston;
  31
  32   private Shooter() {
  33     flyWheel1 = new CANTalon(Constants.Shooter.FLY_WHEEL1);
  34     flyWheel2 = new CANTalon(Constants.Shooter.FLY_WHEEL2);
  35     indexWheel = new CANTalon(Constants.Shooter.INDEX_WHEEL);
  36
  37     hallEffect = new HallEffectSensor(Constants.Shooter.HALL_EFFECT_PORT, 1);
  38
  39     piston = new DoubleSolenoid(Constants.Shooter.MODULE_NUMBER,
  40         Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE);
  41   }
  42
  43   /**
  44    * Returns shooter object
  45    *
  46    * @return Shooter object
  47    */
  48   public static Shooter getShooter() {
  49     if (shooter == null) {
  50       shooter = new Shooter();
  51     }
  52     return shooter;
  53   }
  54
  55   /**
  56    * Sets fly wheel motor value to input.
  57    *
  58    * @param val
  59    *          motor value from -1 to 1(fastest forward)
  60    */
  61   public void setFlyWheelMotorVal(double val) {
  62     val = MathLib.restrictToRange(val, 0.0, 1.0);
  63     flyWheel1.set(val);
  64     flyWheel2.set(val);
  65   }
  66
  67   /**
  68    * Stops fly wheel motor.
  69    */
  70   public void stopFlyWheel() {
  71     flyWheel1.set(0);
  72     flyWheel2.set(0);
  73   }
  74
  75   /**
  76    * Sets index wheel motor value to input.
  77    *
  78    * @param val
  79    *          motor value from -1 to 1(fastest forward)
  80    */
  81   public void setIndexWheelMotorVal(double val) {
  82     val = MathLib.restrictToRange(val, -1.0, 1.0);
  83     indexWheel.set(val);
  84   }
  85
  86   /**
  87    * Stops index wheel motor.
  88    */
  89   public void stopIndexWheel() {
  90     indexWheel.set(0);
  91   }
  92
  93   @Override
  94   protected void initDefaultCommand() {
  95
  96   }
  97
  98   public double getRPMThreshold() {
  99     return RPM_THRESHOLD;
 100   }
 101
 102   public double getShooterRPM() {
 103     return hallEffect.getRPM();
 104   }
 105
 106   public void setTargetShootingSpeed(double Value) {
 107     targetShootingSpeed = Value;
 108   }
 109
 110   public void decrementTargetShootingSpeed() {
 111     this.targetShootingSpeed -= this.SHOOTING_SPEED_INCREMENT;
 112   }
 113
 114   public void incrementTargetShootingSpeed() {
 115     this.targetShootingSpeed += this.SHOOTING_SPEED_INCREMENT;
 116   }
 117
 118   public void resetTargetShootingSpeed() {
 119     this.targetShootingSpeed = this.DEFAULT_SHOOTING_SPEED;
 120   }
 121
 122   public double getTargetShootingSpeed() {
 123     return targetShootingSpeed;
 124   }
 125
 126   public void reverseIndexWheel() {
 127     this.setIndexWheelMotorVal(-DEFAULT_INDEXING_MOTOR_VALUE);
 128   }
 129
 130   public void runIndexWheel() {
 131     this.setIndexWheelMotorVal(DEFAULT_INDEXING_MOTOR_VALUE);
 132   }
 133
 134   public double calculateShooterSpeed() {
 135     this.wheelController.setSetPoint(targetShootingSpeed);
 136     double calculatedShooterIncrement = this.wheelController
 137         .calcPID(this.getShooterRPM());
 138     currentShooterMotorValue += calculatedShooterIncrement;
 139     return currentShooterMotorValue;
 140   }
 141
 142   public void initializePIDController() {
 143     this.wheelController = new PIDController(wheelP, wheelI, wheelD);
 144     this.wheelController.setDoneRange(10);
 145     this.wheelController.setMaxOutput(1.0);
 146     this.wheelController.setMinDoneCycles(3);
 147     this.wheelController.setSetPoint(this.targetShootingSpeed);
 148     this.currentShooterMotorValue = 0;
 149   }
 150
 151   public Value getPistonValue() {
 152     return piston.get();
 153   }
 154
 155   public void setHighGear() {
 156     changeGear(Constants.Shooter.HIGH_GEAR);
 157   }
 158
 159   public void setLowGear() {
 160     changeGear(Constants.Shooter.LOW_GEAR);
 161   }
 162
 163   private void changeGear(DoubleSolenoid.Value gear) {
 164     piston.set(gear);
 165   }
 166 }