Merge pull request #15 from FairportRobotics/feature/swervedrive
TalonFX Swerve Drive Support
This commit is contained in:
Tyler Wilcox
GitHub
commit
fae46eb9b8
2
.project
2
.project
|
|
@ -1,6 +1,6 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<projectDescription>
|
||||
<name>RoboLib</name>
|
||||
<name>potential-octo-eureka-RoboLib</name>
|
||||
<comment>Project RoboLib created by Buildship.</comment>
|
||||
<projects>
|
||||
</projects>
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,5 @@
|
|||
wpiVersion = 2026.2.1
|
||||
|
||||
ctreVersion = 26.1.0
|
||||
|
||||
advantageKitVersion = 4.0.0
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<classpath>
|
||||
<classpathentry kind="src" output="bin/main" path="src/main/java">
|
||||
<attributes>
|
||||
<attribute name="gradle_scope" value="main"/>
|
||||
<attribute name="gradle_used_by_scope" value="main,test"/>
|
||||
</attributes>
|
||||
</classpathentry>
|
||||
<classpathentry kind="src" output="bin/test" path="src/test/java">
|
||||
<attributes>
|
||||
<attribute name="gradle_scope" value="test"/>
|
||||
<attribute name="gradle_used_by_scope" value="test"/>
|
||||
<attribute name="test" value="true"/>
|
||||
</attributes>
|
||||
</classpathentry>
|
||||
<classpathentry kind="src" path="build/generated/sources/annotationProcessor/java/main"/>
|
||||
<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-17/"/>
|
||||
<classpathentry kind="con" path="org.eclipse.buildship.core.gradleclasspathcontainer"/>
|
||||
<classpathentry kind="output" path="bin/default"/>
|
||||
</classpath>
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<projectDescription>
|
||||
<name>RoboLib-lib</name>
|
||||
<comment>Project lib created by Buildship.</comment>
|
||||
<projects>
|
||||
</projects>
|
||||
<buildSpec>
|
||||
<buildCommand>
|
||||
<name>org.eclipse.jdt.core.javabuilder</name>
|
||||
<arguments>
|
||||
</arguments>
|
||||
</buildCommand>
|
||||
<buildCommand>
|
||||
<name>org.eclipse.buildship.core.gradleprojectbuilder</name>
|
||||
<arguments>
|
||||
</arguments>
|
||||
</buildCommand>
|
||||
</buildSpec>
|
||||
<natures>
|
||||
<nature>org.eclipse.jdt.core.javanature</nature>
|
||||
<nature>org.eclipse.buildship.core.gradleprojectnature</nature>
|
||||
</natures>
|
||||
<filteredResources>
|
||||
<filter>
|
||||
<id>1743377880685</id>
|
||||
<name></name>
|
||||
<type>30</type>
|
||||
<matcher>
|
||||
<id>org.eclipse.core.resources.regexFilterMatcher</id>
|
||||
<arguments>node_modules|\.git|__CREATED_BY_JAVA_LANGUAGE_SERVER__</arguments>
|
||||
</matcher>
|
||||
</filter>
|
||||
</filteredResources>
|
||||
</projectDescription>
|
||||
|
|
@ -25,14 +25,14 @@ repositories {
|
|||
mavenLocal()
|
||||
gradlePluginPortal()
|
||||
|
||||
maven {url "https://maven.photonvision.org/repository/snapshots"}
|
||||
maven {url "https://maven.photonvision.org/repository/internal"}
|
||||
maven {url "https://maven.ctr-electronics.com/release/"}
|
||||
maven {url "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"}
|
||||
}
|
||||
|
||||
dependencies {
|
||||
// Use JUnit Jupiter for testing.
|
||||
testImplementation 'org.junit.jupiter:junit-jupiter:5.8.1'
|
||||
testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
|
||||
testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
|
||||
|
||||
// WPI Lib
|
||||
implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion"
|
||||
|
|
@ -48,6 +48,9 @@ dependencies {
|
|||
// CTRE Phoenix 6
|
||||
implementation "com.ctre.phoenix6:wpiapi-java:${ctreVersion}"
|
||||
|
||||
// AdvantageKit
|
||||
implementation "org.littletonrobotics.akit:akit-java:${advantageKitVersion}"
|
||||
|
||||
}
|
||||
|
||||
tasks.named('test') {
|
||||
|
|
|
|||
|
|
@ -1,11 +0,0 @@
|
|||
package org.fairportrobotics.frc.robolib.DriveSystems.SwerveDrive;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
public class SwerveDriveSubsystem extends SubsystemBase{
|
||||
|
||||
public SwerveDriveSubsystem(){
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,344 @@
|
|||
package org.fairportrobotics.frc.robolib.drivesystems.swerve;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
|
||||
public class SwerveBuilder {
|
||||
|
||||
private ArrayList<SwerveModule> modules = new ArrayList<SwerveModule>();
|
||||
private CANBus canBusObj;
|
||||
|
||||
private double maxAngularVelRadiansSecond = 1.0;
|
||||
private double maxLinearVelMetersSecond = Math.PI;
|
||||
|
||||
private int pigeonId;
|
||||
|
||||
public 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the max linear velocity in meters per second
|
||||
*
|
||||
* @param maxVel Max linear velocity in meters per second
|
||||
* @return itself
|
||||
*/
|
||||
public SwerveBuilder withMaxLinearVelocity(double maxVel) {
|
||||
this.maxLinearVelMetersSecond = maxVel;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the max angular velocity in radians per second
|
||||
*
|
||||
* @param maxVel Max angular velocity in radians per second
|
||||
* @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]));
|
||||
}
|
||||
|
||||
public class SwerveModuleBuilder {
|
||||
|
||||
private int driveMotorId;
|
||||
private double driveKP;
|
||||
private double driveKI;
|
||||
private double driveKD;
|
||||
private double driveKV;
|
||||
private boolean driveInverted = false;
|
||||
private int steerMotorId;
|
||||
private double steerKP;
|
||||
private double steerKI;
|
||||
private double steerKD;
|
||||
private double steerKS;
|
||||
private double steerKV;
|
||||
private double steerKA;
|
||||
private int steerEncoderId;
|
||||
private double steerOffset;
|
||||
private Translation2d moduleLocation;
|
||||
private double gearRatio;
|
||||
private double wheelDiameterInMeters;
|
||||
private String moduleName;
|
||||
|
||||
public SwerveModuleBuilder() {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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,
|
||||
steerEncoderId, steerOffset, canBusObj, moduleLocation, gearRatio, wheelDiameterInMeters,
|
||||
moduleName);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,209 @@
|
|||
package org.fairportrobotics.frc.robolib.drivesystems.swerve;
|
||||
|
||||
import java.util.Arrays;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.Utils;
|
||||
import com.ctre.phoenix6.hardware.Pigeon2;
|
||||
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
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;
|
||||
|
||||
public class SwerveDriveSystem implements Subsystem{
|
||||
|
||||
private SwerveDriveKinematics driveKiniematics;
|
||||
private ChassisSpeeds chassisSpeeds;
|
||||
private Translation2d centerOfRotation;
|
||||
|
||||
private SwerveDrivePoseEstimator3d poseEstimator;
|
||||
|
||||
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;
|
||||
MAX_ANGULAR_SPEED = maxAngularVelRadiansSecond;
|
||||
|
||||
this.modules = modules;
|
||||
|
||||
driveKiniematics = new SwerveDriveKinematics(
|
||||
Arrays.stream(modules).map((m) -> m.getModuleLocation()).toArray(size -> new Translation2d[size])
|
||||
);
|
||||
|
||||
gyro = new Pigeon2(pigeonId, canBus);
|
||||
|
||||
poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero);
|
||||
chassisSpeeds = new ChassisSpeeds();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
SwerveModuleState[] moduleStates = driveKiniematics.toSwerveModuleStates(chassisSpeeds, centerOfRotation);
|
||||
|
||||
for(int i=0; i<modules.length;i++){
|
||||
moduleStates[i].optimize(modules[i].getSteerRotations());
|
||||
}
|
||||
|
||||
poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
|
||||
|
||||
for(int i=0;i<modules.length;i++){
|
||||
modules[i].setRequestedModuleState(moduleStates[i]);
|
||||
modules[i].periodic();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void simulationPeriodic() {
|
||||
double deltaTimeSecond = (mLastSimTime - Utils.getCurrentTimeSeconds());
|
||||
|
||||
if(mLastSimTime != 0){
|
||||
Pose3d oldPose = getRobotPose();
|
||||
poseEstimator.resetPose(new Pose3d(oldPose.getX() + (chassisSpeeds.vxMetersPerSecond * (deltaTimeSecond)), oldPose.getY() + chassisSpeeds.vyMetersPerSecond * (deltaTimeSecond), 0, oldPose.getRotation().plus(new Rotation3d(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * (deltaTimeSecond))))));
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set robot speed
|
||||
* @param chassisSpeeds
|
||||
* @param centerOfRotation
|
||||
*/
|
||||
private void setChassisSpeed(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation){
|
||||
this.chassisSpeeds = chassisSpeeds;
|
||||
this.centerOfRotation = centerOfRotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set robot speed relative to field
|
||||
* @param x Forward robot speed in meters per second
|
||||
* @param y Left robot speed in meters per second
|
||||
* @param rot CCW robot speed in radians per second
|
||||
*/
|
||||
public void setChassisSpeedsFromJoystickFieldRelative(double x, double y, double rot){
|
||||
this.setChassisSpeed(ChassisSpeeds.fromFieldRelativeSpeeds(
|
||||
MAX_LINEAR_SPEED * x,
|
||||
MAX_LINEAR_SPEED * y,
|
||||
MAX_ANGULAR_SPEED * rot,
|
||||
Rotation2d.fromDegrees(getCurrentYaw().magnitude())
|
||||
), new Translation2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Set robot speed relative to robot
|
||||
* @param x Forward robot speed in meters per second
|
||||
* @param y Left robot speed in meters per second
|
||||
* @param rot CCW robot speed in radians per second
|
||||
*/
|
||||
public void setChassisSpeedsFromJoystickRobotRelative(double x, double y, double rot){
|
||||
this.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(
|
||||
MAX_LINEAR_SPEED * x,
|
||||
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();
|
||||
}
|
||||
|
||||
private SwerveModulePosition[] getModulePositions(){
|
||||
return Arrays.stream(modules).map((SwerveModule m) -> m.getModulePosition()).toArray(SwerveModulePosition[]::new);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,240 @@
|
|||
package org.fairportrobotics.frc.robolib.drivesystems.swerve;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.configs.CANcoderConfiguration;
|
||||
import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.FeedbackConfigs;
|
||||
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
|
||||
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.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.SensorDirectionValue;
|
||||
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
|
||||
|
||||
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.wpilibj.smartdashboard.SmartDashboard;
|
||||
|
||||
public class SwerveModule {
|
||||
|
||||
private final int CAN_UPDATE_FREQUENCY = 50;
|
||||
private final double STEER_MAX_CURRENT = 60;
|
||||
private final double DRIVE_MAX_CURRENT = 80;
|
||||
|
||||
private final double gearRatio;
|
||||
private final double wheelDiameterInMeters;
|
||||
|
||||
private SwerveModuleState requestedModuleState;
|
||||
|
||||
private TalonFX driveMotor;
|
||||
private VelocityVoltage driveRequest = new VelocityVoltage(0);
|
||||
|
||||
private TalonFX steerMotor;
|
||||
private PositionVoltage steerPosRequest = new PositionVoltage(0);
|
||||
|
||||
private CANcoder steerEncoder;
|
||||
|
||||
private String moduleName;
|
||||
|
||||
private Translation2d moduleLocation;
|
||||
|
||||
public SwerveModule(
|
||||
int driveMotorId,
|
||||
double driveKP,
|
||||
double driveKI,
|
||||
double driveKD,
|
||||
double driveKV,
|
||||
boolean driveInverted,
|
||||
int steerMotorId,
|
||||
double steerKP,
|
||||
double steerKI,
|
||||
double steerKD,
|
||||
double steerKS,
|
||||
double steerKV,
|
||||
double steerKA,
|
||||
int steerEncoderId,
|
||||
double steerOffset,
|
||||
CANBus canBusObj,
|
||||
Translation2d moduleLocation,
|
||||
double gearRatio,
|
||||
double wheelDiameterInMeters,
|
||||
String moduleName
|
||||
){
|
||||
|
||||
steerEncoder = new CANcoder(steerEncoderId, canBusObj);
|
||||
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.getConfigurator().apply(generateDriveTalonConfiguration(driveKP, driveKI, driveKD, driveKV, driveInverted));
|
||||
|
||||
steerMotor = new TalonFX(steerMotorId, canBusObj);
|
||||
steerMotor.optimizeBusUtilization();
|
||||
steerMotor.getConfigurator().apply(generateSteerTalonConfiguration(steerKP, steerKI, steerKD, steerKS, steerKV, steerKA));
|
||||
// steerMotor.setNeutralMode(NeutralModeValue.Brake);
|
||||
|
||||
this.moduleLocation = moduleLocation;
|
||||
|
||||
this.gearRatio = gearRatio;
|
||||
this.wheelDiameterInMeters = wheelDiameterInMeters;
|
||||
|
||||
this.moduleName = moduleName;
|
||||
}
|
||||
|
||||
public void setSteerRotations(Rotation2d rotations){
|
||||
steerMotor.setControl(steerPosRequest.withPosition(rotations.getRotations()));
|
||||
}
|
||||
|
||||
public Rotation2d getSteerRotations(){
|
||||
return new Rotation2d(steerEncoder.getAbsolutePosition().getValue());
|
||||
}
|
||||
|
||||
public void setDriveSpeed(double speed){
|
||||
// Convert wheel speed to rotor speed
|
||||
// (speed / Circumfrence of wheel) * gear ratio
|
||||
double rotorVelocity = (speed / (Math.PI * wheelDiameterInMeters)) * gearRatio;
|
||||
driveMotor.setControl(driveRequest.withSlot(0).withVelocity(rotorVelocity));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get drive motor speed in rotations per second
|
||||
* @return
|
||||
*/
|
||||
public double getDriveMotorSpeed(){
|
||||
return driveMotor.getVelocity().refresh().getValueAsDouble();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get drive wheel speed in meters per second
|
||||
* @return
|
||||
*/
|
||||
public double getDriveWheelSpeed(){
|
||||
return (driveMotor.getVelocity().refresh().getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
|
||||
}
|
||||
|
||||
public Translation2d getModuleLocation(){
|
||||
return moduleLocation;
|
||||
}
|
||||
|
||||
public void setRequestedModuleState(SwerveModuleState state){
|
||||
this.setSteerRotations(state.angle);
|
||||
this.setDriveSpeed(state.speedMetersPerSecond);
|
||||
requestedModuleState = state;
|
||||
}
|
||||
|
||||
public TalonFX getSteerMotor(){
|
||||
return steerMotor;
|
||||
}
|
||||
|
||||
public TalonFX getDriveMotor(){
|
||||
return driveMotor;
|
||||
}
|
||||
|
||||
public CANcoder getSteerEncoder(){
|
||||
return steerEncoder;
|
||||
}
|
||||
|
||||
public SwerveModuleState getRequestedModuleState(){
|
||||
return requestedModuleState;
|
||||
}
|
||||
|
||||
public SwerveModuleState getActualModuleState() {
|
||||
return new SwerveModuleState(getDriveWheelSpeed(), getSteerRotations());
|
||||
}
|
||||
|
||||
public SwerveModulePosition getModulePosition(){
|
||||
return new SwerveModulePosition(this.getModuleDistance(), getSteerRotations());
|
||||
}
|
||||
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber(moduleName + " steerPow", steerMotor.getClosedLoopOutput().getValueAsDouble());
|
||||
SmartDashboard.putNumber(moduleName + " steer error", steerMotor.getClosedLoopError().getValueAsDouble());
|
||||
SmartDashboard.putNumber(moduleName + " steer setpoint", steerMotor.getClosedLoopReference().getValueAsDouble());
|
||||
|
||||
SmartDashboard.putNumber(moduleName + " steerPos", getSteerRotations().getRotations());
|
||||
}
|
||||
|
||||
public String getModuleName(){
|
||||
return moduleName;
|
||||
}
|
||||
|
||||
private double getModuleDistance(){
|
||||
double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) * (Math.PI * wheelDiameterInMeters);
|
||||
return distance;
|
||||
}
|
||||
|
||||
private TalonFXConfiguration generateDriveTalonConfiguration(double kP, double kI, double kD, double kV, boolean driveInverted){
|
||||
return new TalonFXConfiguration().withSlot0(
|
||||
new Slot0Configs()
|
||||
.withKP(kP)
|
||||
.withKI(kI)
|
||||
.withKD(kD)
|
||||
.withKV(kV)
|
||||
)
|
||||
.withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withInverted(driveInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive)
|
||||
).withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
.withStatorCurrentLimit(DRIVE_MAX_CURRENT)
|
||||
);
|
||||
}
|
||||
|
||||
private TalonFXConfiguration generateSteerTalonConfiguration(double kP, double kI, double kD, double kS, double kV, double kA){
|
||||
return new TalonFXConfiguration().withSlot0(
|
||||
new Slot0Configs()
|
||||
.withKP(kP)
|
||||
.withKI(kI)
|
||||
.withKD(kD)
|
||||
.withKS(kS)
|
||||
.withKV(kV)
|
||||
.withKA(kA)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign)
|
||||
).withMotionMagic(
|
||||
new MotionMagicConfigs()
|
||||
.withMotionMagicCruiseVelocity(160)
|
||||
.withMotionMagicAcceleration(160)
|
||||
.withMotionMagicJerk(1600)
|
||||
).withFeedback(
|
||||
new FeedbackConfigs()
|
||||
.withFeedbackRemoteSensorID(steerEncoder.getDeviceID())
|
||||
.withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
|
||||
.withSensorToMechanismRatio(1.0)
|
||||
.withRotorToSensorRatio(12.8)
|
||||
).withClosedLoopGeneral(
|
||||
new ClosedLoopGeneralConfigs()
|
||||
.withContinuousWrap(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withInverted(InvertedValue.Clockwise_Positive)
|
||||
).withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
.withStatorCurrentLimit(STEER_MAX_CURRENT)
|
||||
);
|
||||
}
|
||||
|
||||
private CANcoderConfiguration generateCanCoderConfiguration(double steerOffset){
|
||||
return new CANcoderConfiguration().withMagnetSensor(
|
||||
new MagnetSensorConfigs()
|
||||
.withMagnetOffset(-steerOffset)
|
||||
.withSensorDirection(SensorDirectionValue.CounterClockwise_Positive)
|
||||
.withAbsoluteSensorDiscontinuityPoint(0.5)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
/*
|
||||
* This Java source file was generated by the Gradle 'init' task.
|
||||
*/
|
||||
package com.fairportrobotics.frc.robolib;
|
||||
package org.fairportrobotics.frc.robolib;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import static org.junit.jupiter.api.Assertions.*;
|
||||
|
|
|
|||
Loading…
Reference in New Issue