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"?>
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
<projectDescription>
|
<projectDescription>
|
||||||
<name>RoboLib</name>
|
<name>potential-octo-eureka-RoboLib</name>
|
||||||
<comment>Project RoboLib created by Buildship.</comment>
|
<comment>Project RoboLib created by Buildship.</comment>
|
||||||
<projects>
|
<projects>
|
||||||
</projects>
|
</projects>
|
||||||
|
|
|
||||||
|
|
@ -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
|
# Including in FRC Project
|
||||||
To include this library in a Java FRC Robot 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()
|
mavenLocal()
|
||||||
gradlePluginPortal()
|
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://maven.ctr-electronics.com/release/"}
|
||||||
|
maven {url "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"}
|
||||||
}
|
}
|
||||||
|
|
||||||
dependencies {
|
dependencies {
|
||||||
// Use JUnit Jupiter for testing.
|
// 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
|
// WPI Lib
|
||||||
implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion"
|
implementation "edu.wpi.first.cscore:cscore-java:$wpiVersion"
|
||||||
|
|
@ -48,6 +48,9 @@ dependencies {
|
||||||
// CTRE Phoenix 6
|
// CTRE Phoenix 6
|
||||||
implementation "com.ctre.phoenix6:wpiapi-java:${ctreVersion}"
|
implementation "com.ctre.phoenix6:wpiapi-java:${ctreVersion}"
|
||||||
|
|
||||||
|
// AdvantageKit
|
||||||
|
implementation "org.littletonrobotics.akit:akit-java:${advantageKitVersion}"
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
tasks.named('test') {
|
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.
|
* 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 org.junit.jupiter.api.Test;
|
||||||
import static org.junit.jupiter.api.Assertions.*;
|
import static org.junit.jupiter.api.Assertions.*;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue