diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock
index 20b6e7e..563891a 100644
Binary files a/.gradle/buildOutputCleanup/buildOutputCleanup.lock and b/.gradle/buildOutputCleanup/buildOutputCleanup.lock differ
diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin
deleted file mode 100644
index 4706583..0000000
Binary files a/.gradle/buildOutputCleanup/outputFiles.bin and /dev/null differ
diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml
index a0b9141..9a56aa1 100644
--- a/.idea/jarRepositories.xml
+++ b/.idea/jarRepositories.xml
@@ -34,17 +34,22 @@
-
+
-
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index cf0c82e..14bbd1a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/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;
+
+ }
}
diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java
new file mode 100644
index 0000000..8d906a3
--- /dev/null
+++ b/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;
+ }
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ed3b7fc..6fa66f5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -11,7 +11,14 @@
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}
@@ -19,35 +26,42 @@
* (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;
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
new file mode 100644
index 0000000..96bb852
--- /dev/null
+++ b/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
+ *
+ * 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 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);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/commands/Fire.java b/src/main/java/frc/robot/subsystems/shooter/commands/Fire.java
new file mode 100644
index 0000000..55ee07a
--- /dev/null
+++ b/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.
+ }
+}