Shooter

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

@@ -16,4 +16,17 @@
  * 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 = 1;
+        public static final double kI = 0;
+        public static final double kD = 1.5;
+        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,13 @@
+package frc.robot;
+
+public class Ports {
+
+    public static class Shooter{
+        public static final int portMain = 23;
+        public static final int portAux1 = 24;
+        public static final int portAux2 = 25;
+        public static final boolean MAIN_INVERTED = false;
+        public static final boolean AUX1_INVERTED = true;
+        public static final boolean AUX2_INVERTED = true;
+    }
+}

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

@@ -11,43 +11,57 @@
 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.JoystickButton;
+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}
  * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-
-
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
-  public RobotContainer() {
-    // Configure the button bindings
-    configureButtonBindings();
-  }
-
-  /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-   */
-  private void configureButtonBindings() {
-  }
-
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-
-    // An ExampleCommand will run in autonomous
-    return null;
-  }
+    // 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);
+    private final JoystickButton a = new JoystickButton(xboxController, XboxController.Button.kA.value);
+    Shooter sniper = new Shooter();
+
+
+    /**
+     * The container for the robot.  Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+
+        // Configure the button bindings
+        configureButtonBindings();
+    }
+
+    /**
+     * Use this method to define your button->command mappings.  Buttons can be created by
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+     * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+//        a.whileHeld(new Fire(sniper));
+        RT.whileActiveOnce(new Fire(sniper));
+    }
+
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+
+        // An ExampleCommand will run in autonomous
+        return null;
+    }
 }

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

@@ -0,0 +1,76 @@
+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 com.ctre.phoenix.motorcontrol.can.VictorSPX;
+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
+ * <p>
+ * motorMain - the leading motor, all calculations are done
+ * based on this motor
+ * <p>
+ * motorAux1/2 - these two motors follow motorMain
+ */
+public class Shooter extends SubsystemBase {
+    private final TalonSRX motorMain = new TalonSRX(portMain);
+    private final VictorSPX motorAux1 = new VictorSPX(portAux1);
+    private final VictorSPX motorAux2 = new VictorSPX(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);
+
+        motorMain.setInverted(MAIN_INVERTED);
+        motorAux1.setInverted(AUX1_INVERTED);
+        motorAux2.setInverted(AUX2_INVERTED);
+    }
+
+
+    /**
+     * 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));
+    }
+
+    public void setPower(double power) {
+        motorMain.set(ControlMode.PercentOutput, power);
+    }
+
+    /**
+     * 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);
+        sniper.setPower(0.5);
+        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.
+    }
+}