Shooter

src/main/java/frc/robot/Constants.java

@@ -16,4 +16,16 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
+    public static class Shooter {
+        public static final double DISTANCE = 5.25;
+        public static final double SPEED = 35.668789;
+        public static final double kP = 0;
+        public static final double kI = 0;
+        public static final double kD = 0;
+        public static final double ANGLE = 30; // 30 degrees
+        public static final double INIT_HEIGHT = 0;
+        public static final double WHEEL_RADIUS = 0;
+        public static final double TICKS = 4096;
+    }
 }

src/main/java/frc/robot/Ports.java

@@ -0,0 +1,10 @@
+package frc.robot;
+
+public class Ports {
+
+    public static class Shooter{
+        public static final int portMain = 0;
+        public static final int portAux1 = 0;
+        public static final int portAux2 = 0;
+    }
+}

src/main/java/frc/robot/RobotContainer.java

@@ -11,7 +11,13 @@
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.ExampleSubsystem.ExampleSubsystem;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.commands.Fire;
+
+import java.util.function.BooleanSupplier;
+
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -20,12 +26,16 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
+    public static final XboxController xboxController = new XboxController(1);
+    private final Trigger RT = new Trigger(() -> xboxController.getRawAxis(XboxController.Axis.kRightTrigger.value) > 0.3);
+    Shooter sniper = new Shooter();
 
 
   /**
    * The container for the robot.  Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
+
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -37,6 +47,7 @@ public RobotContainer() {
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+      RT.whileActiveOnce(new Fire(sniper));
   }
 
 

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -0,0 +1,65 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.UnitModel;
+
+import static frc.robot.Ports.Shooter.*;
+import static frc.robot.Constants.Shooter.*;
+
+/**
+ * This is the class used to do all necessary calculations
+ * for the shooter
+ *
+ * motorMain - the leading motor, all calculations are done
+ * based on this motor
+ *
+ * motorAux1/2 - these two motors follow motorMain
+ */
+public class Shooter extends SubsystemBase {
+    private final TalonSRX motorMain = new TalonSRX(portMain);
+    private final TalonSRX motorAux1 = new TalonSRX(portAux1);
+    private final TalonSRX motorAux2 = new TalonSRX(portAux2);
+    public static UnitModel unitMan = new UnitModel(TICKS);
+
+    /**
+     * Here the constants for the PID is set for the TalonSRX
+     * computer, and the aux motors are set to follow
+     */
+    public Shooter() {
+        motorMain.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder);
+        motorAux1.follow(motorMain);
+        motorAux2.follow(motorMain);
+        motorMain.config_kP(0, kP);
+        motorMain.config_kI(0, kI);
+        motorMain.config_kD(0, kD);
+    }
+
+
+    /**
+     * This function gets the velocity from the TalonSRX,
+     * and converts it to the required units
+     * @return the velocity in rps
+     */
+    public double getVelocity(){
+        double ticks100ms = motorMain.getSelectedSensorVelocity();
+        return unitMan.toVelocity(ticks100ms);
+    }
+
+    /**
+     * This function simply sets the speed for the motor to get to
+     * @param velocity is the target velocity for the motors
+     */
+    public void setVelocity(double velocity){
+        motorMain.set(ControlMode.Velocity, unitMan.toTicks100ms(velocity));
+    }
+
+    /**
+     * This program stops the motors
+     */
+    public void terminate(){
+        motorMain.set(ControlMode.PercentOutput, 0);
+    }
+}

src/main/java/frc/robot/subsystems/shooter/commands/Fire.java

@@ -0,0 +1,89 @@
+package frc.robot.subsystems.shooter.commands;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.shooter.Shooter;
+
+import static frc.robot.Constants.Shooter.*;
+
+/**
+ * This class fires the shooter
+ * sniper - this variable is the shooter
+ * distance - this is the target distance
+ * targetSpeed - this is the speed the motors
+ * need to get to
+ */
+public class Fire extends CommandBase {
+    private Shooter sniper;
+    private double targetSpeed;
+
+    /**
+     * Constructor
+     *
+     * @param sniper is the Shooter
+     */
+    public Fire(Shooter sniper) {
+        this.sniper = sniper;
+        addRequirements(sniper);
+    }
+
+    /**
+     * This function simply gets the value
+     * for the target speed
+     */
+    @Override
+    public void initialize() {
+        targetSpeed = SPEED;
+    }
+
+    /**
+     * This function runs in a loop and sets
+     * the velocity required according to the
+     * Shooter class function - setVelocity
+     */
+    @Override
+    public void execute() {
+        sniper.setVelocity(targetSpeed);
+        RobotContainer.xboxController.setRumble(GenericHID.RumbleType.kLeftRumble, 1);
+    }
+
+    /**
+     * This finishes the program
+     *
+     * @param interrupted is self explanatory
+     */
+    @Override
+    public void end(boolean interrupted) {
+        RobotContainer.xboxController.setRumble(GenericHID.RumbleType.kLeftRumble, 0
+        );
+        sniper.terminate();
+    }
+
+    /**
+     * While the program is running,
+     * it will not be finished and hence:
+     *
+     * @return is always false
+     */
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    /**
+     * This function gets the target speed
+     * required for the motors according to
+     * the distance the ball needs to travel
+     *
+     * @param distance is how far the ball will fly
+     * @return the speed in rpm
+     */
+    public double getTargetSpeed(double distance) {
+        distance = MathUtil.clamp(distance, 1.4, 11); //The camera can't really see beyond these distances, which means they are most likely erroneous.
+        return MathUtil.clamp(.0755 * Math.pow(distance, 4) - 1.38254 * Math.pow(distance, 3) + 8.6493 * Math.pow(distance, 2) - 16.905 * distance + 71.998
+                , 50, 120); //Prevent the shooter from speeding up too much, and from not activating.
+    }
+}