By extending this class, developers can use more intuitive method names + * that align with the specific actions associated with each button on the + * controller. This aims to enhance code readability and understanding, making it + * easier for developers to work with the controller in a more expressive manner.
+ * + *Example usage:
+ *
+ * {@code
+ * CommandController driver = new CommandController(0);
+ * driver.greenButton().onTrue(() -> {}); // insert command
+ *
+ * @author Simbotics
+ *
+ * @see CommandXboxController
+ */
+public class CommandController {
+ CommandXboxController joystick;
+
+ public CommandController(int port) {
+ this.joystick = new CommandXboxController(port);
+ }
+
+ /**
+ * Gets the left joystick X axis.
+ *
+ * @return The left joystick X axis.
+ */
+ public double getLeftX() {
+ return this.joystick.getLeftX();
+ }
+
+ /**
+ * Gets the left joystick Y axis.
+ *
+ * @return The left joystick Y axis.
+ */
+ public double getLeftY() {
+ return -this.joystick.getLeftY();
+ }
+
+ /**
+ * Gets the pressed state of the left trigger.
+ *
+ * @return The pressed state of the left trigger as a trigger.
+ */
+ public Trigger leftTrigger() {
+ return this.joystick.rightTrigger();
+ }
+
+ /**
+ * Gets the pressed state of the right trigger.
+ *
+ * @return The pressed state of the right trigger as a trigger.
+ */
+ public Trigger rightTrigger() {
+ return this.joystick.rightTrigger();
+ }
+
+ /**
+ * Gets the right joystick X axis.
+ *
+ * @return The right joystick X axis.
+ */
+ public double getRightX() {
+ return this.joystick.getRightX();
+ }
+
+ /**
+ * Gets the right joystick Y axis.
+ *
+ * @return The right joystick Y axis.
+ */
+ public double getRightY() {
+ return -this.joystick.getRightY();
+ }
+
+ /**
+ * Gets the green buttons trigger.
+ *
+ * @return the trigger for the green button
+ */
+ public Trigger greenButton() {
+ return this.joystick.a();
+ }
+
+ /**
+ * Gets the blue buttons trigger.
+ *
+ * @return the trigger for the blue button
+ */
+ public Trigger blueButton() {
+ return this.joystick.x();
+ }
+
+ /**
+ * Gets the red buttons trigger.
+ *
+ * @return the trigger for the red button
+ */
+ public Trigger redButton() {
+ return this.joystick.b();
+ }
+
+ /**
+ * Gets the yellow buttons trigger.
+ *
+ * @return the trigger for the yellow button
+ */
+ public Trigger yellowButton() {
+ return this.joystick.y();
+ }
+
+ /**
+ * Gets the back button trigger
+ *
+ * @return the trigger for the back button
+ */
+ public Trigger backButton() {
+ return this.joystick.back();
+ }
+
+ /**
+ * Gets the back button trigger
+ *
+ * @return the trigger for the back button
+ */
+ public Trigger startButton() {
+ return this.joystick.start();
+ }
+
+ /**
+ * Gets the pressed state of the top left back button.
+ *
+ * @return The value of the pressed state as a trigger.
+ */
+ public Trigger leftBumper() {
+ return this.joystick.leftBumper();
+ }
+
+ /**
+ * Gets the pressed state of the top right back button.
+ *
+ * @return The value of the pressed state as a trigger.
+ */
+ public Trigger rightBumper() {
+ return this.joystick.rightBumper();
+ }
+
+ /**
+ * Gets the pressed state of the left joystick button.
+ *
+ * @return The state of the left joystick button as a trigger.
+ */
+ public Trigger leftStickClick() {
+ return this.joystick.button(9);
+ }
+
+ /**
+ * Gets the pressed state of the right joystick button.
+ *
+ * @return The state of the of the right joystick button as a boolean.
+ */
+ public Trigger rightStickClick() {
+ return this.joystick.button(10);
+ }
+
+ /** POV methods. */
+ public int getPOVVal() {
+ return this.joystick.getHID().getPOV(0);
+ }
+
+ /**
+ * Get trigger for when POV angle is 180
+ *
+ * @return trigger for when POV is 180
+ */
+ public Trigger POVDown() {
+ return this.joystick.povDown();
+ }
+
+ /**
+ * Get trigger for when POV angle is 90
+ *
+ * @return trigger for when POV is 90
+ */
+ public Trigger POVRight() {
+ return this.joystick.povRight();
+ }
+
+ /**
+ * Get trigger for when POV angle is 0
+ *
+ * @return trigger for when POV is 0
+ */
+ public Trigger POVUp() {
+ return this.joystick.povUp();
+ }
+
+ /**
+ * Get trigger for when POV angle is 270
+ *
+ * @return trigger for when POV is 270
+ */
+ public Trigger POVLeft() {
+ return this.joystick.povLeft();
+ }
+
+ /**
+ * Sets the amount of rumble for supported controllers.
+ *
+ * @param rumble Amount of rumble; Either 0 or 1.
+ */
+ public void setRumble(double rumble) {
+ this.joystick.getHID().setRumble(RumbleType.kLeftRumble, rumble);
+ this.joystick.getHID().setRumble(RumbleType.kRightRumble, rumble);
+ }
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8bafc7b..f9fc442 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,8 +4,6 @@
package frc.robot;
-import com.ctre.phoenix6.SignalLogger;
-
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
@@ -24,8 +22,6 @@ public void robotInit() {
m_robotContainer = new RobotContainer();
m_robotContainer.drivetrain.getDaqThread().setThreadPriority(99);
-
- SignalLogger.start();
}
@Override
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 4d4825f..36551bc 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,90 +4,85 @@
package frc.robot;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.intake.enums.IntakeGamepiece;
+import frc.robot.subsystems.intake.states.scoring.cone.HighCone;
+import frc.robot.subsystems.led.LEDSubsystem;
public class RobotContainer {
- final double MaxSpeed = 6; // 6 meters per second desired top speed
- final double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity
+ private class Controller {
+ public static final CommandController driver = new CommandController(0);
+ public static final CommandController operator = new CommandController(1);
+ }
+
+ private final LEDSubsystem ledSubsystem;
+ private final IntakeSubsystem intakeSubsystem;
+
+ private final PowerDistribution pdp = new PowerDistribution();
- /* Setting up bindings for necessary control of the swerve drive platform */
- CommandXboxController joystick = new CommandXboxController(0); // My joystick
- CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
- SwerveRequest.FieldCentric drive =
+ // Set up the base for the drive and drivetrain
+ final DriveSubsystem drivetrain = DriveConstants.DriveTrain;
+ private final SwerveRequest.FieldCentric drive =
new SwerveRequest.FieldCentric()
- .withIsOpenLoop(true)
- .withDeadband(MaxSpeed * 0.1)
- .withRotationalDeadband(MaxAngularRate * 0.1); // I want field-centric
- // driving in open loop
- SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
- SwerveRequest.RobotCentric forwardStraight =
- new SwerveRequest.RobotCentric().withIsOpenLoop(true);
- SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+ .withDeadband(DriveConstants.kMaxAngularRate * 0.1)
+ .withRotationalDeadband(DriveConstants.kMaxAngularRate * 0.1)
+ .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
/* Path follower */
- // Command runAuto = drivetrain.getAutoPath("Tests");
+ private Command runAuto = drivetrain.getAutoPath("Tests");
- Telemetry logger = new Telemetry(MaxSpeed);
-
- Pose2d odomStart = new Pose2d(0, 0, new Rotation2d(0, 0));
+ private final Telemetry logger = new Telemetry(DriveConstants.kMaxSpeed);
private void configureBindings() {
- drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
+ ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
+ drivetrain.registerTelemetry(logger::telemeterize);
+
+ drivetrain.setDefaultCommand(
drivetrain
.applyRequest(
() ->
drive
- .withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with
+ .withVelocityX(
+ -Controller.driver.getLeftY()
+ * DriveConstants.kMaxSpeed) // Drive forward with
// negative Y (forward)
.withVelocityY(
- -joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left)
+ -Controller.driver.getLeftX()
+ * DriveConstants.kMaxSpeed) // Drive left with negative X (left)
.withRotationalRate(
- -joystick.getRightX()
- * MaxAngularRate) // Drive counterclockwise with negative X (left)
+ -Controller.driver.getRightX()
+ * DriveConstants
+ .kMaxAngularRate) // Drive counterclockwise with negative X
+ // (left)
)
.ignoringDisable(true));
- joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
- joystick
- .b()
- .whileTrue(
- drivetrain.applyRequest(
- () ->
- point.withModuleDirection(
- new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
-
- // if (Utils.isSimulation()) {
- // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
- // }
- drivetrain.registerTelemetry(logger::telemeterize);
-
- joystick
- .pov(0)
- .whileTrue(
- drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
- joystick
- .pov(180)
- .whileTrue(
- drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
+ Controller.operator
+ .yellowButton()
+ .toggleOnTrue(intakeSubsystem.intakeHoldCommand(IntakeGamepiece.CONE));
+ Controller.operator
+ .blueButton()
+ .toggleOnTrue(intakeSubsystem.intakeHoldCommand(IntakeGamepiece.CUBE));
+ Controller.operator
+ .greenButton()
+ .toggleOnTrue(intakeSubsystem.intakeScoreCommand(new HighCone(), IntakeGamepiece.CONE));
}
public RobotContainer() {
configureBindings();
+ ledSubsystem = new LEDSubsystem();
+ intakeSubsystem = new IntakeSubsystem(pdp);
}
public Command getAutonomousCommand() {
- /* First put the drivetrain into auto run mode, then run the auto */
- return new InstantCommand(() -> {});
- }
-
- public boolean seedPoseButtonDown() {
- return joystick.leftBumper().getAsBoolean();
+ return runAuto;
}
}
diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java
index 16e9089..bb0861a 100644
--- a/src/main/java/frc/robot/Telemetry.java
+++ b/src/main/java/frc/robot/Telemetry.java
@@ -1,5 +1,6 @@
package frc.robot;
+import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
@@ -10,8 +11,6 @@
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
-import edu.wpi.first.wpilibj.DataLogManager;
-import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -20,8 +19,6 @@
public class Telemetry {
private final double MaxSpeed;
- private int logEntry;
- private int odomEntry;
/**
* Construct a telemetry object, with the specified max speed of the robot
@@ -30,36 +27,36 @@ public class Telemetry {
*/
public Telemetry(double maxSpeed) {
MaxSpeed = maxSpeed;
- logEntry = DataLogManager.getLog().start("odometry", "double[]");
- odomEntry = DataLogManager.getLog().start("odom period", "double");
+ SignalLogger.start();
}
/* What to publish over networktables for telemetry */
- NetworkTableInstance inst = NetworkTableInstance.getDefault();
+ private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
/* Robot pose for field positioning */
- NetworkTable table = inst.getTable("Pose");
- DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
- StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+ private final NetworkTable table = inst.getTable("Pose");
+ private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+ private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
/* Robot speeds for general checking */
- NetworkTable driveStats = inst.getTable("Drive");
- DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
- DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
- DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
- DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();
+ private final NetworkTable driveStats = inst.getTable("Drive");
+ private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
+ private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
+ private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
+ private final DoublePublisher odomFreq =
+ driveStats.getDoubleTopic("Odometry Frequency").publish();
/* Keep a reference of the last pose to calculate the speeds */
- Pose2d m_lastPose = new Pose2d();
- double lastTime = Utils.getCurrentTimeSeconds();
+ private Pose2d m_lastPose = new Pose2d();
+ private double lastTime = Utils.getCurrentTimeSeconds();
/* Mechanisms to represent the swerve module states */
- Mechanism2d[] m_moduleMechanisms =
+ private final Mechanism2d[] m_moduleMechanisms =
new Mechanism2d[] {
new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1),
};
/* A direction and length changing ligament for speed representation */
- MechanismLigament2d[] m_moduleSpeeds =
+ private final MechanismLigament2d[] m_moduleSpeeds =
new MechanismLigament2d[] {
m_moduleMechanisms[0]
.getRoot("RootSpeed", 0.5, 0.5)
@@ -75,7 +72,7 @@ public Telemetry(double maxSpeed) {
.append(new MechanismLigament2d("Speed", 0.5, 0)),
};
/* A direction changing and length constant ligament for module direction */
- MechanismLigament2d[] m_moduleDirections =
+ private final MechanismLigament2d[] m_moduleDirections =
new MechanismLigament2d[] {
m_moduleMechanisms[0]
.getRoot("RootDirection", 0.5, 0.5)
@@ -110,7 +107,7 @@ public void telemeterize(SwerveDriveState state) {
speed.set(velocities.getNorm());
velocityX.set(velocities.getX());
velocityY.set(velocities.getY());
- odomPeriod.set(1.0 / state.OdometryPeriod);
+ odomFreq.set(1.0 / state.OdometryPeriod);
/* Telemeterize the module's states */
for (int i = 0; i < 4; ++i) {
@@ -121,12 +118,8 @@ public void telemeterize(SwerveDriveState state) {
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
}
- DataLogManager.getLog()
- .appendDoubleArray(
- logEntry,
- new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()},
- (long) (Timer.getFPGATimestamp() * 1000000));
- DataLogManager.getLog()
- .appendDouble(odomEntry, state.OdometryPeriod, (long) (Timer.getFPGATimestamp() * 1000000));
+ SignalLogger.writeDoubleArray(
+ "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
+ SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds");
}
}
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java
deleted file mode 100644
index a66c7d8..0000000
--- a/src/main/java/frc/robot/generated/TunerConstants.java
+++ /dev/null
@@ -1,133 +0,0 @@
-package frc.robot.generated;
-
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
-import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SwerveModuleSteerFeedbackType;
-import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
-import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
-
-import edu.wpi.first.math.util.Units;
-import frc.robot.CommandSwerveDrivetrain;
-
-public class TunerConstants {
- static class CustomSlotGains extends Slot0Configs {
- public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) {
- this.kP = kP;
- this.kI = kI;
- this.kD = kD;
- this.kV = kV;
- this.kS = kS;
- }
- }
-
- private static final CustomSlotGains steerGains = new CustomSlotGains(100, 0, 0.05, 0, 0);
- private static final CustomSlotGains driveGains = new CustomSlotGains(3, 0, 0, 0, 0);
-
- private static final double kCoupleRatio = 3.5;
-
- private static final double kDriveGearRatio = 7.363636364;
- private static final double kSteerGearRatio = 15.42857143;
- private static final double kWheelRadiusInches =
- 2.167; // Estimated at first, then fudge-factored to make odom match record
- private static final int kPigeonId = 1;
- private static final boolean kSteerMotorReversed = true;
- private static final String kCANbusName = "rio";
- private static final boolean kInvertLeftSide = false;
- private static final boolean kInvertRightSide = true;
-
- private static double kSteerInertia = 0.00001;
- private static double kDriveInertia = 0.001;
-
- private static final SwerveDrivetrainConstants DrivetrainConstants =
- new SwerveDrivetrainConstants()
- .withPigeon2Id(kPigeonId)
- .withCANbusName(kCANbusName)
- .withSupportsPro(true);
-
- private static final SwerveModuleConstantsFactory ConstantCreator =
- new SwerveModuleConstantsFactory()
- .withDriveMotorGearRatio(kDriveGearRatio)
- .withSteerMotorGearRatio(kSteerGearRatio)
- .withWheelRadius(kWheelRadiusInches)
- .withSlipCurrent(30)
- .withSteerMotorGains(steerGains)
- .withDriveMotorGains(driveGains)
- .withSpeedAt12VoltsMps(
- 6) // Theoretical free speed is 10 meters per second at 12v applied output
- .withSteerInertia(kSteerInertia)
- .withDriveInertia(kDriveInertia)
- .withFeedbackSource(SwerveModuleSteerFeedbackType.FusedCANcoder)
- .withCouplingGearRatio(
- kCoupleRatio) // Every 1 rotation of the azimuth results in couple ratio drive turns
- .withSteerMotorInverted(kSteerMotorReversed);
-
- private static final int kFrontLeftDriveMotorId = 5;
- private static final int kFrontLeftSteerMotorId = 4;
- private static final int kFrontLeftEncoderId = 2;
- private static final double kFrontLeftEncoderOffset = -0.83544921875;
-
- private static final double kFrontLeftXPosInches = 10.5;
- private static final double kFrontLeftYPosInches = 10.5;
- private static final int kFrontRightDriveMotorId = 7;
- private static final int kFrontRightSteerMotorId = 6;
- private static final int kFrontRightEncoderId = 3;
- private static final double kFrontRightEncoderOffset = -0.15234375;
-
- private static final double kFrontRightXPosInches = 10.5;
- private static final double kFrontRightYPosInches = -10.5;
- private static final int kBackLeftDriveMotorId = 1;
- private static final int kBackLeftSteerMotorId = 0;
- private static final int kBackLeftEncoderId = 0;
- private static final double kBackLeftEncoderOffset = -0.4794921875;
-
- private static final double kBackLeftXPosInches = -10.5;
- private static final double kBackLeftYPosInches = 10.5;
- private static final int kBackRightDriveMotorId = 3;
- private static final int kBackRightSteerMotorId = 2;
- private static final int kBackRightEncoderId = 1;
- private static final double kBackRightEncoderOffset = -0.84130859375;
-
- private static final double kBackRightXPosInches = -10.5;
- private static final double kBackRightYPosInches = -10.5;
-
- private static final SwerveModuleConstants FrontLeft =
- ConstantCreator.createModuleConstants(
- kFrontLeftSteerMotorId,
- kFrontLeftDriveMotorId,
- kFrontLeftEncoderId,
- kFrontLeftEncoderOffset,
- Units.inchesToMeters(kFrontLeftXPosInches),
- Units.inchesToMeters(kFrontLeftYPosInches),
- kInvertLeftSide);
- private static final SwerveModuleConstants FrontRight =
- ConstantCreator.createModuleConstants(
- kFrontRightSteerMotorId,
- kFrontRightDriveMotorId,
- kFrontRightEncoderId,
- kFrontRightEncoderOffset,
- Units.inchesToMeters(kFrontRightXPosInches),
- Units.inchesToMeters(kFrontRightYPosInches),
- kInvertRightSide);
- private static final SwerveModuleConstants BackLeft =
- ConstantCreator.createModuleConstants(
- kBackLeftSteerMotorId,
- kBackLeftDriveMotorId,
- kBackLeftEncoderId,
- kBackLeftEncoderOffset,
- Units.inchesToMeters(kBackLeftXPosInches),
- Units.inchesToMeters(kBackLeftYPosInches),
- kInvertLeftSide);
- private static final SwerveModuleConstants BackRight =
- ConstantCreator.createModuleConstants(
- kBackRightSteerMotorId,
- kBackRightDriveMotorId,
- kBackRightEncoderId,
- kBackRightEncoderOffset,
- Units.inchesToMeters(kBackRightXPosInches),
- Units.inchesToMeters(kBackRightYPosInches),
- kInvertRightSide);
-
- public static final CommandSwerveDrivetrain DriveTrain =
- new CommandSwerveDrivetrain(
- DrivetrainConstants, 250, FrontLeft, FrontRight, BackLeft, BackRight);
-}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
new file mode 100644
index 0000000..f0c8855
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java
@@ -0,0 +1,142 @@
+package frc.robot.subsystems.drive;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
+
+import frc.robot.subsystems.drive.swerve.SwerveModuleLocation;
+import frc.robot.subsystems.drive.swerve.SwerveModules;
+
+public class DriveConstants {
+ /** Only touch these if you know what you're doing */
+ private static final DriveSlotGains steerGains = new DriveSlotGains(100.0, 0, 0.05, 0, 0);
+
+ private static final DriveSlotGains driveGains = new DriveSlotGains(3.0, 0, 0, 0, 0);
+
+ public static final double kMaxSpeed = 6.0; // 6 meters per second desired top speed
+ public static final double kMaxAngularRate =
+ Math.PI; // Half a rotation per second max angular velocity
+
+ private static final double kCoupleRatio = 3.5;
+
+ private static final double kWheelRadiusInches =
+ 2.167; // Estimated at first, then fudge-factored to make odom
+ // match record
+
+ private static final String kCANbusName = "rio";
+ private static final int kPigeonId = 1;
+
+ public static final double kSpeedAt12VoltsMps = 5.0;
+ public static final double kSimLoopPeriod = 0.005; // 5 ms
+
+ private static final boolean kSteerMotorReversed = true;
+ public static final boolean kInvertLeftSide = false;
+ public static final boolean kInvertRightSide = true;
+
+ /** The ratios of the drive and steer gears in the swerve modules */
+ private class GearRatio {
+ private static final double kDriveGearRatio = 7.363636364;
+ private static final double kSteerGearRatio = 15.42857143;
+ }
+
+ /**
+ * The simulated voltage necessary to overcome friction for swerve. This is only used for
+ * simulation
+ */
+ private class FrictionVoltage {
+ private static final double kSteerFrictionVoltage = 0.25;
+ private static final double kDriveFrictionVoltage = 0.25;
+ }
+
+ /**
+ * The inertia of the drive and steer motors in the swerve modules. These should be left to
+ * default
+ */
+ private class Intertia {
+ private static final double kSteerInertia = 0.00001;
+ private static final double kDriveInertia = 0.001;
+ }
+
+ /**
+ * The CAN IDs of the drive motors in the swerve modules NOTE: Be sure to change these to the
+ * correct values at the start of the season
+ */
+ public class DriveMotorId {
+ public static final int kFrontLeftDriveMotorId = 5;
+ public static final int kFrontRightDriveMotorId = 7;
+ public static final int kBackLeftDriveMotorId = 1;
+ public static final int kBackRightDriveMotorId = 3;
+ }
+
+ /**
+ * The CAN IDs of the steer motors in the swerve modules NOTE: Be sure to change these to the
+ * correct values at the start of the season
+ */
+ public class SteerMotorId {
+ public static final int kFrontLeftSteerMotorId = 4;
+ public static final int kFrontRightSteerMotorId = 6;
+ public static final int kBackLeftSteerMotorId = 0;
+ public static final int kBackRightSteerMotorId = 2;
+ }
+
+ /**
+ * The CAN IDs of the encoders in the swerve modules NOTE: Be sure to change these to the correct
+ * values at the start of the season
+ */
+ public class EncoderId {
+ public static final int kFrontLeftEncoderId = 2;
+ public static final int kFrontRightEncoderId = 3;
+ public static final int kBackLeftEncoderId = 0;
+ public static final int kBackRightEncoderId = 1;
+ }
+
+ public class EncoderOffset {
+ public static final double kFrontLeftEncoderOffset = -0.83544921875;
+ public static final double kFrontRightEncoderOffset = -0.15234375;
+ public static final double kBackLeftEncoderOffset = -0.4794921875;
+ public static final double kBackRightEncoderOffset = -0.84130859375;
+ }
+
+ /**
+ * The X and Y positions of the swerve modules relative to the center of the robot in inches NOTE:
+ * Be sure to change these to the correct values at the start of the season
+ */
+ public class SwerveModulePosition {
+ public static final SwerveModuleLocation kFrontLeft = new SwerveModuleLocation(10.5, 10.5);
+ public static final SwerveModuleLocation kFrontRight = new SwerveModuleLocation(10.5, -10.5);
+ public static final SwerveModuleLocation kBackLeft = new SwerveModuleLocation(-10.5, 10.5);
+ public static final SwerveModuleLocation kBackRight = new SwerveModuleLocation(-10.5, -10.5);
+ }
+
+ private static final SwerveDrivetrainConstants DrivetrainConstants =
+ new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName);
+
+ public static final SwerveModuleConstantsFactory ConstantCreator =
+ new SwerveModuleConstantsFactory()
+ .withDriveMotorGearRatio(GearRatio.kDriveGearRatio)
+ .withSteerMotorGearRatio(GearRatio.kSteerGearRatio)
+ .withWheelRadius(kWheelRadiusInches)
+ .withSlipCurrent(30)
+ .withSteerMotorGains(steerGains)
+ .withDriveMotorGains(driveGains)
+ .withSpeedAt12VoltsMps(
+ kSpeedAt12VoltsMps) // Theoretical free speed is 10 meters per second at 12v applied
+ // output
+ .withSteerInertia(Intertia.kSteerInertia)
+ .withDriveInertia(Intertia.kDriveInertia)
+ .withFeedbackSource(SteerFeedbackType.FusedCANcoder)
+ .withSteerFrictionVoltage(FrictionVoltage.kSteerFrictionVoltage)
+ .withDriveFrictionVoltage(FrictionVoltage.kDriveFrictionVoltage)
+ .withCouplingGearRatio(
+ kCoupleRatio) // Every 1 rotation of the azimuth results in couple ratio drive turns
+ .withSteerMotorInverted(kSteerMotorReversed);
+
+ /** Construct the final drivetrain object that we interact with */
+ public static final DriveSubsystem DriveTrain =
+ new DriveSubsystem(
+ DrivetrainConstants,
+ SwerveModules.FrontLeft,
+ SwerveModules.FrontRight,
+ SwerveModules.BackLeft,
+ SwerveModules.BackRight);
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSlotGains.java b/src/main/java/frc/robot/subsystems/drive/DriveSlotGains.java
new file mode 100644
index 0000000..2879a83
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSlotGains.java
@@ -0,0 +1,13 @@
+package frc.robot.subsystems.drive;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+
+public class DriveSlotGains extends Slot0Configs {
+ public DriveSlotGains(double kP, double kI, double kD, double kV, double kS) {
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ this.kV = kV;
+ this.kS = kS;
+ }
+}
diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
similarity index 51%
rename from src/main/java/frc/robot/CommandSwerveDrivetrain.java
rename to src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index 5211b54..33e576e 100644
--- a/src/main/java/frc/robot/CommandSwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -1,7 +1,8 @@
-package frc.robot;
+package frc.robot.subsystems.drive;
import java.util.function.Supplier;
+import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
@@ -15,32 +16,33 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.Subsystem;
-/**
- * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used
- * in command-based projects easily.
- */
-public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
- private SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds();
+public class DriveSubsystem extends SwerveDrivetrain implements Subsystem {
+ private Notifier m_simNotifier = null;
+ private double m_lastSimTime;
- public CommandSwerveDrivetrain(
- SwerveDrivetrainConstants driveTrainConstants,
- double OdometryUpdateFrequency,
- SwerveModuleConstants... modules) {
- super(driveTrainConstants, OdometryUpdateFrequency, modules);
- configurePathPlanner();
- }
+ private SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds();
- public CommandSwerveDrivetrain(
- SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
- super(driveTrainConstants, modules);
- configurePathPlanner();
+ @Override
+ public void simulationPeriodic() {
+ updateSimState(0.02, 12);
}
+ /**
+ * Configures path planner to have a holonomic path follower so it can move in any direction. This
+ * is meant for holonomic drivetrains AND swerve
+ */
private void configurePathPlanner() {
+ double driveBaseRadius = 0;
+ for (var moduleLocation : m_moduleLocations) {
+ driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm());
+ }
+
AutoBuilder.configureHolonomic(
() -> this.getState().Pose, // Supplier of current robot pose
this::seedFieldRelative, // Consumer for seeding pose against auto
@@ -51,13 +53,18 @@ private void configurePathPlanner() {
new HolonomicPathFollowerConfig(
new PIDConstants(10, 0, 0),
new PIDConstants(10, 0, 0),
- 1,
- 1,
- new ReplanningConfig(),
- 0.004),
- this); // Subsystem for requirements
+ DriveConstants.kSpeedAt12VoltsMps,
+ driveBaseRadius,
+ new ReplanningConfig()),
+ this);
}
+ /**
+ * Applies and runs a command on the swerve drivetrain
+ *
+ * @param requestSupplier The supplier of the swerve request to apply (the request)
+ * @return A command which runs the specified request
+ */
public Command applyRequest(SupplierEach subsystem represents a different purpose of the robot such as arms, LEDs, drivetrains, + * shooter, etc. + */ +public class IntakeSubsystem extends SubsystemBase { + + private VictorSP intakeMotor = new VictorSP(IntakeConstants.INTAKE_MOTOR_CHANNEL); + private PowerDistribution pdp; + + public IntakeSubsystem(PowerDistribution pdp) { + this.pdp = pdp; + } + + /** + * This method runs once every 20ms. + * + *
This is useful for updating subsystem-specific state that you don't want to offload to a + * Command. + * + *
Teams should try to be consistent within their own codebases about which responsibilities + * will be handled by Commands, and which will be handled here. + */ + @Override + public void periodic() { + SmartDashboard.putNumber( + "INTAKE CURRENT", this.pdp.getCurrent(IntakeConstants.INTAKE_MOTOR_CHANNEL)); + SmartDashboard.putNumber("INTAKE SPEED", this.intakeMotor.get()); + } + + /** + * Runs a command that stops the intake + * + * @return a command that stops the intake + */ + public Command stopIntakeCommand() { + return runOnce(() -> this.intakeMotor.set(0)).ignoringDisable(true); + } + + /** + * Runs a command to score a gamepiece. + * + * @param scoringState the state to run the intake in + * @param expectedPiece the type of gamepiece to expect when scoring + * @return a command that scores a gamepiece + */ + public Command intakeScoreCommand(ScoringState scoringState, IntakeGamepiece expectedGamepiece) { + + SmartDashboard.putString("INTAKE STATE", scoringState.getStateName()); + + return run(() -> { + Commands.runOnce( + () -> outtakeCommand(scoringState.getOuttakeSpeed(expectedGamepiece)), this) + .andThen(Commands.waitSeconds(0.5)) + .andThen(stopIntakeCommand()); + }) + .andThen(stopIntakeCommand()); + } + + /** + * Runs a command that intakes and holds a gamepiece + * + * @param gamepiece the type of gamepiece to expect + * @return a command that forces the intake to hold the specified gamepiece + */ + public Command intakeHoldCommand(IntakeGamepiece gamepiece) { + return run(() -> { + this.intakeMotor.set(IntakeConstants.INTAKING_SPEED); + + // this will keep the motor running as long as the current is low enough + // this results in something similar to an if statement + Commands.waitUntil( + () -> + this.pdp.getCurrent(IntakeConstants.INTAKE_MOTOR_CHANNEL) + < IntakeConstants.INTAKE_AMP_THRESHOLD); + + if (gamepiece.equals(IntakeGamepiece.CUBE)) { + // wait a short amount of time so the gamepiece gets pulled in + Commands.waitSeconds(IntakeConstants.INTAKE_CUBE_DELAY); + this.intakeMotor.set(IntakeConstants.HOLD_CUBE_SPEED); + } + if (gamepiece.equals(IntakeGamepiece.CONE)) { + // we have a cone, so run the motor at a higher speed + Commands.waitSeconds(IntakeConstants.INTAKE_CONE_DELAY); + this.intakeMotor.set(IntakeConstants.HOLD_CONE_SPEED); + } + }) + .finallyDo(() -> this.intakeMotor.set(0)) + .ignoringDisable(false); + } + + /** + * Runs a command that spits out its gamepiece + * + * @param speed speed to run the motor at, this is pre-inverted + * @return a command that forces the intake to spit out its gamepiece + */ + public Command outtakeCommand(double speed) { + return runOnce(() -> this.intakeMotor.set(-speed)); + } + + /** + * This method is similar to periodic, but only runs in simulation mode. + * + *
Useful for updating subsystem-specific state that needs to be maintained for simulations,
+ * such as for updating edu.wpi.first.wpilibj.simulation classes and setting simulated sensor
+ * readings.
+ */
+ @Override
+ public void simulationPeriodic() {
+ SmartDashboard.putNumber(
+ "INTAKE CURRENT", this.pdp.getCurrent(IntakeConstants.INTAKE_MOTOR_CHANNEL));
+ SmartDashboard.putNumber("INTAKE SPEED", this.intakeMotor.get());
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepiece.java b/src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepiece.java
new file mode 100644
index 0000000..4fccfd5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepiece.java
@@ -0,0 +1,6 @@
+package frc.robot.subsystems.intake.enums;
+
+public enum IntakeGamepiece {
+ CONE,
+ CUBE
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/ScoringState.java b/src/main/java/frc/robot/subsystems/intake/states/ScoringState.java
new file mode 100644
index 0000000..0326901
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/ScoringState.java
@@ -0,0 +1,68 @@
+package frc.robot.subsystems.intake.states;
+
+import frc.robot.subsystems.intake.enums.IntakeGamepiece;
+
+public abstract class ScoringState {
+ double outtakeSpeed;
+ double cubeOuttakeSpeed;
+ double coneOuttakeSpeed;
+
+ String stateName;
+
+ /**
+ * Creates a scoring state with the same outtake speed for cubes and cones
+ *
+ * @param outtakeSpeed The speed that should be used to outtake the gamepiece
+ * @param stateName The name of the scoring state that should be used for logging
+ */
+ protected ScoringState(double outtakeSpeed, String stateName) {
+ this.outtakeSpeed = outtakeSpeed;
+ this.stateName = stateName;
+ }
+
+ /**
+ * Creates a scoring state with different outtake speeds for cubes and cones
+ *
+ * @param cubeOuttakeSpeed The speed that should be used to outtake the cube gamepiece
+ * @param coneOuttakeSpeed The speed that should be used to outtake the cone gamepiece
+ * @param stateName The name of the scoring state that should be used for logging
+ */
+ protected ScoringState(double cubeOuttakeSpeed, double coneOuttakeSpeed, String stateName) {
+ this.cubeOuttakeSpeed = cubeOuttakeSpeed;
+ this.coneOuttakeSpeed = coneOuttakeSpeed;
+ this.stateName = stateName;
+ }
+
+ /**
+ * Gets the name of the scoring state
+ *
+ * @return The name of the scoring state
+ */
+ public String getStateName() {
+ return this.stateName;
+ }
+
+ /**
+ * Gets the outtake speed for the scoring state depending on the gamepiece that is expected to be
+ * scored
+ *
+ * @param expectedGamepiece The gamepiece that is expected to be scored
+ * @return The outtake speed for the scoring state
+ */
+ public double getOuttakeSpeed(IntakeGamepiece expectedGamepiece) {
+
+ if (expectedGamepiece == null) {
+ return this.outtakeSpeed;
+ }
+
+ // Based on the gamepiece that is expected to be scored, return the appropriate outtake speed
+ switch (expectedGamepiece) {
+ case CUBE:
+ return this.cubeOuttakeSpeed;
+ case CONE:
+ return this.coneOuttakeSpeed;
+ default:
+ return 0.0;
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighCone.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighCone.java
new file mode 100644
index 0000000..b8a2032
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighCone.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighCone extends ScoringState {
+
+ public HighCone() {
+ super(IntakeConstants.OuttakeSpeeds.HIGH_CONE, "High Cone");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighConeAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighConeAuto.java
new file mode 100644
index 0000000..95c87c5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighConeAuto.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighConeAuto extends ScoringState {
+
+ public HighConeAuto() {
+ super(IntakeConstants.OuttakeSpeeds.HIGH_CONE_AUTO, "High Cone Auto");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidCone.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidCone.java
new file mode 100644
index 0000000..4bc00f9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidCone.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidCone extends ScoringState {
+
+ public MidCone() {
+ super(IntakeConstants.OuttakeSpeeds.MID_CONE, "Mid Cone");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidConeTipped.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidConeTipped.java
new file mode 100644
index 0000000..6be2495
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidConeTipped.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidConeTipped extends ScoringState {
+
+ public MidConeTipped() {
+ super(IntakeConstants.OuttakeSpeeds.MID_CONE_TIPPED, "Mid Cone Tipped");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCube.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCube.java
new file mode 100644
index 0000000..b260fd8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCube.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighCube extends ScoringState {
+
+ public HighCube() {
+ super(IntakeConstants.OuttakeSpeeds.HIGH_CUBE, "High Cube");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCubeAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCubeAuto.java
new file mode 100644
index 0000000..65d5964
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCubeAuto.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighCubeAuto extends ScoringState {
+
+ public HighCubeAuto() {
+ super(IntakeConstants.OuttakeSpeeds.HIGH_CUBE, "High Cube Auto");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCube.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCube.java
new file mode 100644
index 0000000..dea9eac
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCube.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidCube extends ScoringState {
+
+ public MidCube() {
+ super(IntakeConstants.OuttakeSpeeds.MID_CUBE, "Mid Cube");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCubeAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCubeAuto.java
new file mode 100644
index 0000000..75cc262
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCubeAuto.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidCubeAuto extends ScoringState {
+
+ public MidCubeAuto() {
+ super(IntakeConstants.OuttakeSpeeds.MID_CUBE, "Mid Cube Auto");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/level/Low.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/level/Low.java
new file mode 100644
index 0000000..de19cf1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/level/Low.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.level;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class Low extends ScoringState {
+
+ public Low() {
+ super(IntakeConstants.OuttakeSpeeds.LOW_CUBE, IntakeConstants.OuttakeSpeeds.LOW_CONE, "Low");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/level/LowAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/level/LowAuto.java
new file mode 100644
index 0000000..cf1cc1f
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/states/scoring/level/LowAuto.java
@@ -0,0 +1,14 @@
+package frc.robot.subsystems.intake.states.scoring.level;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class LowAuto extends ScoringState {
+
+ public LowAuto() {
+ super(
+ IntakeConstants.OuttakeSpeeds.LOW_CUBE_AUTO,
+ IntakeConstants.OuttakeSpeeds.LOW_CONE_AUTO,
+ "Low Auto");
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDColour.java b/src/main/java/frc/robot/subsystems/led/LEDColour.java
new file mode 100644
index 0000000..c908bb2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDColour.java
@@ -0,0 +1,89 @@
+package frc.robot.subsystems.led;
+
+/**
+ * This is only used for setting solid colour states. Please see the LEDModes.java class for special
+ * LED modes
+ */
+public class LEDColour {
+ private double red, green, blue;
+
+ public LEDColour(int red, int green, int blue) {
+ this.red = red;
+ this.green = green;
+ this.blue = blue;
+ }
+
+ public LEDColour(double red, double green, double blue) {
+ this.red = red;
+ this.green = green;
+ this.blue = blue;
+ }
+
+ /**
+ * Copies the RGB values from an LEDState object to the current one
+ *
+ * @param ledState The LEDState to copy the RGB values from
+ * @return The current instane of LEDColour with the new RGB values
+ */
+ public LEDColour copy(LEDColour ledState) {
+ this.red = ledState.red;
+ this.green = ledState.green;
+ this.blue = ledState.blue;
+
+ return this;
+ }
+
+ /**
+ * Gets the red value of an LED state object
+ *
+ * @return The double value of the red value in the RGB sequence
+ */
+ public double getRed() {
+ return this.red;
+ }
+
+ /**
+ * Gets the red value of an LED state object as an integer
+ *
+ * @return The integer value of the red value in the RGB sequence
+ */
+ public int getRedInt() {
+ return (int) this.red;
+ }
+
+ /**
+ * Gets the green value of an LED state object
+ *
+ * @return The double value of the green value in the RGB sequence
+ */
+ public double getGreen() {
+ return this.green;
+ }
+
+ /**
+ * Gets the green value of an LED state object as an integer
+ *
+ * @return The integer value of the green value in the RGB sequence
+ */
+ public int getGreenInt() {
+ return (int) this.green;
+ }
+
+ /**
+ * Gets the blue value of an LED state object
+ *
+ * @return The integer value of the blue value in the RGB sequence
+ */
+ public double getBlue() {
+ return this.blue;
+ }
+
+ /**
+ * Gets the blue value of an LED state object as an integer
+ *
+ * @return The integer value of the blue value in the RGB sequence
+ */
+ public int getBlueInt() {
+ return (int) this.blue;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDConstants.java b/src/main/java/frc/robot/subsystems/led/LEDConstants.java
new file mode 100644
index 0000000..970777a
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDConstants.java
@@ -0,0 +1,7 @@
+package frc.robot.subsystems.led;
+
+public class LEDConstants {
+
+ public static final int ledsPerSegment =
+ 16; // The number LEDs (actual diodes) there are on each segment
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSegment.java b/src/main/java/frc/robot/subsystems/led/LEDSegment.java
new file mode 100644
index 0000000..9c6e9c7
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDSegment.java
@@ -0,0 +1,76 @@
+package frc.robot.subsystems.led;
+
+import frc.robot.subsystems.led.exceptions.InvalidLEDSegmentException;
+import frc.robot.subsystems.led.modes.Breathing;
+import frc.robot.subsystems.led.modes.LEDMode;
+import frc.robot.subsystems.led.modes.Solid;
+
+/**
+ * We are going to attach a name to each index in the LED modes array here. Remember that all
+ * indexes start at 0 and NOT 1
+ */
+public enum LEDSegment {
+
+ // Add all aliases for segments below
+ FrontLeft(0, new Solid(new LEDColour(255, 0, 0))), // Set the LEDs to solid red
+ BackLeft(1, new Breathing(new LEDColour(0, 255, 0))), // Create a green breathing effect
+ BackRight(2, new Breathing(new LEDColour(0, 255, 0))), // Create a green breathing effect
+ FrontRight(3, new Solid(new LEDColour(255, 0, 0))); // Set the LEDs to solid red
+
+ public final int segmentNumber; // The segment number of the LED strip (starts at 1 and goes up)
+ public LEDMode ledMode; // The mode of the LED strip
+
+ private LEDSegment(int segmentNumber, LEDMode defaultLedMode) {
+ this.segmentNumber = segmentNumber;
+ this.ledMode = defaultLedMode;
+ }
+
+ /**
+ * Checks if the LED segment is a valid segment based off of the number of segments in
+ * LEDConstants.java
+ *
+ * @return True if it is within the index bounds, false if it isn't
+ */
+ private boolean isValid() {
+ // Return a boolean based on if the segment number is greater than number of
+ // segments
+ return (this.getSegmentIdentifier() < LEDSubsystem.ledSegments.size());
+ }
+
+ /**
+ * Sets the mode of the current segment
+ *
+ * @param ledMode The mode to set the LEDs to
+ */
+ public void setLedMode(LEDMode ledMode) {
+ // Guard clause to check if the segment is within the bounds of the number of
+ // available segments
+ if (!this.isValid()) {
+ throw new InvalidLEDSegmentException(
+ String.format(
+ "Invalid LED segment: %d. Number of segments: %d",
+ this.getSegmentIdentifier(),
+ LEDSubsystem.ledSegments.size())); // Throw an exception with the segment information
+ }
+
+ this.ledMode = ledMode;
+ }
+
+ /**
+ * Retrieves the true segment number of an LED segment
+ *
+ * @return The segment number of the LED segment
+ */
+ public int getSegmentIdentifier() {
+ return this.segmentNumber;
+ }
+
+ /**
+ * Gets the LED mode for this segment
+ *
+ * @return The LED mode of the current segment as an LEDMode object
+ */
+ public LEDMode getLedMode() {
+ return this.ledMode;
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
new file mode 100644
index 0000000..e0242d8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
@@ -0,0 +1,60 @@
+package frc.robot.subsystems.led;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.led.modes.LEDMode;
+
+public class LEDSubsystem extends SubsystemBase {
+
+ public static List