Documentation

README.md

@@ -1,5 +1,5 @@
-# Potential Octo Eureka(POE)
+# RoboLib
-The official library for Team 578 R3 - Red Raider Robotics
+The official library for Team 578 R3 - Fairport Robotics
 
 # Including in FRC Project
 To include this library in a Java FRC Robot project. 

gradle.properties

@@ -1,5 +1,5 @@
-wpiVersion = 2025.3.1
+wpiVersion = 2026.2.1
 
-ctreVersion = 25.1.0
+ctreVersion = 26.1.0
 
 advantageKitVersion = 4.0.0

lib/.project

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <projectDescription>
-	<name>lib</name>
+	<name>RoboLib-lib</name>
 	<comment>Project lib created by Buildship.</comment>
 	<projects>
 	</projects>

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveBuilder.java

@@ -8,8 +8,8 @@ import edu.wpi.first.math.geometry.Translation2d;
 
 public class SwerveBuilder {
 
-    ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
+    private ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
-    CANBus canBusObj;
+    private CANBus canBusObj;
 
     private double maxAngularVelRadiansSecond = 1.0;
     private double maxLinearVelMetersSecond = Math.PI;
@@ -20,16 +20,33 @@ public class SwerveBuilder {
 
     }
 
+    /**
+     * Set the CAN ID of the Pigeon IMU
+     * @param pigeonId CAN Bus ID
+     * @return itself
+     */
     public SwerveBuilder withPigeonId(int pigeonId) {
         this.pigeonId = pigeonId;
         return this;
     }
 
+    /**
+     * Set the CAN Bus name
+     * @param canBusname Name of the CAN Bus
+     * @return itself
+     */
     public SwerveBuilder withCanbusName(String canBusname) {
         this.canBusObj = new CANBus(canBusname);
         return this;
     }
 
+    /**
+     * Add a SwerveModule
+     * 
+     * Use the inner class SwerveModuleBuilder to construct a SwerveModule
+     * @param module 
+     * @return itself
+     */
     public SwerveBuilder withSwerveModule(SwerveModule module) {
         this.modules.add(module);
         return this;
@@ -39,7 +56,7 @@ public class SwerveBuilder {
      * Set the max linear velocity in meters per second
      * 
      * @param maxVel Max linear velocity in meters per second
-     * @return The swerve builder
+     * @return itself
      */
     public SwerveBuilder withMaxLinearVelocity(double maxVel) {
         this.maxLinearVelMetersSecond = maxVel;
@@ -50,13 +67,17 @@ public class SwerveBuilder {
      * Set the max angular velocity in radians per second
      * 
      * @param maxVel Max angular velocity in radians per second
-     * @return The swerve builder
+     * @return itself
      */
     public SwerveBuilder withMaxAngularVelocity(double maxVel) {
         this.maxAngularVelRadiansSecond = maxVel;
         return this;
     }
 
+    /**
+     * Build the configured instance of a SwerveDriveSystem
+     * @return instance of SwerveDriveSystem
+     */
     public SwerveDriveSystem build() {
         return new SwerveDriveSystem(pigeonId, canBusObj, maxLinearVelMetersSecond, maxAngularVelRadiansSecond,
                 modules.toArray(size -> new SwerveModule[size]));
@@ -88,101 +109,229 @@ public class SwerveBuilder {
 
         }
 
+        /**
+         * Set the drive motor CAN Bus ID
+         * @param driveMotorId ID of the drive motor
+         * @return itself
+         */
         public SwerveModuleBuilder withDriveMotorId(int driveMotorId) {
             this.driveMotorId = driveMotorId;
             return this;
         }
 
+        /**
+         * Set the drive motors closed loop proportional gain value.
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * 
+         * @param driveKP Proportional gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withDriveKP(double driveKP) {
             this.driveKP = driveKP;
             return this;
         }
 
+
+        /**
+         * Set the drive motors closed loop integral gain value.
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * 
+         * @param driveKP Integral gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withDriveKI(double driveKI) {
             this.driveKI = driveKI;
             return this;
         }
 
+
+        /**
+         * Set the drive motors closed loop derrivative gain value.
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * 
+         * @param driveKP Derrivative gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withDriveKD(double driveKD) {
             this.driveKD = driveKD;
             return this;
         }
 
+        /**
+         * Set the drive motors closed loop feed forward velocity gain.
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * @param driveKV velocity gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withDriveKV(double driveKV) {
             this.driveKV = driveKV;
             return this;
         }
 
+        /**
+         * Set the drive motors direction to be inverted.
+         * <br><br>
+         * When inverted positive values will be Counter Clockwise
+         * 
+         * @return itself
+         */
         public SwerveModuleBuilder withDriveInverted(){
             this.driveInverted = true;
             return this;
         }
 
+        /**
+         * Set the steer motors CAN Bus ID
+         * @param steerMotorId ID of steer motor
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerMotorId(int steerMotorId) {
             this.steerMotorId = steerMotorId;
             return this;
         }
 
+        /**
+         * Set the steer motors closed loop proportional gain value
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * @param steerKP Proportional gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerKP(double steerKP) {
             this.steerKP = steerKP;
             return this;
         }
 
+        /**
+         * Set the steer motors closed loop integral gain value
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * @param steerKP Integral gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerKI(double steerKI) {
             this.steerKI = steerKI;
             return this;
         }
 
+        /**
+         * Set the steer motors closed loop derrivative gain value
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * @param steerKP Derrivative gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerKD(double steerKD) {
             this.steerKD = steerKD;
             return this;
         }
 
+        /**
+         * Set the steer motors closed loop feed forward static gain.
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * @param steerKS Static gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerKS(double steerKS) {
             this.steerKS = steerKS;
             return this;
         }
 
+        /**
+         * Set the steer motors closed loop feed forward velocity gain.
+         * <br><br>
+         * Refer here for more info: https://v6.docs.ctr-electronics.com/en/latest/docs/api-reference/device-specific/talonfx/basic-pid-control.html
+         * @param steerKV velocity gain in Volts
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerKV(double steerKV) {
             this.steerKV = steerKV;
             return this;
         }
 
-        public SwerveModuleBuilder withSteerKA(double steerKA) {
+        /**
-            this.steerKA = steerKA;
+         * Set the steer encoders CAN Bus ID
-            return this;
+         * @param steerEncoderId ID of steer encoder
-        }
+         * @return itself
-
+         */
         public SwerveModuleBuilder withSteerEncoderId(int steerEncoderId) {
             this.steerEncoderId = steerEncoderId;
             return this;
         }
 
+        /**
+         * Set the offset of the steer encoder.
+         * <br><br>
+         * This value can be found from Pheonix tuner.
+         * <br><br>
+         * Ensure the configured offset is 0.0 and then set this to
+         * be the Absolute Position when the wheel is properly aligned
+         * @param offsetRotations offset in the range of [-0.5, 0.5)
+         * @return itself
+         */
         public SwerveModuleBuilder withSteerOffset(double offsetRotations) {
             this.steerOffset = offsetRotations;
             return this;
         }
 
+        /**
+         * Set the swerve modules location relative to the center of the robot.
+         * <br><br>
+         * Refer to https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html
+         * for how the cordinate system works.
+         * <br><br>
+         * The location is in meters from the center of the robot.
+         * 
+         * @param moduleLocation location of the swerve module
+         * @return
+         */
         public SwerveModuleBuilder withModuleLocation(Translation2d moduleLocation) {
             this.moduleLocation = moduleLocation;
             return this;
         }
 
+        /**
+         * Set the gear ratio of the drive motor to the drive wheel.
+         * <br><br>
+         * This ratio is the number of rotations the motor has to make for one(1) rotation
+         * of the drive wheel.
+         * @param gearRatio the gear ratio
+         * @return itself
+         */
         public SwerveModuleBuilder withGearRatio(double gearRatio) {
             this.gearRatio = gearRatio;
             return this;
         }
 
+        /**
+         * Set the diameter of the drive wheel in meters.
+         * @param wheelDiameterInMeters diameter of the drive wheel
+         * @return itself
+         */
         public SwerveModuleBuilder withWheelDiameter(double wheelDiameterInMeters) {
             this.wheelDiameterInMeters = wheelDiameterInMeters;
             return this;
         }
 
+        /**
+         * Set the friendly name of this module. This name is used in logging
+         * @param moduleName name of the module
+         * @return itself
+         */
         public SwerveModuleBuilder withModuleName(String moduleName) {
             this.moduleName = moduleName;
             return this;
         }
 
+        /**
+         * Build the configured instance of a SwerveModule
+         * @return a SwerveModule object
+         */
         public SwerveModule build() {
             return new SwerveModule(driveMotorId, driveKP, driveKI, driveKD, driveKV, driveInverted, steerMotorId, steerKP, steerKI,
                     steerKD, steerKS, steerKV, steerKA,

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java

@@ -17,7 +17,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class SwerveDriveSystem implements Subsystem{
 
@@ -29,13 +28,35 @@ public class SwerveDriveSystem implements Subsystem{
 
     private SwerveModule[] modules;
 
+    /**
+     * The max linear speed of the robot in meters per second
+     */
     private double MAX_LINEAR_SPEED = 3.9;
+    /**
+     * The max angular rotation speed in radians per second
+     */
     private double MAX_ANGULAR_SPEED = Math.PI * 2;
 
     private Pigeon2 gyro;
 
+    /**
+     * Only used during simulation.
+     * This variable tracks when the last simulation loop started
+     * to calculate how long it's been since the last simulation loop
+     */
     private double mLastSimTime = 0;
 
+    /**
+     * An implementation of Swerve drive.
+     * 
+     * You should not directly instantiate this class.
+     * You should use SwerveBuilder to create an instance.
+     * @param pigeonId
+     * @param canBus
+     * @param maxLinearVelMetersSecond
+     * @param maxAngularVelRadiansSecond
+     * @param modules
+     */
     public SwerveDriveSystem(int pigeonId, CANBus canBus, double maxLinearVelMetersSecond, double maxAngularVelRadiansSecond, SwerveModule... modules){
 
         MAX_LINEAR_SPEED = maxLinearVelMetersSecond;
@@ -55,7 +76,7 @@ public class SwerveDriveSystem implements Subsystem{
 
     @Override
     public void periodic() {
-        SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds);
+        SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
 
         for(int i=0; i<modules.length;i++){
             moduleStates[i].optimize(modules[i].getSteerRotations());
@@ -81,39 +102,64 @@ public class SwerveDriveSystem implements Subsystem{
         mLastSimTime = Utils.getCurrentTimeSeconds();
     }
 
+    /**
+     * Get the location of the swerve modules
+     * The order of the array is the same order from when they were defined
+     * @return an array of Translation2d
+     */
     public Translation2d[] getModuleLocations(){
         return Arrays.stream(modules).map((SwerveModule m) -> m.getModuleLocation()).toArray(Translation2d[]::new);
     }
 
+    /**
+     * Get the requested state of the swerve modules.
+     * The order of the array is the same order from when they were defined
+     * 
+     * @return an array of SwerveModuleState
+     */
     public SwerveModuleState[] getRequestedModuleStates(){
         return Arrays.stream(modules).map((SwerveModule m) -> m.getRequestedModuleState()).toArray(SwerveModuleState[]::new);
     }
 
+    /**
+     * Get the actual state of the swerve modules.
+     * The order of the array is the same order from when they were defined
+     * @return an array of SwerveModuleState
+     */
     public SwerveModuleState[] getActualModuleStates(){
         return Arrays.stream(modules).map((SwerveModule m) -> m.getActualModuleState()).toArray(SwerveModuleState[]::new);
     }
 
+    /**
+     * Get an array of the SwerveModules configured for this system
+     * @return Array of SwerveModules
+     */
     public SwerveModule[] getModules(){
         return Arrays.stream(modules).toArray(SwerveModule[]::new);
     }
 
-    public int getNumberOfModules(){
+    /**
-        return modules.length;
+     * Get the current yaw angle from the Gyro
-    }
+     * @return The current yaw as an Angle
-
+     */
-    public Angle getCurrentYaw(){
+    private Angle getCurrentYaw(){
-        return gyro.getRoll().getValue();
+        return gyro.getYaw().getValue();
     }
 
+    /**
+     * Get the instance of the gyro
+     * @return
+     */
     public Pigeon2 getGyro(){
         return gyro;
     }
 
-    public void setChassisSpeed(ChassisSpeeds chassisSpeeds){
+    /**
-        this.setChassisSpeed(chassisSpeeds, new Translation2d());
+     * Set robot speed
-    }
+     * @param chassisSpeeds
-
+     * @param centerOfRotation
-    public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
+     */
+    private void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
         this.chassisSpeeds = chassisSpeeds;
         this.centerOfRotation = centerOfRotation;
     }
@@ -130,7 +176,7 @@ public class SwerveDriveSystem implements Subsystem{
             MAX_LINEAR_SPEED * y,
             MAX_ANGULAR_SPEED * rot,
             Rotation2d.fromDegrees(getCurrentYaw().magnitude())
-        ));
+        ), new Translation2d());
     }
 
     /**
@@ -145,9 +191,13 @@ public class SwerveDriveSystem implements Subsystem{
             MAX_LINEAR_SPEED * y,
             MAX_ANGULAR_SPEED * rot,
             Rotation2d.fromDegrees(getCurrentYaw().magnitude())
-        ));
+        ), new Translation2d());
     }
 
+    /**
+     * Get the current field location of the robot
+     * @return The current pose as a Pose3d
+     */
     public Pose3d getRobotPose(){
         return poseEstimator.getEstimatedPosition();
     }

lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java

@@ -1,7 +1,5 @@
 package org.fairportrobotics.frc.robolib.drivesystems.swerve;
 
-import java.lang.reflect.Modifier;
-
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
@@ -12,14 +10,12 @@ import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 
@@ -27,7 +23,6 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class SwerveModule {
@@ -45,7 +40,6 @@ public class SwerveModule {
     private VelocityVoltage driveRequest = new VelocityVoltage(0);
 
     private TalonFX steerMotor;
-    private MotionMagicVoltage steerMMRequest = new MotionMagicVoltage(0);
     private PositionVoltage steerPosRequest = new PositionVoltage(0);
 
     private CANcoder steerEncoder;
@@ -78,17 +72,17 @@ public class SwerveModule {
     ){
 
         steerEncoder = new CANcoder(steerEncoderId, canBusObj);
-        // steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
+        steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
-        // steerEncoder.optimizeBusUtilization();
+        steerEncoder.optimizeBusUtilization();
         steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
 
         driveMotor = new TalonFX(driveMotorId, canBusObj);
-        // driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
+        driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
-        // driveMotor.optimizeBusUtilization();
+        driveMotor.optimizeBusUtilization();
         driveMotor.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted));
 
         steerMotor = new TalonFX(steerMotorId, canBusObj);
-        // steerMotor.optimizeBusUtilization();
+        steerMotor.optimizeBusUtilization();
         steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA));
         // steerMotor.setNeutralMode(NeutralModeValue.Brake);
 
@@ -102,12 +96,9 @@ public class SwerveModule {
 
     public void setSteerRotations(Rotation2d rotations){
         steerMotor.setControl(steerPosRequest.withPosition(rotations.getRotations()));
-        // steerMotor.setControl(steerMMRequest.withPosition(rotations.getRotations()));
     }
 
     public Rotation2d getSteerRotations(){
-        // Maybe go back to non-Absolute Pos
-        // Or try reading from the steer motot for the pos
         return new Rotation2d(steerEncoder.getAbsolutePosition().getValue());
     }