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