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