FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Documentation

This commit is contained in:
Tyler Wilcox 2026-01-31 17:31:45 -05:00 committed by Tyler Wilcox
parent 61fcb90730
commit 9ce8f2fb34
6 changed files with 233 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

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

4
gradle.properties
View File

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

2
lib/.project
View File

@ -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>

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

@ -8,8 +8,8 @@ import edu.wpi.first.math.geometry.Translation2d;
public class SwerveBuilder {
ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
CANBus canBusObj;
private ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
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;
return this;
}
/**
* Set the steer encoders CAN Bus ID
* @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,

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

@ -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;
}
public Angle getCurrentYaw(){
return gyro.getRoll().getValue();
/**
* Get the current yaw angle from the Gyro
* @return The current yaw as an Angle
*/
private Angle getCurrentYaw(){
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());
}
public void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
/**
* Set robot speed
* @param chassisSpeeds
* @param 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();
}

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

@ -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.optimizeBusUtilization();
steerEncoder.getAbsolutePosition().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
steerEncoder.optimizeBusUtilization();
steerEncoder.getConfigurator().apply(generateCanCoderConfiguration(steerOffset));
driveMotor = new TalonFX(driveMotorId, canBusObj);
// driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
// driveMotor.optimizeBusUtilization();
driveMotor.getVelocity().setUpdateFrequency(CAN_UPDATE_FREQUENCY);
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());
}