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
   7 import com.ctre.CANTalon;
   8
   9 import edu.wpi.first.wpilibj.DoubleSolenoid;
  10 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  11 import edu.wpi.first.wpilibj.command.Subsystem;
  12
  13 public class Shooter extends Subsystem {
  14   public double wheelP = 0.0001, wheelI = 0, wheelD = 0.0004;
  15   private static Shooter shooter;
  16   private static HallEffectSensor hallEffect;
  17   private final CANTalon flyWheel1, flyWheel2, indexWheel;
  18
  19   private static final double DEFAULT_INDEXING_SPEED = -0.75;
  20   private static final double DEFAULT_SHOOTING_SPEED = 2800; // rpm
  21   private static final double SHOOTING_SPEED_INCREMENT = 25;
  22   private static final int ACCEPTABLE_SHOOTING_DEVIATION = 300;
  23
  24   private double currentShootingSpeed = DEFAULT_SHOOTING_SPEED;
  25
  26   private final DoubleSolenoid piston;
  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     piston = new DoubleSolenoid(Constants.Shooter.PISTON_MODULE,
  36         Constants.Shooter.PISTON_FORWARD, Constants.Shooter.PISTON_REVERSE);
  37   }
  38
  39   /**
  40    * Returns shooter object
  41    *
  42    * @return Shooter object
  43    */
  44   public static Shooter getShooter() {
  45     if (shooter == null) {
  46       shooter = new Shooter();
  47     }
  48     return shooter;
  49   }
  50
  51   /**
  52    * Sets fly wheel motor value to input.
  53    *
  54    * @param val
  55    *          motor value from -1 to 1(fastest forward)
  56    */
  57   public void setFlyWheelMotorVal(double val) {
  58     val = MathLib.restrictToRange(val, 0.0, 1.0);
  59     flyWheel1.set(val);
  60     flyWheel2.set(val);
  61   }
  62
  63   /**
  64    * Stops fly wheel motor.
  65    */
  66   public void stopFlyWheel() {
  67     flyWheel1.set(0);
  68     flyWheel2.set(0);
  69   }
  70
  71   /**
  72    * Sets index wheel motor value to input.
  73    *
  74    * @param val
  75    *          motor value from -1 to 1(fastest forward)
  76    */
  77   public void setIndexWheelMotorVal(double val) {
  78     val = MathLib.restrictToRange(val, -1.0, 1.0);
  79     indexWheel.set(val);
  80   }
  81
  82   /**
  83    * Stops index wheel motor.
  84    */
  85   public void stopIndexWheel() {
  86     indexWheel.set(0);
  87   }
  88
  89   @Override
  90   protected void initDefaultCommand() {
  91
  92   }
  93
  94   public double getShooterRPM() {
  95     return hallEffect.getRPM();
  96   }
  97
  98   public void setCurrentShootingSpeed(double Value) {
  99     currentShootingSpeed = Value;
 100   }
 101
 102   public void decrementCurrentShootingSpeed() {
 103     this.currentShootingSpeed -= this.SHOOTING_SPEED_INCREMENT;
 104   }
 105
 106   public void incrementCurrentShootingSpeed() {
 107     this.currentShootingSpeed += this.SHOOTING_SPEED_INCREMENT;
 108   }
 109
 110   public void resetCurrentShootingSpeed() {
 111     this.currentShootingSpeed = this.DEFAULT_SHOOTING_SPEED;
 112   }
 113
 114   public double getCurrentShootingSpeed() {
 115     return currentShootingSpeed;
 116   }
 117
 118   public void reverseIndexWheel() {
 119     this.setIndexWheelMotorVal(-DEFAULT_INDEXING_SPEED);
 120   }
 121
 122   public void runIndexWheel() {
 123     this.setIndexWheelMotorVal(DEFAULT_INDEXING_SPEED);
 124   }
 125
 126   public Value getPistonValue() {
 127     return piston.get();
 128   }
 129
 130   public void setHighGear() {
 131     changeGear(Constants.Shooter.HIGH_GEAR);
 132   }
 133
 134   public void setLowGear() {
 135     changeGear(Constants.Shooter.LOW_GEAR);
 136   }
 137
 138   private void changeGear(DoubleSolenoid.Value gear) {
 139     piston.set(gear);
 140   }
 141
 142   public boolean isShooterRPMAtTargetSpeed() {
 143     return isShooterRPMWithinRangeOfTargetSpeed(ACCEPTABLE_SHOOTING_DEVIATION);
 144   }
 145
 146   public boolean isShooterRPMWithinRangeOfTargetSpeed(int acceptableRPMError) {
 147     double shooterSpeed = getShooterRPM();
 148     if (shooterSpeed > DEFAULT_SHOOTING_SPEED - acceptableRPMError
 149         && shooterSpeed < DEFAULT_SHOOTING_SPEED + acceptableRPMError) {
 150       return true;
 151     }
 152     return false;
 153   }
 154 }