challenge-bot.com Git - 3501/stronghold-2016/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.Lidar;
   5
   6 import edu.wpi.first.wpilibj.CANTalon;
   7 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
   8 import edu.wpi.first.wpilibj.DoubleSolenoid;
   9 import edu.wpi.first.wpilibj.Encoder;
  10 import edu.wpi.first.wpilibj.command.PIDSubsystem;
  11
  12 /***
  13  * The Shooter consists of a platform and wheel, each controlled by separate
  14  * motors. The piston controlling the platform pushes the ball onto the wheel.
  15  * The wheel is controlled by a motor, which is running before the ball is
  16  * pushed onto the wheel. The spinning wheel propels the ball.
  17  *
  18  * @author superuser
  19  *
  20  */
  21
  22 public class Shooter extends PIDSubsystem {
  23   private static double sp = 0.015, si = 0, sd = 0;
  24   private static double encoderTolerance = 5.0;
  25   private static double pidOutput = 5.0;
  26
  27   private CANTalon shooter;
  28   private DoubleSolenoid hood, punch;
  29   private Encoder encoder;
  30   private Lidar lidar;
  31
  32   public Shooter() {
  33     super(sp, si, sd);
  34
  35     shooter = new CANTalon(Constants.Shooter.PORT);
  36     hood = new DoubleSolenoid(Constants.Shooter.HOOD_FORWARD,
  37         Constants.Shooter.HOOD_REVERSE);
  38     punch = new DoubleSolenoid(Constants.Shooter.PUNCH_FORWARD,
  39         Constants.Shooter.PUNCH_REVERSE);
  40
  41     encoder = new Encoder(Constants.Shooter.ENCODER_PORT_A,
  42         Constants.Shooter.ENCODER_PORT_B, false, EncodingType.k4X);
  43
  44     lidar = new Lidar(Constants.Shooter.LIDAR_I2C_PORT);
  45   }
  46
  47   /***
  48    * This method checks to see if the ball has successfully passed through the
  49    * intake rollers and is inside.
  50    *
  51    * @return whether the presence of the ball is true or false and returns the
  52    *         state of the condition (true or false).
  53    */
  54
  55   public boolean isBallInside() {
  56     return true;
  57   }
  58
  59   public void setSpeed(double speed) {
  60     setShooterPID();
  61     setSetpoint(speed);
  62   }
  63
  64   // This getDistance() will return the distance using the lidar from the
  65   // desired target during match. This distance is returned in units of
  66   // CENTIMETERS.
  67   public double getDistance() {
  68     return lidar.getDistance();
  69   }
  70
  71   public void stop() {
  72     this.setSpeed(0.0);
  73   }
  74
  75   public double getSpeed() {
  76     return encoder.getRate();
  77   }
  78
  79   // Use negative # for decrement. Positive for increment.
  80
  81   public void changeSpeed(double change) {
  82     double newSpeed = getSpeed() + change;
  83     setSpeed(newSpeed);
  84   }
  85
  86   // Punch Commands
  87   public void punch() {
  88     punch.set(Constants.Shooter.punch);
  89   }
  90
  91   public void retractPunch() {
  92     punch.set(Constants.Shooter.retract);
  93   }
  94
  95   public boolean isHoodOpen() {
  96     return hood.get() == Constants.Shooter.open;
  97   }
  98
  99   public void openHood() {
 100     hood.set(Constants.Shooter.open);
 101   }
 102
 103   public void closeHood() {
 104     hood.set(Constants.Shooter.closed);
 105   }
 106
 107   @Override
 108   protected void initDefaultCommand() {
 109   }
 110
 111   @Override
 112   protected double returnPIDInput() {
 113     return sensorFeedback();
 114   }
 115
 116   @Override
 117   protected void usePIDOutput(double output) {
 118     shooter.set(output);
 119     pidOutput = output;
 120   }
 121
 122   private double sensorFeedback() {
 123     return this.getSpeed();
 124   }
 125
 126   public void setShooterPID() {
 127     this.updatePID();
 128     this.setAbsoluteTolerance(encoderTolerance);
 129     this.setOutputRange(-1.0, 1.0);
 130     this.setInputRange(-500.0, 500.0);
 131     this.enable();
 132   }
 133
 134   public void updatePID() {
 135     this.getPIDController().setPID(sp, si, sd);
 136   }
 137 }