-- java.lang.Object
+
- java.lang.Object
-- java.lang.Enum<E> (implements java.lang.Comparable<T>, java.io.Serializable)
+
- java.lang.Enum<E> (implements java.lang.Comparable<T>, java.io.Serializable)
@@ -185,7 +197,6 @@ $('.navPadding').css('padding-top', $('.fixedNav').css("height"));
- Overview
- Package
- Class
-- Use
- Tree
- Deprecated
- Index
diff --git a/docs/package-search-index.js b/docs/package-search-index.js
index fe97fbc..9c5017f 100644
--- a/docs/package-search-index.js
+++ b/docs/package-search-index.js
@@ -1 +1 @@
-packageSearchIndex = [{"l":"All Packages","url":"allpackages-index.html"},{"l":"frc.robot.subsystems.swervedrive.swerve"},{"l":"frc.robot.subsystems.swervedrive.swerve.encoders"},{"l":"frc.robot.subsystems.swervedrive.swerve.kinematics"},{"l":"frc.robot.subsystems.swervedrive.swerve.motors"}]
\ No newline at end of file
+packageSearchIndex = [{"l":"All Packages","url":"allpackages-index.html"},{"l":"frc.robot.subsystems.swervedrive2.swervelib"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.encoders"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.imu"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.math"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.motors"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.parser"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.parser.deserializer"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.parser.json"},{"l":"frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules"}]
\ No newline at end of file
diff --git a/docs/search.js b/docs/search.js
index b773531..8492271 100644
--- a/docs/search.js
+++ b/docs/search.js
@@ -50,7 +50,7 @@ function getURLPrefix(ui) {
return ui.item.m + slash;
} else if ((ui.item.category === catTypes && ui.item.p) || ui.item.category === catMembers) {
$.each(packageSearchIndex, function(index, item) {
- if (ui.item.p == item.l) {
+ if (item.m && ui.item.p == item.l) {
urlPrefix = item.m + slash;
}
});
diff --git a/docs/stylesheet.css b/docs/stylesheet.css
index fa24676..de945ed 100644
--- a/docs/stylesheet.css
+++ b/docs/stylesheet.css
@@ -443,28 +443,28 @@ table tr td dl, table tr td dl dt, table tr td dl dd {
margin:0px;
white-space:pre;
}
+.constantsSummary caption a:link, .constantsSummary caption a:visited,
+.useSummary caption a:link, .useSummary caption a:visited {
+ color:#1f389c;
+}
.overviewSummary caption a:link, .memberSummary caption a:link, .typeSummary caption a:link,
-.constantsSummary caption a:link, .deprecatedSummary caption a:link,
+.deprecatedSummary caption a:link,
.requiresSummary caption a:link, .packagesSummary caption a:link, .providesSummary caption a:link,
.usesSummary caption a:link,
.overviewSummary caption a:hover, .memberSummary caption a:hover, .typeSummary caption a:hover,
-.constantsSummary caption a:hover, .deprecatedSummary caption a:hover,
+.useSummary caption a:hover, .constantsSummary caption a:hover, .deprecatedSummary caption a:hover,
.requiresSummary caption a:hover, .packagesSummary caption a:hover, .providesSummary caption a:hover,
.usesSummary caption a:hover,
.overviewSummary caption a:active, .memberSummary caption a:active, .typeSummary caption a:active,
-.constantsSummary caption a:active, .deprecatedSummary caption a:active,
+.useSummary caption a:active, .constantsSummary caption a:active, .deprecatedSummary caption a:active,
.requiresSummary caption a:active, .packagesSummary caption a:active, .providesSummary caption a:active,
.usesSummary caption a:active,
.overviewSummary caption a:visited, .memberSummary caption a:visited, .typeSummary caption a:visited,
-.constantsSummary caption a:visited, .deprecatedSummary caption a:visited,
+.deprecatedSummary caption a:visited,
.requiresSummary caption a:visited, .packagesSummary caption a:visited, .providesSummary caption a:visited,
.usesSummary caption a:visited {
color:#FFFFFF;
}
-.useSummary caption a:link, .useSummary caption a:hover, .useSummary caption a:active,
-.useSummary caption a:visited {
- color:#1f389c;
-}
.overviewSummary caption span, .memberSummary caption span, .typeSummary caption span,
.useSummary caption span, .constantsSummary caption span, .deprecatedSummary caption span,
.requiresSummary caption span, .packagesSummary caption span, .providesSummary caption span,
@@ -767,6 +767,7 @@ main, nav, header, footer, section {
ul.ui-autocomplete {
position:fixed;
z-index:999999;
+ background-color: #FFFFFF;
}
ul.ui-autocomplete li {
float:left;
@@ -776,6 +777,9 @@ ul.ui-autocomplete li {
.resultHighlight {
font-weight:bold;
}
+.ui-autocomplete .result-item {
+ font-size: inherit;
+}
#search {
background-image:url('resources/glass.png');
background-size:13px;
diff --git a/docs/type-search-index.js b/docs/type-search-index.js
index abfb93f..f0342bd 100644
--- a/docs/type-search-index.js
+++ b/docs/type-search-index.js
@@ -1 +1 @@
-typeSearchIndex = [{"l":"All Classes","url":"allclasses-index.html"},{"p":"frc.robot.subsystems.swervedrive.swerve.motors","l":"CTRESwerveMotor.CTRE_pidIdx"},{"p":"frc.robot.subsystems.swervedrive.swerve.motors","l":"CTRESwerveMotor.CTRE_remoteSensor"},{"p":"frc.robot.subsystems.swervedrive.swerve.motors","l":"CTRESwerveMotor.CTRE_slotIdx"},{"p":"frc.robot.subsystems.swervedrive.swerve.encoders","l":"CTRECANCoder"},{"p":"frc.robot.subsystems.swervedrive.swerve.motors","l":"CTRESwerveMotor"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveMotor.ModuleMotorType"},{"p":"frc.robot.subsystems.swervedrive.swerve.encoders","l":"PWMAnalogEncoder"},{"p":"frc.robot.subsystems.swervedrive.swerve.encoders","l":"PWMDutyCycleEncoder"},{"p":"frc.robot.subsystems.swervedrive.swerve.motors","l":"REVSwerveMotor.REV_slotIdx"},{"p":"frc.robot.subsystems.swervedrive.swerve.encoders","l":"REVAbsoluteEncoder"},{"p":"frc.robot.subsystems.swervedrive.swerve.motors","l":"REVSwerveMotor"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveDrive"},{"p":"frc.robot.subsystems.swervedrive.swerve.kinematics","l":"SwerveDriveKinematics2"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveEncoder"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveModule"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveDrive.SwerveModuleConfig"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveModule.SwerveModuleLocation"},{"p":"frc.robot.subsystems.swervedrive.swerve.kinematics","l":"SwerveModuleState2"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveMotor"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveParser"},{"p":"frc.robot.subsystems.swervedrive.swerve","l":"SwerveModule.Verbosity"}]
\ No newline at end of file
+typeSearchIndex = [{"l":"All Classes","url":"allclasses-index.html"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.encoders","l":"CANCoderSwerve"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"DeviceJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules","l":"LocationJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"ModuleJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"MotorConfigDouble"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"MotorConfigInt"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.imu","l":"NavXSwerve"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser","l":"PIDFConfig"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.deserializer","l":"PIDFRange"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.imu","l":"Pigeon2Swerve"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.imu","l":"PigeonSwerve"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.motors","l":"SparkMaxSwerve"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"frc.robot.subsystems.swervedrive2.swervelib","l":"SwerveController"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"frc.robot.subsystems.swervedrive2.swervelib","l":"SwerveDrive"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser.json","l":"SwerveDriveJson"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.imu","l":"SwerveIMU"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.math","l":"SwerveKinematics2"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.math","l":"SwerveMath"},{"p":"frc.robot.subsystems.swervedrive2.swervelib","l":"SwerveModule"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.math","l":"SwerveModuleState2"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.motors","l":"SwerveMotor"},{"p":"frc.robot.subsystems.swervedrive2.swervelib.parser","l":"SwerveParser"}]
\ No newline at end of file
diff --git a/swervelib/SwerveController.java b/swervelib/SwerveController.java
new file mode 100644
index 0000000..95af09b
--- /dev/null
+++ b/swervelib/SwerveController.java
@@ -0,0 +1,109 @@
+package frc.robot.subsystems.swervedrive2.swervelib;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveControllerConfiguration;
+
+/**
+ * Controller class used to convert raw inputs into robot speeds.
+ */
+public class SwerveController
+{
+
+ /**
+ * {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} for controlling the
+ * robot heading, and deadband for heading joystick.
+ */
+ public final SwerveControllerConfiguration config;
+ /**
+ * PID Controller for the robot heading.
+ */
+ private final PIDController thetaController; // TODO: Switch to ProfilePIDController
+ /**
+ * Last angle to robot was set to.
+ */
+ public double lastAngle;
+
+ /**
+ * Construct the SwerveController object which is used for determining the speeds of the robot based on controller
+ * input.
+ */
+ public SwerveController(SwerveControllerConfiguration cfg)
+ {
+ config = cfg;
+ thetaController = config.headingPIDF.createPIDController();
+ thetaController.enableContinuousInput(-Math.PI, Math.PI);
+ lastAngle = 0;
+ }
+
+ /**
+ * Helper function to get the {@link Translation2d} of the chassis speeds given the {@link ChassisSpeeds}.
+ *
+ * @param speeds Chassis speeds.
+ * @return {@link Translation2d} of the speed the robot is going in.
+ */
+ public static Translation2d getTranslation2d(ChassisSpeeds speeds)
+ {
+ return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+ }
+
+ /**
+ * Calculate the hypot deadband and check if the joystick is within it.
+ *
+ * @param x The x value for the joystick in which the deadband should be applied.
+ * @param y The y value for the joystick in which the deadband should be applied.
+ */
+ public boolean withinHypotDeadband(double x, double y)
+ {
+ return Math.hypot(x, y) < config.angleJoyStickRadiusDeadband;
+ }
+
+ /**
+ * Get the chassis speeds based on controller input of 1 joystick and a angle.
+ *
+ * @param xInput X joystick input for the robot to move in the X direction.
+ * @param yInput Y joystick input for the robot to move in the Y direction.
+ * @param angle The desired angle of the robot in radians.
+ * @param currentHeadingAngleRadians The current robot heading in radians.
+ * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive.
+ */
+ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double angle, double currentHeadingAngleRadians)
+ {
+ // Calculates an angular rate using a PIDController and the commanded angle. This is then scaled by
+ // the drivebase's maximum angular velocity.
+ double omega = thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity;
+ // Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function
+ // to allow for precise control and fast movement.
+ double x = Math.pow(xInput, 3) * config.maxSpeed;
+ double y = Math.pow(yInput, 3) * config.maxSpeed;
+
+ return new ChassisSpeeds(x, y, omega);
+ }
+
+ /**
+ * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for
+ * the angle of the robot.
+ *
+ * @param xInput X joystick input for the robot to move in the X direction.
+ * @param yInput Y joystick input for the robot to move in the Y direction.
+ * @param headingX X joystick which controls the angle of the robot.
+ * @param headingY Y joystick which controls the angle of the robot.
+ * @param currentHeadingAngleRadians The current robot heading in radians.
+ * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive.
+ */
+ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY,
+ double currentHeadingAngleRadians)
+ {
+ // Converts the horizontal and vertical components to the commanded angle, in radians, unless the joystick is near
+ // the center (i. e. has been released), in which case the angle is held at the last valid joystick input (hold
+ // position when stick released).
+ double angle = withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY);
+ ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians);
+
+ // Used for the position hold feature
+ lastAngle = angle;
+
+ return speeds;
+ }
+}
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index 65097cc..05ac76b 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -1,731 +1,395 @@
-package frc.robot.subsystems.swervedrive.swerve;
+package frc.robot.subsystems.swervedrive2.swervelib;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.util.sendable.SendableRegistry;
-import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.drive.RobotDriveBase;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
-import frc.robot.subsystems.swervedrive.swerve.SwerveModule.SwerveModuleLocation;
-import frc.robot.subsystems.swervedrive.swerve.SwerveModule.Verbosity;
-import frc.robot.subsystems.swervedrive.swerve.SwerveMotor.ModuleMotorType;
-import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveDriveKinematics2;
-import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveModuleState2;
-import java.io.Closeable;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.SwerveIMU;
+import frc.robot.subsystems.swervedrive2.swervelib.math.SwerveKinematics2;
+import frc.robot.subsystems.swervedrive2.swervelib.math.SwerveModuleState2;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveControllerConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveDriveConfiguration;
+import java.util.ArrayList;
+import java.util.List;
/**
- * SwerveDrive base which is meant to be platform agnostic. This implementation expect second order kinematics because
- * second order kinematics prevents the drift that builds up when using first order kinematics. As per this ChiefDelphi
- * post
+ * Swerve Drive class representing and controlling the swerve drive.
*/
-public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseable
+public class SwerveDrive
{
+ //
+ // Swerve base kinematics object
+ /**
+ * Swerve Kinematics object utilizing second order kinematics.
+ */
+ public final SwerveKinematics2 kinematics;
+ /**
+ * Swerve drive configuration.
+ */
+ public final SwerveDriveConfiguration swerveDriveConfiguration;
+ /**
+ * Swerve odometry.
+ */
+ public final SwerveDrivePoseEstimator swerveDrivePoseEstimator;
+ /**
+ * Swerve modules.
+ */
+ private final SwerveModule[] swerveModules;
+ /**
+ * Field object.
+ */
+ public Field2d field = new Field2d();
+ /**
+ * Swerve controller for controlling heading of the robot.
+ */
+ public SwerveController swerveController;
+ /**
+ * Swerve IMU device for sensing the heading of the robot.
+ */
+ private SwerveIMU imu;
+ /**
+ * The current angle of the robot and last time odometry during simulations.
+ */
+ private double angle, lastTime;
+ /**
+ * Time during simulations.
+ */
+ private Timer timer;
/**
- * Count of SwerveModule instances created.
+ * Creates a new swerve drivebase subsystem. Robot is controlled via the drive() method, or via the setModuleStates()
+ * method. The drive() method incorporates kinematics— it takes a translation and rotation, as well as parameters for
+ * field-centric and closed-loop velocity control. setModuleStates() takes a list of SwerveModuleStates and directly
+ * passes them to the modules. This subsystem also handles odometry.
*/
- private static int instances;
-
- /**
- * Front left swerve drive
- */
- public final SwerveModule m_frontLeft;
- /**
- * Back left swerve drive
- */
- public final SwerveModule m_backLeft;
- /**
- * Front right swerve drive
- */
- public final SwerveModule m_frontRight;
- /**
- * Back right swerve drive
- */
- public final SwerveModule m_backRight;
- /**
- * Maximum speed in meters per second.
- */
- public final double m_driverMaxSpeedMPS, m_driverMaxAngularVelocity, m_physicalMaxSpeedMPS, m_driveAccellerationMetersPerSecondSquared;
- /**
- * Swerve drive pose estimator for attempting to figure out our current position.
- */
- public final SwerveDrivePoseEstimator m_swervePoseEstimator;
- /**
- * Pigeon 2.0 centered on the robot.
- */
- public final WPI_Pigeon2 m_pigeonIMU;
- /**
- * Swerve drive kinematics.
- */
- public final SwerveDriveKinematics2 m_swerveKinematics;
- /**
- * Field2d displayed on shuffleboard with current position.
- */
- private final Field2d m_field = new Field2d();
- /**
- * The slew rate limiters to make control smooth.
- */
- private final SlewRateLimiter m_xLimiter, m_yLimiter, m_turningLimiter;
- private final Timer m_driveTimer;
- /**
- * Invert the gyro reading.
- */
- private boolean m_gyroInverted;
- /**
- * Robot desired angle in degrees.
- */
- private double m_angle;
- /**
- * Previous chassis speeds for state-space modeling.
- */
- private ChassisSpeeds m_prevChassisSpeed = new ChassisSpeeds(0, 0, 0);
- /**
- * Previous timer value for state-space modeling.
- */
- private double m_timerPrev;
-
- /**
- * Constructor for Swerve Drive assuming modules have been created and configured with PIDF and conversions.
- *
- * @param frontLeft Front left swerve module configured.
- * @param backLeft Back left swerve module.
- * @param frontRight Front right swerve module.
- * @param backRight Back right swerve module
- * @param pigeon Pigeon IMU.
- * @param driverMaxSpeedMetersPerSecond Maximum speed for all modules to follow when in teleop.
- * @param driverMaxAngularVelocityRadiansPerSecond Maximum angular velocity for turning when using the drive
- * function.
- * @param driverMaxDriveAccelerationMetersPerSecond Maximum acceleration in meters per second for the drive motors
- * when in teleop for the slew rate limiter.
- * @param driverMaxAngularAccelerationRadiansPerSecond Maximum angular acceleration in meters per second for the
- * steering motors when in teleop for the slew rate limiters.
- * @param physicalMaxSpeedMPS Maximum speed a module can go physically, used to desaturate
- * wheel speeds.
- * @param gyroInverted Invert the gyroscope for the robot.
- * @param start The starting pose on the field.
- */
- public SwerveDrive(SwerveModule frontLeft,
- SwerveModule frontRight,
- SwerveModule backLeft,
- SwerveModule backRight, WPI_Pigeon2 pigeon,
- double driverMaxSpeedMetersPerSecond, double driverMaxAngularVelocityRadiansPerSecond,
- double driverMaxDriveAccelerationMetersPerSecond,
- double driverMaxAngularAccelerationRadiansPerSecond,
- double physicalMaxSpeedMPS,
- boolean gyroInverted, Pose2d start)
+ public SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)
{
- instances++;
- if (instances > 1)
+ swerveDriveConfiguration = config;
+ swerveController = new SwerveController(controllerConfig);
+ // Create Kinematics from swerve module locations.
+ kinematics = new SwerveKinematics2(config.moduleLocationsMeters);
+
+ // Create an integrator for angle if the robot is being simulated to emulate an IMU
+ // If the robot is real, instantiate the IMU instead.
+ if (!Robot.isReal())
{
- throw new RuntimeException("Cannot use more than one swerve drive instance at a time.");
+ timer = new Timer();
+ timer.start();
+ lastTime = 0;
+ } else
+ {
+ imu = config.imu;
+ imu.factoryDefault();
}
- m_driveTimer = new Timer();
- m_driveTimer.reset();
- m_driveTimer.start();
- m_timerPrev = m_driveTimer.get();
- m_frontLeft = frontLeft;
- m_backRight = backRight;
- m_backLeft = backLeft;
- m_frontRight = frontRight;
- m_gyroInverted = gyroInverted;
+ this.swerveModules = config.modules;
- m_driverMaxSpeedMPS = driverMaxSpeedMetersPerSecond;
- m_driverMaxAngularVelocity = driverMaxAngularVelocityRadiansPerSecond;
- m_driveAccellerationMetersPerSecondSquared = driverMaxDriveAccelerationMetersPerSecond;
- m_xLimiter = new SlewRateLimiter(driverMaxDriveAccelerationMetersPerSecond);
- m_yLimiter = new SlewRateLimiter(driverMaxDriveAccelerationMetersPerSecond);
- m_turningLimiter = new SlewRateLimiter(driverMaxAngularAccelerationRadiansPerSecond);
- m_physicalMaxSpeedMPS = physicalMaxSpeedMPS;
-
- m_pigeonIMU = pigeon;
- configurePigeonIMU(); // Reset pigeon to 0 and default settings.
-
- m_swerveKinematics = new SwerveDriveKinematics2(frontLeft.swerveModuleLocation,
- frontRight.swerveModuleLocation,
- backLeft.swerveModuleLocation,
- backRight.swerveModuleLocation);
- m_swervePoseEstimator = new SwerveDrivePoseEstimator(
- m_swerveKinematics,
- getRotation(),
- getPositions(),
- start,
+// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
+ swerveDrivePoseEstimator = new SwerveDrivePoseEstimator(
+ kinematics,
+ getYaw(),
+ getModulePositions(),
+ new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
VecBuilder.fill(0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
VecBuilder.fill(0.9, 1.0, 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
- // Inspired by https://github.com/Team364/BaseFalconSwerve/blob/main/src/main/java/frc/robot/subsystems/Swerve.java
- SmartDashboard.putData(m_field);
- SmartDashboard.putData(m_pigeonIMU);
-
- setVoltageCompensation();
- zeroModules(); // Set all modules to 0.
- }
-
- /**
- * Create swerve drive modules
- *
- * @param driveGearRatio Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder ticks
- * per rotation.
- * @param steerGearRatio Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or
- * rotations/1), only applied if using Neo's.
- * @param wheelDiameterMeters The wheel diameter of the swerve drive module in meters.
- * @param wheelBaseMeters The Distance between front and back wheels of the robot in meters.
- * @param driveTrainWidthMeters The Distance between centers of right and left wheels in meters.
- * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor.
- * @param maxSpeedMPS The maximum drive speed in meters per second.
- * @param maxDriveAcceleration The maximum drive acceleration in meters^2 per second.
- * @param steeringMotorInverted The steering motor is inverted.
- * @param drivingMotorInverted The driving motor is inverted.
- * @param configs The swerve module configuration classes for the swerve drive given.
- * @return Array of swerve modules in the order of front left, front right, back left, back right.
- */
- public static SwerveModule[] createModules(
- double driveGearRatio, double steerGearRatio, double wheelDiameterMeters, double wheelBaseMeters,
- double driveTrainWidthMeters, double steeringMotorFreeSpeedRPM, double maxSpeedMPS,
- double maxDriveAcceleration, boolean steeringMotorInverted, boolean drivingMotorInverted,
- SwerveModuleConfig[] configs)
- {
- SwerveModule[] modules = new SwerveModule[configs.length];
- for (int i = 0; i < configs.length; i++)
- {
- int loc;
- switch (configs[i].loc)
- {
- case FrontLeft:
- loc = 0;
- break;
- case BackLeft:
- loc = 2;
- break;
- case FrontRight:
- loc = 1;
- break;
- case BackRight:
- loc = 3;
- break;
- default:
- loc = i;
- }
-
- modules[loc] = configs[i].createModule(driveGearRatio, steerGearRatio, wheelDiameterMeters, wheelBaseMeters,
- driveTrainWidthMeters, steeringMotorFreeSpeedRPM, maxSpeedMPS,
- maxDriveAcceleration, steeringMotorInverted, drivingMotorInverted);
- }
- return modules;
- }
-
- /**
- * Enable voltage compensation to the current battery voltage on all modules.
- */
- public void setVoltageCompensation()
- {
- double currentVoltage = RobotController.getBatteryVoltage();
- m_frontLeft.setVoltageCompensation(currentVoltage);
- m_frontRight.setVoltageCompensation(currentVoltage);
- m_backLeft.setVoltageCompensation(currentVoltage);
- m_backRight.setVoltageCompensation(currentVoltage);
- }
-
- /**
- * Sets the speed to 0 and angle to 0 for all the swerve drive modules.
- */
- public void zeroModules()
- {
- // feedWatchdog();
- // m_frontLeft.setAngle(0, 0);
- // m_backLeft.setAngle(0, 0);
- // m_frontRight.setAngle(0, 0);
- // m_backRight.setAngle(0, 0);
- m_frontRight.set(0);
- m_backLeft.set(0);
- m_frontLeft.set(0);
- m_backRight.set(0);
- set(0, 0, 0, false);
- }
-
- /**
- * Configure the PigeonIMU with factory default settings and a zeroed gyroscope.
- */
- public void configurePigeonIMU()
- {
- m_pigeonIMU.configFactoryDefault();
zeroGyro();
+ SmartDashboard.putData("Field", field);
}
/**
- * Update the swerve drive odometry.
+ * The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and
+ * commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel
+ * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.
*
- * @return Swerve drive odometry.
+ * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per
+ * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is
+ * torwards port (left). In field-relative mode, positive x is away from the alliance wall
+ * (field North) and positive y is torwards the left wall when looking through the driver station
+ * glass (field West).
+ * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot
+ * relativity.
+ * @param fieldRelative Drive mode. True for field-relative, false for robot-relative.
+ * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
- public SwerveDrivePoseEstimator update()
+ public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
{
- Pose2d pos = m_swervePoseEstimator.update(getRotation(), getPositions());
- if (!Robot.isReal())
+ // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if necessary.
+ ChassisSpeeds velocity = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(),
+ translation.getY(),
+ rotation,
+ getYaw()) : new ChassisSpeeds(
+ translation.getX(),
+ translation.getY(),
+ rotation);
+
+ // Display commanded speed for testing
+ SmartDashboard.putString("RobotVelocity", velocity.toString());
+
+ // Calculate required module states via kinematics
+ SwerveModuleState2[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity);
+
+ setModuleStates(swerveModuleStates, isOpenLoop);
+ }
+
+ /**
+ * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.
+ *
+ * @param desiredStates A list of SwerveModuleStates to send to the modules.
+ * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
+ */
+ public void setModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
+ {
+ // Desaturates wheel speeds
+ SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
+
+ // Sets states
+ for (SwerveModule module : swerveModules)
{
- m_pigeonIMU.setYaw(pos.getRotation().getDegrees());
- m_field.getObject("XModules").setPoses(
- pos.plus(new Transform2d(m_frontLeft.swerveModuleLocation, m_frontLeft.getPosition().angle)),
- pos.plus(new Transform2d(m_frontRight.swerveModuleLocation, m_frontRight.getPosition().angle)),
- pos.plus(new Transform2d(m_backLeft.swerveModuleLocation, m_backLeft.getPosition().angle)),
- pos.plus(new Transform2d(m_backRight.swerveModuleLocation, m_backRight.getPosition().angle)));
- }
- m_field.setRobotPose(pos);
-
- return m_swervePoseEstimator;
- }
-
- /**
- * Algebraically apply a deadband using a piece wise function.
- *
- * @param value value to apply deadband to.
- * @param complex Use algebra to determine deadband by starting the value at 0 past deadband.
- * @return Value with deadband applied.
- */
- private double applyDeadband(double value, boolean complex)
- {
- value = Math.abs(value) > m_deadband ? value : 0;
- return complex ? ((1 / (1 - m_deadband)) * (Math.abs(value) - .2)) * Math.signum(value) : value;
- }
-
- /**
- * Drive swerve based off controller input.
- *
- * @param forward The speed (-1 to 1) in which the Y axis should go.
- * @param strafe The speed (-1 to 1) in which the X axis should go.
- * @param turn The speed (-1 to 1) in which the robot should turn.
- * @param fieldRelative Whether or not to use field relative controls.
- */
- public void drive(double forward, double strafe, double turn, boolean fieldRelative)
- {
- // 2. Apply deadband/Dead-Zone
- forward = applyDeadband(forward, true);
- strafe = applyDeadband(strafe, true);
- turn = applyDeadband(turn, true);
-
- // 3. Make the driving smoother
- forward = m_xLimiter.calculate(forward) * m_driverMaxSpeedMPS;
- strafe = m_yLimiter.calculate(strafe) * m_driverMaxSpeedMPS;
- turn = m_turningLimiter.calculate(turn) * m_driverMaxAngularVelocity;
-
- set(forward, strafe, turn, fieldRelative);
- }
-
-
- /**
- * Swerve drive function
- *
- * @param forward forward meters per second, strafe facing left from alliance wall
- * @param strafe strafe meters per second, forward away from alliance wall
- * @param radianPerSecond radians per second
- * @param fieldRelative field relative
- */
- public void set(double forward, double strafe, double radianPerSecond, boolean fieldRelative)
- {
- ChassisSpeeds node = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, radianPerSecond,
- getRotation())
- : new ChassisSpeeds(forward, strafe, radianPerSecond);
- set(node);
- }
-
- /**
- * Set the swerve drive module states.
- *
- * @param node Chassis speeds to set the swerve module too.
- */
- public void set(ChassisSpeeds node)
- {
-
- // Taken from https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5?u=nstrike
- double timerDt = (m_driveTimer.get() - m_timerPrev);
- Pose2d robot_pose_vel = new Pose2d(node.vxMetersPerSecond * timerDt,
- node.vyMetersPerSecond * timerDt,
- Rotation2d.fromRadians(node.omegaRadiansPerSecond * timerDt));
- Twist2d twist_vel = new Pose2d(0, 0, new Rotation2d(0)).log(robot_pose_vel);
- ChassisSpeeds updated_chassis_speeds = new ChassisSpeeds(
- twist_vel.dx / timerDt, twist_vel.dy / timerDt, twist_vel.dtheta / timerDt);
-
- SwerveModuleState2[] moduleStates = m_swerveKinematics.toSwerveModuleStates(updated_chassis_speeds);
-// new Translation2d((m_frontLeft.swerveModuleLocation.getX() + m_frontRight.swerveModuleLocation.getX()) / 2,
-// (m_frontLeft.swerveModuleLocation.getY() + m_backLeft.swerveModuleLocation.getY()) / 2));
-
- setModuleStates(moduleStates);
- m_prevChassisSpeed = updated_chassis_speeds;
- m_timerPrev = m_driveTimer.get();
-
- try
- {
- if (!Robot.isReal())
- {
- m_angle += updated_chassis_speeds.omegaRadiansPerSecond * 0.02;
- }
- this.update();
- } catch (Exception e)
- {
- System.err.println("Cannot update SwerveDrive Odometry!");
+ module.setDesiredState(desiredStates[module.moduleNumber], false); // Todo: Send isOpenLoop
+ SmartDashboard.putNumber("Module " + module.moduleNumber + " Speed Setpoint: ",
+ desiredStates[module.moduleNumber].speedMetersPerSecond);
+ SmartDashboard.putNumber("Module " + module.moduleNumber + " Angle Setpoint: ",
+ desiredStates[module.moduleNumber].angle.getDegrees());
}
}
/**
- * Set the swerve module states given an array of states. Normalize the wheel speeds to abide by maximum supplied
+ * Set field-relative chassis speeds with closed-loop velocity control.
*
- * @param states Module states in a specified order. [front left, front right, back left, back right]
- * @throws RuntimeException If the CANCoder is inaccessible or not configured.
+ * @param chassisSpeeds Field-relative.
*/
- public void setModuleStates(SwerveModuleState2[] states)
+ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{
- feedWatchdog(); // Required
- SwerveDriveKinematics2.desaturateWheelSpeeds(states, m_physicalMaxSpeedMPS);
- m_frontLeft.setState(states[0]);
- m_frontRight.setState(states[1]);
- m_backLeft.setState(states[2]);
- m_backRight.setState(states[3]);
+ setModuleStates(kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getYaw())),
+ false);
}
- /**
- * Sets the wheels into an X formation to prevent movement.
- */
- public void setX()
- {
- setModuleStates(new SwerveModuleState2[]{new SwerveModuleState2(0, Rotation2d.fromDegrees(45), 0),
- new SwerveModuleState2(0, Rotation2d.fromDegrees(-45), 0),
- new SwerveModuleState2(0, Rotation2d.fromDegrees(-45), 0),
- new SwerveModuleState2(0, Rotation2d.fromDegrees(45), 0)});
- }
/**
- * Invert the gyroscope reading.
+ * Gets the current pose (position and rotation) of the robot, as reported by odometry.
*
- * @param isInverted Inversion of the gryoscope, true is inverted.
- */
- public void setGyroInverted(boolean isInverted)
- {
- m_gyroInverted = isInverted;
- }
-
- /**
- * Get the current robot rotation.
- *
- * @return {@link Rotation2d} of the robot.
- */
- public Rotation2d getRotation()
- {
- if (Robot.isReal())
- {
- return m_gyroInverted ? Rotation2d.fromDegrees(360 - m_pigeonIMU.getYaw()) : Rotation2d.fromDegrees(
- m_pigeonIMU.getYaw());
- } else
- {
- return new Rotation2d(m_angle);
- }
- }
-
- /**
- * Get the current Pose, used in autonomous.
- *
- * @return Current pose based off odometry.
+ * @return The robot's pose
*/
public Pose2d getPose()
{
- return m_swervePoseEstimator.getEstimatedPosition();
+ return swerveDrivePoseEstimator.getEstimatedPosition();
}
/**
- * Get current swerve module positions in order.
+ * Gets the current field-relative velocity (x, y and omega) of the robot
*
- * @return Swerve module positions array.
+ * @return A ChassisSpeeds object of the current field-relative velocity
*/
- public SwerveModulePosition[] getPositions()
+ public ChassisSpeeds getFieldVelocity()
{
- return new SwerveModulePosition[]{m_frontLeft.getPosition(), m_frontRight.getPosition(),
- m_backLeft.getPosition(), m_backRight.getPosition()};
+ // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds,
+ // but not the reverse. However, because this transform is a simple rotation, negating the angle
+ // given as the robot angle reverses the direction of rotation, and the conversion is reversed.
+ return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
}
/**
- * Get current swerve module states in order.
+ * Gets the current robot-relative velocity (x, y and omega) of the robot
*
- * @return Swerve module states array.
+ * @return A ChassisSpeeds object of the current robot-relative velocity
*/
- public SwerveModuleState2[] getStates()
+ public ChassisSpeeds getRobotVelocity()
{
- return new SwerveModuleState2[]{m_frontLeft.getState(), m_frontRight.getState(),
- m_backLeft.getState(), m_backRight.getState()};
+ return kinematics.toChassisSpeeds(getStates());
}
+
/**
- * Reset the odometry given the position and using current rotation from the PigeonIMU 2.
+ * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
+ * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
+ * keep working.
*
- * @param pose Current position on the field.
+ * @param pose The pose to set the odometry to
*/
public void resetOdometry(Pose2d pose)
{
- m_swervePoseEstimator.resetPosition(getRotation(), getPositions(), pose);
+ swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
}
/**
- * Resets the drive encoders to currently read a position of 0.
+ * Gets the current module states (azimuth and velocity)
+ *
+ * @return A list of SwerveModuleStates containing the current module states
*/
- public void resetEncoders()
+ public SwerveModuleState2[] getStates()
{
- m_frontLeft.resetEncoders();
- m_backLeft.resetEncoders();
- m_frontRight.resetEncoders();
- m_backRight.resetEncoders();
+ SwerveModuleState2[] states = new SwerveModuleState2[swerveDriveConfiguration.moduleCount];
+ for (SwerveModule module : swerveModules)
+ {
+ states[module.moduleNumber] = module.getState();
+ }
+ return states;
}
/**
- * Set the current rotation of the gyroscope (pigeonIMU 2) to 0.
+ * Gets the current module positions (azimuth and wheel position (meters))
+ *
+ * @return A list of SwerveModulePositions containg the current module positions
+ */
+ public SwerveModulePosition[] getModulePositions()
+ {
+ SwerveModulePosition[] positions = new SwerveModulePosition[swerveDriveConfiguration.moduleCount];
+ for (SwerveModule module : swerveModules)
+ {
+ positions[module.moduleNumber] = module.getPosition();
+ }
+ return positions;
+ }
+
+ /**
+ * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
*/
public void zeroGyro()
{
- m_pigeonIMU.setYaw(0);
- }
-
- /**
- * Stop all running and turning motors.
- */
- @Override
- public void stopMotor()
- {
- feedWatchdog();
- m_frontLeft.stopMotor();
- m_frontRight.stopMotor();
- m_backLeft.stopMotor();
- m_backRight.stopMotor();
- }
-
- /**
- * Update swerve module parameters based on data in the dashboard.
- */
- public void subscribe()
- {
- m_frontRight.subscribe();
- m_frontLeft.subscribe();
- m_backLeft.subscribe();
- m_backRight.subscribe();
- }
-
- /**
- * Publish data from the Swerve Modules to the dashboard.
- *
- * @param level Verbosity level in terms of CAN utilization.
- */
- public void publish(Verbosity level)
- {
- m_frontRight.publish(level);
- m_frontLeft.publish(level);
- m_backRight.publish(level);
- m_backLeft.publish(level);
- }
-
- /**
- * Set the PIDF coefficients for the closed loop PID onboard the motor controller. Tuning the PID
- *
- * P = .5 and increase it by .1 until oscillations occur, then decrease by .05 then .005 until oscillations
- * stop and angle is perfect or near perfect.
- *
- *
- * I = 0 tune this if your PID never quite reaches the target, after tuning D. Increase this by
- * P*.01 each time and adjust accordingly.
- *
- *
- * D = 0 tune this if the PID accelerates too fast, it will smooth the motion. Increase this by P*10
- * and adjust accordingly.
- *
- *
- * F = 0 tune this if the PID is being used for velocity, the F is multiplied by the target and added
- * to the voltage output. Increase this by 0.01 until the PIDF reaches the desired state in a fast enough manner.
- *
- * Documentation for this is best described by CTRE here.
- *
- * @param p Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
- * @param i Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
- * PID Loop.
- * @param d Derivative gain for closed loop. This is multiplied by derivative error (sensor units per
- * PID loop).
- * @param f Feed Fwd gain for Closed loop.
- * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too
- * far from the target. This prevents unstable oscillation if the kI is too large. Value is in
- * sensor units.
- * @param moduleMotorType Swerve drive motor type.
- */
- public void setPIDF(double p, double i, double d, double f, double integralZone,
- ModuleMotorType moduleMotorType)
- {
- m_frontRight.setPIDF(p, i, d, f, integralZone, moduleMotorType);
- m_frontLeft.setPIDF(p, i, d, f, integralZone, moduleMotorType);
- m_backRight.setPIDF(p, i, d, f, integralZone, moduleMotorType);
- m_backLeft.setPIDF(p, i, d, f, integralZone, moduleMotorType);
- }
-
- /**
- * Synchronize internal steering encoders with the absolute encoder.
- */
- public void synchronizeEncoders()
- {
- m_backRight.synchronizeSteeringEncoder();
- m_backLeft.synchronizeSteeringEncoder();
- m_frontRight.synchronizeSteeringEncoder();
- m_frontLeft.synchronizeSteeringEncoder();
- }
-
- /**
- * Get the description of the robot drive base.
- *
- * @return string of the RobotDriveBase
- */
- @Override
- public String getDescription()
- {
- return "Swerve Drive Base";
- }
-
- /**
- * Initializes this {@link Sendable} object.
- *
- * @param builder sendable builder
- */
- @Override
- public void initSendable(SendableBuilder builder)
- {
-// builder.setSmartDashboardType("SwerveDrive");
-// SendableRegistry.addChild(this, m_frontLeft);
-// SendableRegistry.addChild(this, m_frontRight);
-// SendableRegistry.addChild(this, m_backLeft);
-// SendableRegistry.addChild(this, m_backRight);
-// SendableRegistry.addChild(this, m_field);
-// SendableRegistry.addChild(this, m_pigeonIMU);
- }
-
- /**
- * Closes this resource, relinquishing any underlying resources. This method is invoked automatically on objects
- * managed by the {@code try}-with-resources statement.
- *
- * While this interface method is declared to throw {@code
- * Exception}, implementers are strongly encouraged to declare concrete implementations of the {@code close}
- * method to throw more specific exceptions, or to throw no exception at all if the close operation cannot fail.
- *
- *
Cases where the close operation may fail require careful
- * attention by implementers. It is strongly advised to relinquish the underlying resources and to internally
- * mark the resource as closed, prior to throwing the exception. The {@code close} method is unlikely to be
- * invoked more than once and so this ensures that the resources are released in a timely manner. Furthermore it
- * reduces problems that could arise when the resource wraps, or is wrapped, by another resource.
- *
- *
Implementers of this interface are also strongly advised
- * to not have the {@code close} method throw {@link InterruptedException}.
- *
- * This exception interacts with a thread's interrupted status, and runtime misbehavior is likely to occur if an
- * {@code InterruptedException} is {@linkplain Throwable#addSuppressed suppressed}.
- *
- * More generally, if it would cause problems for an exception to be suppressed, the {@code AutoCloseable.close}
- * method should not throw it.
- *
- *
Note that unlike the {@link Closeable#close close}
- * method of {@link Closeable}, this {@code close} method is not required to be idempotent. In other words,
- * calling this {@code close} method more than once may have some visible side effect, unlike {@code Closeable.close}
- * which is required to have no effect if called more than once.
- *
- * However, implementers of this interface are strongly encouraged to make their {@code close} methods idempotent.
- *
- * @throws Exception if this resource cannot be closed
- */
- @Override
- public void close() throws Exception
- {
- SendableRegistry.remove(this);
- }
-
- /**
- * Helper class for easier swerve module creation
- *
- * @param The motor type for the drive motor on the swerve moduule.
- * @param The motor type for the steering motor on the module.
- * @param The absolute encoder type.
- */
- public static class SwerveModuleConfig
- {
-
- public DriveMotorType drive;
- public SteeringMotorType steering;
- public AbsoluteEncoder encoder;
- public double angleOffset;
- public SwerveModuleLocation loc;
-
- /**
- * Swerve Module configuration class to define the motor CAN IDs and absolute encoder offset of a swerve module.
- *
- * @param driveMotor Drive motor for the swerve module.
- * @param steerMotor Steer motor for the swerve module.
- * @param encoderSteering CANCoder for the steering motor on the swerve module.
- * @param offset Absolute encoder offset.
- * @param location Swerve Moduule location on the chassis.
- */
- public SwerveModuleConfig(DriveMotorType driveMotor, SteeringMotorType steerMotor, AbsoluteEncoder encoderSteering,
- double offset,
- SwerveModuleLocation location)
+ // Resets the real gyro or the angle accumulator, depending on whether the robot is being simulated
+ if (Robot.isReal())
{
- drive = driveMotor;
- steering = steerMotor;
- angleOffset = offset;
- encoder = encoderSteering;
- loc = location;
+ imu.setYaw(0);
+ } else
+ {
+ angle = 0;
}
+ swerveController.lastAngle = 0;
+ resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d()));
+ }
- /**
- * Create the swerve module from the configuration.
- *
- * @param driveGearRatio Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder
- * ticks per rotation.
- * @param steerGearRatio Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or
- * rotations/1), only applied if using Neo's.
- * @param wheelDiameterMeters The wheel diameter of the swerve drive module in meters.
- * @param wheelBaseMeters The Distance between front and back wheels of the robot in meters.
- * @param driveTrainWidthMeters The Distance between centers of right and left wheels in meters.
- * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor.
- * @param maxSpeedMPS The maximum drive speed in meters per second.
- * @param maxDriveAcceleration The maximum drive acceleration in meters^2 per second.
- * @param steeringInverted The steering motor is inverted.
- * @param drivingInverted The driving motor is inverted.
- * @return The Swerve Module.
- */
- public SwerveModule createModule(double driveGearRatio,
- double steerGearRatio,
- double wheelDiameterMeters,
- double wheelBaseMeters,
- double driveTrainWidthMeters,
- double steeringMotorFreeSpeedRPM,
- double maxSpeedMPS,
- double maxDriveAcceleration,
- boolean steeringInverted,
- boolean drivingInverted)
+ /**
+ * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.
+ *
+ * @return The yaw as a {@link Rotation2d} angle
+ */
+ public Rotation2d getYaw()
+ {
+ // Read the imu if the robot is real or the accumulator if the robot is simulated.
+ if (Robot.isReal())
{
- return new SwerveModule<>(drive, steering, encoder, loc,
- driveGearRatio, steerGearRatio,
- angleOffset, wheelDiameterMeters,
- wheelBaseMeters,
- driveTrainWidthMeters,
- steeringMotorFreeSpeedRPM,
- maxSpeedMPS, maxDriveAcceleration, .6, .4, steeringInverted, drivingInverted);
+ double[] ypr = new double[3];
+ imu.getYawPitchRoll(ypr);
+ return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[0] : ypr[0]);
+ } else
+ {
+ return new Rotation2d(angle);
}
}
-}
\ No newline at end of file
+ /**
+ * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.
+ *
+ * @return The heading as a {@link Rotation2d} angle
+ */
+ public Rotation2d getPitch()
+ {
+ // Read the imu if the robot is real or the accumulator if the robot is simulated.
+ if (Robot.isReal())
+ {
+ double[] ypr = new double[3];
+ imu.getYawPitchRoll(ypr);
+ return Rotation2d.fromDegrees(swerveDriveConfiguration.invertedIMU ? 360 - ypr[1] : ypr[1]);
+ } else
+ {
+ return new Rotation2d(angle);
+ }
+ }
+
+ /**
+ * Sets the drive motors to brake/coast mode.
+ *
+ * @param brake True to set motors to brake mode, false for coast.
+ */
+ public void setMotorBrake(boolean brake)
+ {
+ for (SwerveModule swerveModule : swerveModules)
+ {
+ swerveModule.setMotorBrake(brake);
+ }
+ }
+
+ /**
+ * Point all modules toward the robot center, thus making the robot very difficult to move.
+ */
+ public void setDriveBrake()
+ {
+ for (SwerveModule swerveModule : swerveModules)
+ {
+ swerveModule.setDesiredState(new SwerveModuleState2(0,
+ swerveModule.configuration.moduleLocation.getAngle(),
+ 0), true);
+ }
+ }
+
+ /**
+ * Get the swerve module poses and on the field relative to the robot.
+ *
+ * @param robotPose Robot pose.
+ * @return Swerve module poses.
+ */
+ public Pose2d[] getSwerveModulePoses(Pose2d robotPose)
+ {
+ Pose2d[] poseArr = new Pose2d[swerveDriveConfiguration.moduleCount];
+ List poses = new ArrayList<>();
+ for (SwerveModule module : swerveModules)
+ {
+ poses.add(robotPose.plus(new Transform2d(module.configuration.moduleLocation, module.getState().angle)));
+ }
+ return poses.toArray(poseArr);
+ }
+
+ /**
+ * Setup the swerve module feedforward.
+ *
+ * @param feedforward Feedforward for the drive motor on swerve modules.
+ */
+ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward)
+ {
+ for (SwerveModule swerveModule : swerveModules)
+ {
+ swerveModule.feedforward = feedforward;
+ }
+ }
+
+ /**
+ * Update odometry should be run every loop.
+ */
+ public void updateOdometry()
+ {
+ // Update odometry
+ swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
+
+ // Update angle accumulator if the robot is simulated
+ if (!Robot.isReal())
+ {
+ angle += kinematics.toChassisSpeeds(getStates()).omegaRadiansPerSecond * (timer.get() - lastTime);
+ lastTime = timer.get();
+ field.getObject("XModules").setPoses(getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()));
+ }
+
+ field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+
+ double[] moduleStates = new double[8];
+ for (SwerveModule module : swerveModules)
+ {
+ SmartDashboard.putNumber("Module" + module.moduleNumber + "CANCoder", module.getCANCoder());
+ SmartDashboard.putNumber("Module" + module.moduleNumber + "Relative Encoder", module.getRelativeEncoder());
+ moduleStates[module.moduleNumber] = module.getState().angle.getDegrees();
+ moduleStates[module.moduleNumber + 1] = module.getState().speedMetersPerSecond;
+ }
+ SmartDashboard.putNumberArray("moduleStates", moduleStates);
+ }
+
+}
diff --git a/swervelib/SwerveEncoder.java b/swervelib/SwerveEncoder.java
deleted file mode 100644
index 408e67a..0000000
--- a/swervelib/SwerveEncoder.java
+++ /dev/null
@@ -1,102 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve;
-
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.MagnetFieldStrength;
-import com.revrobotics.AbsoluteEncoder;
-import edu.wpi.first.wpilibj.AnalogEncoder;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import frc.robot.subsystems.swervedrive.swerve.encoders.CTRECANCoder;
-import frc.robot.subsystems.swervedrive.swerve.encoders.PWMAnalogEncoder;
-import frc.robot.subsystems.swervedrive.swerve.encoders.PWMDutyCycleEncoder;
-import frc.robot.subsystems.swervedrive.swerve.encoders.REVAbsoluteEncoder;
-
-/**
- * Swerve Encoder class definition for common interfaces.
- */
-public abstract class SwerveEncoder
-{
-
- /**
- * The encoder can be directly referenced for configuration purposes of onboard PIDs.
- */
- public AbsoluteEncoderType m_encoder;
-
- /**
- * Get the SwerveEncoder class from the given encoder types
- *
- * @param encoder Encoder
- * @param One of DutyCycleEncoder, AnalogEncoder, AbsoluteEncoder, and CANCoder
- * @return SwerveEncoder subclass
- */
- public static SwerveEncoder fromEncoder(AbsoluteEncoderType encoder)
- {
- if (encoder instanceof CANCoder)
- {
- return new CTRECANCoder((CANCoder) encoder);
- } else if (encoder instanceof DutyCycleEncoder)
- {
- return new PWMDutyCycleEncoder((DutyCycleEncoder) encoder);
- } else if (encoder instanceof AnalogEncoder)
- {
- return new PWMAnalogEncoder((AnalogEncoder) encoder);
- } else if (encoder instanceof AbsoluteEncoder)
- {
- return new REVAbsoluteEncoder((AbsoluteEncoder) encoder);
- }
-
- return null;
- }
-
- /**
- * Configure the absolute encoder if possible.
- */
- public abstract void configure();
-
- /**
- * Reset encoder to factory default settings, if possible.
- */
- public abstract void factoryDefault();
-
- /**
- * Get the magnetic field strength, if available.
- *
- * @return CTRE MagneticFieldStrength Enum.
- */
- public abstract MagnetFieldStrength getMagnetFieldStrength();
-
- /**
- * Get the absolute position in degrees.
- *
- * @return Absolute position (0, 360]
- */
- public abstract double getAbsolutePosition();
-
- /**
- * Get the velocity of the absolute encoder in degrees per second.
- *
- * @return Velocity in degrees per second.
- */
- public abstract double getVelocity();
-
- /**
- * Configure the magnetic offset for the AbsoluteEncoder.
- *
- * @param offset Offset in degrees.
- */
- public abstract void setOffset(double offset);
-
- /**
- * Is the encoder reachable?
- *
- * @return True if reachable, false otherwise.
- */
- public abstract boolean reachable();
-
- /**
- * Configure the sensor direction
- *
- * @param isInverted Inverted or not.
- */
- public abstract void setInverted(boolean isInverted);
-
-}
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index fd87d33..d08a4c0 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -1,938 +1,259 @@
-package frc.robot.subsystems.swervedrive.swerve;
+package frc.robot.subsystems.swervedrive2.swervelib;
-import static java.util.Objects.requireNonNull;
-
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.MagnetFieldStrength;
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.CANSparkMax;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
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.networktables.GenericEntry;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.util.sendable.SendableRegistry;
-import edu.wpi.first.wpilibj.AnalogEncoder;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
-import frc.robot.subsystems.swervedrive.swerve.SwerveMotor.ModuleMotorType;
-import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveModuleState2;
-import java.io.Closeable;
-import java.util.HashMap;
-import java.util.Map;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.math.SwerveModuleState2;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModuleConfiguration;
/**
- * Swerve module for representing a single swerve module of the robot.
- *
- * @param Main motor type that drives the wheel.
- * @param Motor that controls the angle of the wheel.
- * @param Absolute encoder for the swerve drive module.
+ * The Swerve Module class which represents and controls Swerve Modules for the swerve drive.
*/
-public class SwerveModule
- implements MotorController, Sendable, AutoCloseable
+public class SwerveModule
{
/**
- * Swerve Module location object relative to the center of the robot.
+ * Swerve module configuration options.
*/
- public final Translation2d swerveModuleLocation;
+ public final SwerveModuleConfiguration configuration;
/**
- * Motor Controllers for drive motor of the swerve module.
+ * Angle offset from the absolute encoder.
*/
- public final SwerveMotor driveMotor;
- /***
- * Motor Controller for the turning motor of the swerve drive module.
- */
- public final SwerveMotor turningMotor;
+ private final double angleOffset;
/**
- * Enum representing the swerve module's location on the robot, assuming square.
+ * Swerve Motors.
*/
- private final SwerveModuleLocation swerveLocation;
+ private final SwerveMotor angleMotor, driveMotor;
/**
- * Absolute encoder for the swerve module.
+ * Absolute encoder for swerve drive.
*/
- private final SwerveEncoder absoluteEncoder;
+ private final SwerveAbsoluteEncoder absoluteEncoder;
/**
- * The Distance between centers of right and left wheels in meters.
+ * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
- private final double driveTrainWidth;
+ public int moduleNumber;
/**
- * The Distance between front and back wheels of the robot in meters.
+ * Feedforward for drive motor during closed loop control.
*/
- private final double wheelBase;
+ public SimpleMotorFeedforward feedforward;
/**
- * kV for steering feedforward.
+ * Last angle set for the swerve module.
*/
- private final double steeringKV;
- private final ShuffleboardTab moduleTab;
- private final HashMap NT4Entries = new HashMap<>();
+ public double lastAngle;
/**
- * Drive feedforward for PID when driving by velocity.
+ * Current state.
*/
- public SimpleMotorFeedforward driveFeedforward;
+ public double angle, omega, speed, fakePos, lastTime, dt;
/**
- * Angle offset of the CANCoder at initialization.
+ * Timer for simulation.
*/
- public double angleOffset = 0;
- /**
- * Maximum speed in meters per second, used to eliminate unnecessary movement of the module.
- */
- public double maxDriveSpeedMPS;
- /**
- * Inverted drive motor.
- */
- private boolean invertedDrive = false;
- /**
- * Inverted turning motor.
- */
- private boolean invertedTurn = false;
- /**
- * Power to drive motor from -1 to 1.
- */
- private double drivePower = 0;
- /**
- * Store the last angle for optimization.
- */
- private double targetAngle;
- /**
- * Angular velocity in radians per second.
- */
- private double targetAngularVelocityRPS = 0;
- /**
- * Target velocity for the swerve module.
- */
- private double targetVelocity = 0;
-
- ///////////////////////////// CONFIGURATION FUNCTIONS SECTION ///////////////////////////////////////////////////
+ private Timer time;
/**
- * Swerve module constructor. Both motors MUST be a {@link MotorController} class. It is recommended to create
- * a command to reset the encoders when triggered and
+ * Construct the swerve module and initialize the swerve module motors and absolute encoder.
*
- * @param mainMotor Main drive motor. Must be a {@link MotorController} type.
- * @param angleMotor Angle motor for controlling the angle of the swerve module.
- * @param encoder Absolute encoder for the swerve module.
- * @param driveGearRatio Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder ticks
- * per rotation.
- * @param steerGearRatio Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or
- * rotations/1), only applied if using Neo's.
- * @param swervePosition Swerve Module position on the robot.
- * @param steeringOffsetDegrees The current offset of the absolute encoder from 0 in degrees.
- * @param wheelDiameterMeters The wheel diameter of the swerve drive module in meters.
- * @param wheelBaseMeters The Distance between front and back wheels of the robot in meters.
- * @param driveTrainWidthMeters The Distance between centers of right and left wheels in meters.
- * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor.
- * @param maxSpeedMPS The maximum drive speed in meters per second.
- * @param maxDriveAcceleration The maximum drive acceleration in meters^2 per second.
- * @param drivingPowerLimit The power limit for the closed loop PID of the driver motor.
- * @param steeringPowerLimit The power limit for the closed loop PID of the steering motor.
- * @param steeringInverted The steering motor is inverted.
- * @param drivingInverted The driving motor is inverted.
- * @throws RuntimeException if an assertion fails or invalid swerve module location is given.
+ * @param moduleNumber Module number for kinematics.
+ * @param moduleConfiguration Module constants containing CAN ID's and offsets.
*/
- public SwerveModule(DriveMotorType mainMotor, AngleMotorType angleMotor, AbsoluteEncoderType encoder,
- SwerveModuleLocation swervePosition, double driveGearRatio, double steerGearRatio,
- double steeringOffsetDegrees, double wheelDiameterMeters, double wheelBaseMeters,
- double driveTrainWidthMeters, double steeringMotorFreeSpeedRPM, double maxSpeedMPS,
- double maxDriveAcceleration, double drivingPowerLimit, double steeringPowerLimit,
- boolean steeringInverted, boolean drivingInverted)
+ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration)
{
- // Steps to configure swerve drive are as follows
- // 1. Set Current limit of turning motor to 20 amps
- // 2. Enable voltage compensation with optimal battery voltage
- // 3. Configure CANCoder
- // 4. Set inverted motors.
- // 5. Configure status frames
- // 6. Set all motors to brake mode.
- // 7. Set velocity and position conversion factors on drive motor encoder.
- // 8. Set PIDF with integral zone on drive motor controller PID.
- // 9. Set velocity and position conversion factors on turning motor controller. (Usually P=0.01,I=0,D=0,F=0,IZ=1)
- // 10. Set PIDF with integral zone on turning motor controller.
- // 11. Check all CAN devices are active.
- // 12. Reset the angle on the internal encoder to the absolute encoder.
- requireNonNull(mainMotor);
- requireNonNull(angleMotor);
- requireNonNull(encoder);
+ angle = 0;
+ speed = 0;
+ omega = 0;
+ fakePos = 0;
+ this.moduleNumber = moduleNumber;
+ configuration = moduleConfiguration;
+ angleOffset = moduleConfiguration.angleOffset;
- this.wheelBase = wheelBaseMeters;
- this.driveTrainWidth = driveTrainWidthMeters;
+ // Initialize Feedforward for drive motor.
+ feedforward = configuration.createDriveFeedforward();
- assert mainMotor instanceof CANSparkMax || mainMotor instanceof WPI_TalonFX;
- assert angleMotor instanceof CANSparkMax || angleMotor instanceof WPI_TalonFX;
- assert encoder instanceof DutyCycleEncoder || encoder instanceof AnalogEncoder || encoder instanceof CANCoder ||
- encoder instanceof AbsoluteEncoder;
+ // Create motors from configuration and reset them to defaults.
+ angleMotor = moduleConfiguration.angleMotor;
+ driveMotor = moduleConfiguration.driveMotor;
+ angleMotor.factoryDefaults();
+ driveMotor.factoryDefaults();
- absoluteEncoder = SwerveEncoder.fromEncoder(encoder);
- assert absoluteEncoder != null;
+ // Configure voltage comp, current limit, and ramp rate.
+ angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage);
+ driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage);
+ angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit);
+ driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit);
+ angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate);
+ driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate);
+ // Config angle encoders
+ absoluteEncoder = moduleConfiguration.absoluteEncoder;
absoluteEncoder.factoryDefault();
+ absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
+ angleMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(false));
+ angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset);
- driveMotor = SwerveMotor.fromMotor(mainMotor,
- absoluteEncoder,
- ModuleMotorType.DRIVE,
- driveGearRatio,
- wheelDiameterMeters,
- 0,
- drivingPowerLimit);
+ // Config angle motor/controller
+ angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
+ angleMotor.configurePIDWrapping(-180, 180);
+ angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
+ angleMotor.setMotorBrake(false);
- turningMotor = SwerveMotor.fromMotor(angleMotor,
- absoluteEncoder,
- ModuleMotorType.TURNING,
- steerGearRatio,
- wheelDiameterMeters,
- steeringMotorFreeSpeedRPM,
- steeringPowerLimit);
+ // Config drive motor/controller
+ driveMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(true));
+ driveMotor.configurePIDF(moduleConfiguration.velocityPIDF);
+ driveMotor.setInverted(moduleConfiguration.driveMotorInverted);
+ driveMotor.setMotorBrake(true);
- swerveLocation = swervePosition;
- swerveModuleLocation = getSwerveModulePosition(swervePosition);
+ driveMotor.burnFlash();
+ angleMotor.burnFlash();
- assert driveMotor != null;
- assert turningMotor != null;
+ lastAngle = getState().angle.getDegrees();
- // Set the maximum speed for each swerve module for use when trying to optimize movements.
- // Drive feedforward gains
- // public static final double KS = 0;
- // public static final double KV = 12 / MAX_SPEED; // Volt-seconds per meter (max voltage divided by max
- // speed)
- // public static final double KA = 12 / MAX_ACCELERATION; // Volt-seconds^2 per meter (max voltage divided
- // by max accel)
- maxDriveSpeedMPS = maxSpeedMPS;
- driveFeedforward = new SimpleMotorFeedforward(0, 12 / maxDriveSpeedMPS, 12 / maxDriveAcceleration);
- steeringKV = (12 * 60) / (steeringMotorFreeSpeedRPM * Math.toRadians(360 / steerGearRatio));
-
- driveMotor.setInverted(drivingInverted);
- invertedDrive = drivingInverted;
- turningMotor.setInverted(steeringInverted);
- invertedTurn = steeringInverted;
-
- absoluteEncoder.configure();
-
- setAngleOffset(steeringOffsetDegrees);
- resetEncoders();
-
- // Convert CANCoder to read data as unsigned 0 to 360 for synchronization purposes.
- absoluteEncoder.configure();
-
- assert activeCAN();
-
- resetEncoders();
- synchronizeSteeringEncoder();
-
- driveMotor.saveConfig();
- turningMotor.saveConfig();
-
- // targetAngle = getState().angle.getDegrees();
- targetAngle = 0;
-
- if (!remoteIntegratedEncoder() && Robot.isReal())
+ if (!Robot.isReal())
{
- Robot.getInstance().addPeriodic(this::synchronizeSteeringEncoder, 0.5);
+ time = new Timer();
+ time.start();
+ lastTime = time.get();
}
-
- // Shuffleboard Data
- moduleTab = Shuffleboard.getTab(SwerveModule.SwerveModuleLocationToString(swerveLocation));
- publish(Verbosity.SETUP);
-
}
/**
- * Convert {@link SwerveModuleLocation} to {@link String} representation.
+ * Synchronize the integrated angle encoder with the absolute encoder.
+ */
+ public void synchronizeEncoders()
+ {
+ angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset);
+ }
+
+ /**
+ * Set the desired state of the swerve module.
*
- * @param swerveLocation Swerve position to convert.
- * @return {@link String} name of the {@link SwerveModuleLocation} enum.
+ * @param desiredState Desired swerve module state.
+ * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
*/
- public static String SwerveModuleLocationToString(SwerveModuleLocation swerveLocation)
+ public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop)
{
- switch (swerveLocation)
+ SwerveModuleState simpleState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
+ simpleState = SwerveModuleState.optimize(simpleState, getState().angle);
+ desiredState = new SwerveModuleState2(simpleState.speedMetersPerSecond, simpleState.angle,
+ desiredState.omegaRadPerSecond);
+
+ SmartDashboard.putNumber("Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
+ SmartDashboard.putNumber("Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
+ SmartDashboard.putNumber("Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
+
+ if (isOpenLoop)
{
- case FrontLeft:
- return "Front Left Module";
- case BackLeft:
- return "Back Left Module";
- case FrontRight:
- return "Front Right Module";
- case BackRight:
- return "Back Right Module";
- default:
- return "Unknown";
- }
- }
-
- /**
- * Reset the REV encoders onboard the SparkMax's to 0, and set's the drive motor to position to 0 and synchronizes the
- * internal steering encoders with the absolute encoder.
- */
- public void resetEncoders()
- {
- driveMotor.setEncoder(0);
-
- if (!remoteIntegratedEncoder())
- {
- turningMotor.setEncoder(0);
- synchronizeSteeringEncoder();
- }
- }
-
- /**
- * Synchronizes the internal encoder for the steering motor with the value from the absolute encoder.
- */
- public void synchronizeSteeringEncoder()
- {
- if (!remoteIntegratedEncoder())
- {
- if (absoluteEncoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED)
- {
- System.err.println("CANCoder magnetic field strength is unacceptable, will not synchronize encoders.");
- return;
- }
- turningMotor.setEncoder(absoluteEncoder.getAbsolutePosition() - angleOffset);
- }
- }
-
- /////////////////////// END OF CONFIGURATION FUNCTIONS SECTION //////////////////////////
-
- ////////////////////////////// STATUS FUNCTIONS SECTION //////////////////////////////////////////////////////
-
- /**
- * Set the PIDF coefficients for the closed loop PID onboard the motor controller. Tuning the PID
- *
- * P = .5 and increase it by .1 until oscillations occur, then decrease by .05 then .005 until oscillations
- * stop and angle is perfect or near perfect.
- *
- *
- * I = 0 tune this if your PID never quite reaches the target, after tuning D. Increase this by
- * P*.01 each time and adjust accordingly.
- *
- *
- * D = 0 tune this if the PID accelerates too fast, it will smooth the motion. Increase this by P*10
- * and adjust accordingly.
- *
- *
- * F = 0 tune this if the PID is being used for velocity, the F is multiplied by the target and added
- * to the voltage output. Increase this by 0.01 until the PIDF reaches the desired state in a fast enough manner.
- *
- * Documentation for this is best described by CTRE here.
- *
- * @param p Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
- * @param i Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
- * PID Loop.
- * @param d Derivative gain for closed loop. This is multiplied by derivative error (sensor units per
- * PID loop).
- * @param f Feed Fwd gain for Closed loop.
- * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too
- * far from the target. This prevents unstable oscillation if the kI is too large. Value is in
- * sensor units.
- * @param moduleMotorType Swerve drive motor type.
- */
- public void setPIDF(double p, double i, double d, double f, double integralZone,
- ModuleMotorType moduleMotorType)
- {
- if ((moduleMotorType == ModuleMotorType.DRIVE))
- {
- driveMotor.setPIDF(p, i, d, f, integralZone);
+ double percentOutput = desiredState.speedMetersPerSecond / configuration.maxSpeed;
+ driveMotor.set(percentOutput);
} else
{
- turningMotor.setPIDF(p, i, d, f, integralZone);
+ double velocity = desiredState.speedMetersPerSecond;
+ driveMotor.setReference(velocity, feedforward.calculate(velocity));
}
- }
- //////////////////////////// END OF STATUS FUNCTIONS SECTION ////////////////////////////////////////////////
+ // Prevents module rotation if speed is less than 1%
+ double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ?
+ lastAngle :
+ desiredState.angle.getDegrees());
+ angleMotor.setReference(angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
+ lastAngle = angle;
- //////////////////////////// ODOMETRY AND STATE FUNCTIONS SECTION ///////////////////////////////////////////
+ if (!Robot.isReal())
+ {
+ dt = time.get() - lastTime;
+ fakePos += (speed * dt);
+ lastTime = time.get();
+ }
- /**
- * Configure the magnetic offset in the CANCoder.
- *
- * @param offset Magnetic offset in degrees.
- */
- public void setAngleOffset(double offset)
- {
- // System.out.println(offset);
- angleOffset = offset;
- absoluteEncoder.setOffset(0);
+ this.angle = desiredState.angle.getDegrees();
+ omega = desiredState.omegaRadPerSecond;
+ speed = desiredState.speedMetersPerSecond;
}
/**
- * Check that the link is good on the swerve module and CAN bus is able to retrieve data.
- *
- * @return true on all devices are accessible over CAN.
- */
- public boolean activeCAN()
- {
- boolean drive = driveMotor.reachable(), turn = turningMotor.reachable(), encoder = absoluteEncoder.reachable();
-
- return drive && turn && encoder;
- }
-
- /**
- * Checks if the absolute encoder is able to be read directly by the motor controller instead of using the
- * synchronizeEncoder() function periodically.
- *
- * @return If the absolute sensor in the steering motor is directly accessed by the motor controller.
- */
- public boolean remoteIntegratedEncoder()
- {
- return turningMotor.remoteIntegratedEncoder();
- }
-
- /**
- * Set the angle of the swerve module.
- *
- * @param angle Angle in degrees.
- * @param feedforward The feedforward for the PID.
- */
- public void setAngle(double angle, double feedforward)
- {
- turningMotor.setTarget(Math.round(angle) % 180, feedforward);
- }
-
- /**
- * Set the angle of the swerve module without feedforward.
+ * Set the angle for the module.
*
* @param angle Angle in degrees.
*/
- private void setAngle(double angle)
+ public void setAngle(double angle)
{
- turningMotor.setTarget(angle, 0);
+ lastAngle = this.angle;
+ this.angle = angle;
+ angleMotor.setReference(angle, 1 * configuration.angleKV);
}
/**
- * Set the drive motor velocity in MPS.
+ * Get the Swerve Module state.
*
- * @param velocity Velocity in MPS.
- */
- public void setVelocity(double velocity)
- {
- driveMotor.setTarget(velocity, driveFeedforward.calculate(velocity) / RobotController.getBatteryVoltage());
- }
-
- /**
- * Get the module state.
- *
- * @return SwerveModuleState with the encoder inputs.
- * @throws RuntimeException Exception if CANCoder doesnt exist
+ * @return Current SwerveModule state.
*/
public SwerveModuleState2 getState()
{
- double mps = driveMotor.get();
- double angularVelocityRPS;
- Rotation2d angle;
- angle = Rotation2d.fromDegrees(Math.round(
- Robot.isReal() ? absoluteEncoder.getAbsolutePosition() - angleOffset : targetAngle));
- angularVelocityRPS = Robot.isReal() ? Math.toRadians(absoluteEncoder.getVelocity()) : targetAngularVelocityRPS;
- //^ Convert degrees per second to radians per second.
- return new SwerveModuleState2(mps, angle, angularVelocityRPS);
- }
-
- /////////////////// END OF ODOMETRY AND STATE FUNCTIONS SECTION ////////////////////////////////////////
-
- /////////////////// DIAGNOSTIC AND TUNING FUNCTIONS SECTION ////////////////////////////////////////////
-
- /**
- * Set the module speed and angle based off the module state.
- *
- * @param state Module state.
- */
- public void setState(SwerveModuleState2 state)
- {
- // state.angle = state.angle.minus(Rotation2d.fromDegrees(angleOffset));
- // inspired by https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L22
- Rotation2d currentAngle = getState().angle;
-// SwerveModuleState optimizedState = SwerveModuleState.optimize(new SwerveModuleState(state.speedMetersPerSecond,
-// state.angle), currentAngle);
- SwerveModuleState2 optimizedState = SwerveModuleState2.optimize(state, currentAngle);
- double angle = optimizedState.angle.getDegrees(); // getDegrees returns in the range of -180
- // to 180 we want 0 to
- // 360.
- double velocity = optimizedState.speedMetersPerSecond;
- // turn motor code
- // Prevent rotating module if speed is less then 1%. Prevents Jittering.
- angle = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? targetAngle : angle;
- setAngle(angle, optimizedState.angularVelocityRadPerSecond * steeringKV);
- setVelocity(velocity);
- targetAngle = angle;
- targetVelocity = velocity;
- targetAngularVelocityRPS = optimizedState.angularVelocityRadPerSecond;
- }
-
- /**
- * Get the swerve module position based off the sensors.
- *
- * @return Swerve Module position, with the angle as what the angle is supposed to be, not what it actually is.
- */
- public SwerveModulePosition getPosition()
- {
- return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(targetAngle));
- }
-
- //////////////////////////// END OF DIAGNOSTIC AND TUNING FUNCTIONS SECTION /////////////////////////
-
- /**
- * Subscribe from data within smart dashboard to make changes to the swerve module.
- */
- public void subscribe()
- {
-
- }
-
- //////////////////////////// ENUMS SECTION //////////////////////////////////////////////////////////
-
- /**
- * Create a widget and add the entry to the hashmap of entries for network tables.
- *
- * @param name Key to display on shuffleboard.
- * @param defaultValue Default value.
- * @return Widget that can be modified.
- */
- private SimpleWidget addEntry(String name, Object defaultValue)
- {
- if (NT4Entries.containsKey(name))
+ double velocity;
+ Rotation2d azimuth;
+ double omega;
+ if (Robot.isReal())
{
- GenericEntry entry = NT4Entries.get(name).getEntry();
- if (defaultValue instanceof Integer)
- {
- entry.setInteger((int) defaultValue);
- } else if (defaultValue instanceof Double)
- {
- entry.setDouble((double) defaultValue);
- } else if (defaultValue instanceof Boolean)
- {
- entry.setBoolean((boolean) defaultValue);
- } else if (defaultValue instanceof Double[])
- {
- entry.setDoubleArray((Double[]) defaultValue);
- } else
- {
- throw new RuntimeException("Cannot publish value for " + name);
- }
- return NT4Entries.get(name);
- }
- try
- {
- SimpleWidget widget = moduleTab.add(name, defaultValue);
- NT4Entries.put(name, widget);
- return widget;
- } catch (Exception e)
- {
- System.err.println(e);
- }
- throw new RuntimeException("Error creating entry");
- }
-
- /**
- * Publish data to the smart dashboard relating to this swerve module.
- *
- * @param level Verbosity level, affects the CAN utilization, on HIGH it will enable the update button.
- */
- public void publish(Verbosity level)
- {
- boolean goodLaptop = false;
- switch (level)
- {
- case SETUP:
- // PID
- addEntry("drive/pid/kP", driveMotor.kP);
- addEntry("drive/pid/kI", driveMotor.kI);
- addEntry("drive/pid/kD", driveMotor.kD);
- addEntry("drive/pid/kF", driveMotor.kF);
- addEntry("drive/pid/kIZ", driveMotor.kIZ);
-
- addEntry("steer/pid/kP", turningMotor.kP);
- addEntry("steer/pid/kI", turningMotor.kI);
- addEntry("steer/pid/kD", turningMotor.kD);
- addEntry("steer/pid/kF", turningMotor.kF);
- addEntry("steer/pid/kIZ", turningMotor.kIZ);
-
- // Pretty Widget Setup
- if (goodLaptop)
- {
- addEntry("Angle (Degrees)",
- new Double[]{targetAngle, turningMotor.get(), absoluteEncoder.getAbsolutePosition()})
- .withPosition(0, 1)
- .withSize(4, 4)
- .withProperties(Map.of("visible time", 5))
- .withWidget(BuiltInWidgets.kGraph);
- addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition())
- .withPosition(0, 0)
- .withSize(2, 1);
- addEntry("Integrated Angle", turningMotor.get())
- .withPosition(2, 0)
- .withSize(2, 1);
- addEntry("Target Angle", targetAngle)
- .withPosition(8, 4)
- .withSize(2, 2);
-
- addEntry("Velocity (Meters Per Second)", new Double[]{targetVelocity, driveMotor.get()})
- .withPosition(4, 1)
- .withSize(4, 4)
- .withProperties(Map.of("visible time", 5))
- .withWidget(BuiltInWidgets.kGraph);
- addEntry("Motor Velocity", driveMotor.get())
- .withPosition(4, 0)
- .withSize(4, 1);
- addEntry("Target Velocity", targetVelocity)
- .withPosition(8, 2)
- .withSize(2, 2);
-
- // Inverted Motors.
- addEntry("Steer Motor Inverted", invertedTurn)
- .withPosition(0, 5)
- .withSize(4, 1);
- addEntry("Drive Motor inverted", invertedDrive)
- .withPosition(4, 5)
- .withSize(4, 1);
-
- addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value)
- .withPosition(10, 2)
- .withSize(2, 1);
- addEntry("Drive Motor Current", driveMotor.getAmps())
- .withPosition(10, 5)
- .withSize(2, 1);
-
- // Angle Constants
- addEntry("Absolute Encoder Offset", angleOffset)
- .withProperties(Map.of("min", -180, "max", 180))
- .withPosition(10, 3)
- .withSize(2, 2)
- .withWidget(BuiltInWidgets.kDial);
- } else
- {
- addEntry("Target Angle", targetAngle)
- .withProperties(Map.of("min", -180, "max", 180))
- .withWidget(BuiltInWidgets.kDial)
- .withPosition(0, 0)
- .withSize(4, 2);
- addEntry("Integrated Angle", turningMotor.get())
- .withProperties(Map.of("min", 0, "max", 360))
- .withWidget(BuiltInWidgets.kDial)
- .withPosition(0, 2)
- .withSize(2, 2);
- addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition())
- .withProperties(Map.of("min", 0, "max", 360))
- .withWidget(BuiltInWidgets.kDial)
- .withPosition(2, 2)
- .withSize(2, 2);
-
- addEntry("Target Velocity", targetVelocity)
- .withPosition(4, 0)
- .withProperties(Map.of("min", -maxDriveSpeedMPS, "max", maxDriveSpeedMPS))
- .withSize(4, 2)
- .withWidget(BuiltInWidgets.kNumberBar);
- addEntry("Motor Velocity", driveMotor.get())
- .withPosition(4, 2)
- .withProperties(Map.of("min", -maxDriveSpeedMPS, "max", maxDriveSpeedMPS))
- .withSize(4, 2)
- .withWidget(BuiltInWidgets.kNumberBar);
-
- // Inverted Motors.
- addEntry("Steer Motor Inverted", invertedTurn)
- .withPosition(0, 4)
- .withSize(4, 1);
- addEntry("Drive Motor inverted", invertedDrive)
- .withPosition(4, 4)
- .withSize(4, 1);
-
- addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value)
- .withPosition(8, 2)
- .withSize(2, 1);
-
- addEntry("Drive Motor Current", driveMotor.getAmps())
- .withPosition(10, 2)
- .withSize(2, 1);
-
- // Angle Constants
- addEntry("Absolute Encoder Offset", angleOffset)
- .withProperties(Map.of("min", -180, "max", 180))
- .withPosition(8, 3)
- .withSize(2, 2)
- .withWidget(BuiltInWidgets.kDial);
- }
-
- addEntry("CAN Connection", activeCAN())
- .withPosition(8, 0)
- .withSize(4, 2);
-
- case HIGH:
- // Update if button is set.
- // The higher the better, 2 and 3 are what we want.
- addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value);
- case NORMAL:
- // Steering Encoder Values
- if (goodLaptop)
- {
- addEntry("Angle (Degrees)",
- new Double[]{targetAngle, turningMotor.get(), absoluteEncoder.getAbsolutePosition()});
- }
- addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition());
- addEntry("Integrated Angle", turningMotor.get());
-
- // Driving Encoder Values
- if (goodLaptop)
- {
- addEntry("Velocity (Meters Per Second)", new Double[]{targetVelocity, driveMotor.get()});
- }
- addEntry("Motor Velocity", driveMotor.get());
- addEntry("Drive Motor Current", driveMotor.getAmps());
-
- // CAN Bus is accessible
- addEntry("CAN Connection", activeCAN());
-
- case LOW:
- // PID
- addEntry("Target Velocity", targetVelocity);
- addEntry("Target Angle", targetAngle);
-
- }
- }
-
- /**
- * Initializes this {@link Sendable} object.
- *
- * @param builder sendable builder
- */
- @Override
- public void initSendable(SendableBuilder builder)
- {
- builder.setSmartDashboardType(SwerveModuleLocationToString(swerveLocation) + " SwerveDriveModule");
- builder.setActuator(true);
- builder.setSafeState(this::stopMotor);
- }
-
- //////////////////////////////////// END OF ENUMS SECTION //////////////////////////////////////////////
-
- ////////////////////////////////// OVERRIDES SECTION ///////////////////////////////////////////////////
-
- /**
- * Closes this resource, relinquishing any underlying resources. This method is invoked automatically on objects
- * managed by the {@code try}-with-resources statement.
- *
- * While this interface method is declared to throw {@code
- * Exception}, implementers are strongly encouraged to declare concrete implementations of the {@code close}
- * method to throw more specific exceptions, or to throw no exception at all if the close operation cannot fail.
- *
- *
Cases where the close operation may fail require careful
- * attention by implementers. It is strongly advised to relinquish the underlying resources and to internally
- * mark the resource as closed, prior to throwing the exception. The {@code close} method is unlikely to be
- * invoked more than once and so this ensures that the resources are released in a timely manner. Furthermore it
- * reduces problems that could arise when the resource wraps, or is wrapped, by another resource.
- *
- *
Implementers of this interface are also strongly advised
- * to not have the {@code close} method throw {@link InterruptedException}.
- *
- * This exception interacts with a thread's interrupted status, and runtime misbehavior is likely to occur if an
- * {@code InterruptedException} is {@linkplain Throwable#addSuppressed suppressed}.
- *
- * More generally, if it would cause problems for an exception to be suppressed, the {@code AutoCloseable.close}
- * method should not throw it.
- *
- *
Note that unlike the {@link Closeable#close close}
- * method of {@link Closeable}, this {@code close} method is not required to be idempotent. In other words,
- * calling this {@code close} method more than once may have some visible side effect, unlike {@code Closeable.close}
- * which is required to have no effect if called more than once.
- *
- * However, implementers of this interface are strongly encouraged to make their {@code close} methods idempotent.
- */
- @Override
- public void close()
- {
- SendableRegistry.remove(this);
- }
-
- /**
- * Disable the motor controller.
- */
- @Override
- public void disable()
- {
- stopMotor();
- }
-
- /**
- * Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
- */
- @Override
- public void stopMotor()
- {
-// turningMotor.stop();
- driveMotor.stop();
- }
-
- /**
- * Common interface for setting the speed of a motor controller.
- *
- * @param speed The speed to set. Value should be between -1.0 and 1.0.
- */
- @Override
- public void set(double speed)
- {
- drivePower = speed;
- driveMotor.set(speed);
- }
-
- /**
- * Common interface for getting the current set speed of a motor controller.
- *
- * @return The current set speed. Value is between -1.0 and 1.0.
- */
- @Override
- public double get()
- {
- return drivePower;
- }
-
- /**
- * Get the swerve module position in {@link Translation2d} from the enum passed.
- *
- * @param swerveLocation Swerve module location enum.
- * @return Location as {@link Translation2d}.
- * @throws RuntimeException If Enum value is not defined.
- */
- private Translation2d getSwerveModulePosition(SwerveModuleLocation swerveLocation)
- {
- // Modeling off of https://github.com/Stampede3630/2022-Code/blob/master/src/main/java/frc/robot/SwerveDrive.java
- switch (swerveLocation)
- {
- case FrontLeft:
- return new Translation2d(wheelBase / 2, driveTrainWidth / 2);
- case BackLeft:
- return new Translation2d(-wheelBase / 2, driveTrainWidth / 2);
- case FrontRight:
- return new Translation2d(wheelBase / 2, -driveTrainWidth / 2);
- case BackRight:
- return new Translation2d(-wheelBase / 2, -driveTrainWidth / 2);
- default:
- throw new RuntimeException("Invalid location given");
- }
- }
-
- /**
- * Set the voltage compensation for the swerve module motor.
- *
- * @param nominalVoltage Nominal voltage for operation to output to.
- */
- public void setVoltageCompensation(double nominalVoltage)
- {
- driveMotor.setVoltageCompensation(nominalVoltage);
- turningMotor.setVoltageCompensation(nominalVoltage);
- }
-
- //////////////////////////////////// END OF OVERRIDES SECTION ////////////////////////////////////////////
-
- //////////////////////////////////// STATIC FUNCTIONS SECTION ////////////////////////////////////////////
-
- /**
- * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
- * voltage compensation. This is useful to protect the motor from current spikes.
- *
- * @param currentLimit Current limit in AMPS at free speed.
- * @param type Swerve Drive Motor type to configure.
- */
- public void setCurrentLimit(int currentLimit, ModuleMotorType type)
- {
- if (type == ModuleMotorType.DRIVE)
- {
- driveMotor.setCurrentLimit(currentLimit);
+ velocity = driveMotor.getVelocity();
+ azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
+ omega = angleMotor.getVelocity();
} else
{
- turningMotor.setCurrentLimit(currentLimit);
+ velocity = speed;
+ azimuth = Rotation2d.fromDegrees(this.angle);
+ omega = this.omega;
}
+ return new SwerveModuleState2(velocity, azimuth, omega);
}
- /////////////////////////////////// END OF STATIC FUNCTIONS SECTION //////////////////////////////////////////
-
- /////////////////////////////////// EXTRA FUNCTIONS THAT PROBABLY WONT MATTER ////////////////////////////////
+ public SwerveModulePosition getPosition()
+ {
+ double position;
+ Rotation2d azimuth;
+ if (Robot.isReal())
+ {
+ position = driveMotor.getPosition();
+ azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
+ } else
+ {
+ position = fakePos;
+ azimuth = Rotation2d.fromDegrees(angle + (Math.toDegrees(omega) * dt));
+ }
+ SmartDashboard.putNumber("Module " + moduleNumber + "Angle", azimuth.getDegrees());
+ return new SwerveModulePosition(position, azimuth);
+ }
/**
- * Set the steering motor to be inverted.
+ * Get the CANCoder absolute position.
*
- * @param isInverted The state of inversion, true is inverted.
+ * @return Absolute encoder angle in degrees.
*/
- public void setInvertedTurn(boolean isInverted)
+ public double getCANCoder()
{
- invertedTurn = isInverted;
- turningMotor.setInverted(isInverted);
+ return absoluteEncoder.getAbsolutePosition();
}
/**
- * Invert the absolute encoder.
+ * Get the relative encoder angle in degrees.
*
- * @param isInverted The state of inversion, true is inverted.
+ * @return Angle in degrees.
*/
- public void setInvertedAbsoluteEncoder(boolean isInverted)
+ public double getRelativeEncoder()
{
- absoluteEncoder.setInverted(isInverted);
+ return angleMotor.getPosition();
}
- ////////////// CUSTOM INVERSION FUNCTIONS SECTION //////////////////////////
-
/**
- * Common interface for returning if a motor controller is in the inverted state or not.
+ * Set the brake mode.
*
- * @return isInverted The state of the inversion true is inverted.
+ * @param brake Set the brake mode.
*/
- @Override
- public boolean getInverted()
+ public void setMotorBrake(boolean brake)
{
- return invertedDrive;
+ driveMotor.setMotorBrake(brake);
}
-
- /**
- * Common interface for inverting direction of a motor controller.
- *
- * @param isInverted The state of inversion, true is inverted.
- */
- @Override
- public void setInverted(boolean isInverted)
- {
- invertedDrive = isInverted;
- driveMotor.setInverted(isInverted);
- }
-
- /**
- * Swerve Module location on the robot.
- */
- public enum SwerveModuleLocation
- {
- FrontLeft,
- BackLeft,
- FrontRight,
- BackRight
- }
-
- /**
- * Verbosity levels for data publishing,
- */
- public enum Verbosity
- {
- /**
- * The bare minimum and not utilize the CAN bus when reporting data. Only posts data from attributes.
- */
- LOW,
- /**
- * Utilize the CAN bus minimally.
- */
- NORMAL,
- /**
- * Extensively use the CAN bus to fetch data and report back.
- */
- HIGH,
- /**
- * Creates every field for the module.
- */
- SETUP
- }
-
- ////////////////////////////// END OF CUSTOM INVERSION FUNCTIONS SECTION /////////////////////////////////////
-
-}
\ No newline at end of file
+}
diff --git a/swervelib/SwerveMotor.java b/swervelib/SwerveMotor.java
deleted file mode 100644
index f462e3a..0000000
--- a/swervelib/SwerveMotor.java
+++ /dev/null
@@ -1,211 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve;
-
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.revrobotics.CANSparkMax;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import frc.robot.subsystems.swervedrive.swerve.motors.CTRESwerveMotor;
-import frc.robot.subsystems.swervedrive.swerve.motors.REVSwerveMotor;
-
-public abstract class SwerveMotor
-{
-
- /**
- * Module motor type.
- */
- public ModuleMotorType m_motorType;
- /**
- * PIDF Values.
- */
- public double kP, kI, kD, kF, kIZ;
- /**
- * Target value for PID.
- */
- public double target;
-
- /**
- * Constructor for Swerve Motor, expecting CANSparkMax or TalonFX. Clears sticky faults and restores factory
- * defaults.
- *
- * @param motor Motor controller.
- * @param absoluteEncoder The absolute encoder used for the module, if the motor is a turning motor and the encoder is
- * compatible it will set the encoder as the remote integrated encoder and does not need
- * periodic synchronization.
- * @param type Swerve module motor type.
- * @param gearRatio Gear ratio.
- * @param wheelDiameter Wheel diameter in meters.
- * @param freeSpeedRPM Free speed RPM of the motor.
- * @param powerLimit Power limit for the motor.
- * @param Motor type to use.
- */
- public static SwerveMotor fromMotor(MotorType motor,
- SwerveEncoder absoluteEncoder,
- ModuleMotorType type,
- double gearRatio,
- double wheelDiameter,
- double freeSpeedRPM, double powerLimit)
- {
- if (motor instanceof CANSparkMax)
- {
- return new REVSwerveMotor(((CANSparkMax) motor),
- absoluteEncoder,
- type,
- gearRatio,
- wheelDiameter,
- freeSpeedRPM,
- powerLimit);
- }
- if (motor instanceof WPI_TalonFX)
- {
- return new CTRESwerveMotor((WPI_TalonFX) motor,
- absoluteEncoder,
- type,
- gearRatio,
- wheelDiameter,
- freeSpeedRPM,
- powerLimit);
- }
- return null;
- }
-
-
- /**
- * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base.
- *
- * @param min Minimum output.
- * @param max Maximum output.
- */
- public abstract void setPIDOutputRange(double min, double max);
-
- /**
- * Set the PIDF coefficients for the closed loop PID onboard the SparkMax.
- *
- * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
- * Default is 1.0
- * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
- * PID Loop.
- * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID
- * loop). Default is 0.1
- * @param F Feed Fwd gain for Closed loop.
- * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far
- * from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor
- * units.
- */
- public abstract void setPIDF(double P, double I, double D, double F, double integralZone);
-
- /**
- * Configures the conversion factor based upon which motor.
- *
- * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning
- * motor.
- */
- public abstract void setConversionFactor(double conversionFactor);
-
- /**
- * Set the target for the PID loop.
- *
- * @param target The PID target to aim for.
- * @param feedforward The feedforward for this target.
- */
- public abstract void setTarget(double target, double feedforward);
-
- /**
- * Stop the motor by sending 0 volts to it.
- */
- public abstract void stop();
-
- /**
- * Set the speed of the drive motor from -1 to 1.
- *
- * @param speed Speed from -1 to 1.
- */
- public abstract void set(double speed);
-
- /**
- * Set the voltage of the motor.
- *
- * @param voltage Voltage to output.
- */
- public abstract void setVoltage(double voltage);
-
- /**
- * Get the current value of the encoder corresponding to the PID.
- *
- * @return Current value of the encoder either in velocity or position..
- */
- public abstract double get();
-
- /**
- * Get the current output.
- *
- * @return Output amps.
- */
- public abstract double getAmps();
-
- /**
- * Get the current value of the encoder.
- *
- * @return Current value of the encoder.
- */
- public abstract double getPosition();
-
- /**
- * Set the voltage compensation for the swerve module motor.
- *
- * @param nominalVoltage Nominal voltage for operation to output to.
- */
- public abstract void setVoltageCompensation(double nominalVoltage);
-
- /**
- * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
- * voltage compensation. This is useful to protect the motor from current spikes.
- *
- * @param currentLimit Current limit in AMPS at free speed.
- */
- public abstract void setCurrentLimit(int currentLimit);
-
- /**
- * Set the encoder value.
- *
- * @param value Value to set the encoder to.
- */
- public abstract void setEncoder(double value);
-
- /**
- * Check that the link is good on the swerve module and CAN bus is able to retrieve data.
- *
- * @return true on all devices are accessible over CAN.
- */
- public abstract boolean reachable();
-
- /**
- * Check if the absolute encoder is used inplace of the integrated encoder.
- *
- * @return true, if the absolute encoder is being used as the integrated controller.
- */
- public abstract boolean remoteIntegratedEncoder();
-
- /**
- * Optimize the CAN status frames to reduce utilization.
- */
- public abstract void optimizeCANFrames();
-
- /**
- * Save configuration data to the motor controller so it is persistent on reboot.
- */
- public abstract void saveConfig();
-
- /**
- * Invert the motor.
- *
- * @param inverted Set the motor as inverted.
- */
- public abstract void setInverted(boolean inverted);
-
- /**
- * Motor type for the swerve drive module
- */
- public enum ModuleMotorType
- {
- DRIVE, TURNING
- }
-}
diff --git a/swervelib/SwerveParser.java b/swervelib/SwerveParser.java
deleted file mode 100644
index fc67ba6..0000000
--- a/swervelib/SwerveParser.java
+++ /dev/null
@@ -1,311 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve;
-
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.AnalogEncoder;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
-import frc.robot.subsystems.swervedrive.swerve.SwerveModule.SwerveModuleLocation;
-import java.io.File;
-import java.io.IOException;
-
-/**
- * Swerve Drive JSON parser.
- */
-public class SwerveParser
-{
-
- /**
- * Open JSON file.
- *
- * @param file JSON File to open.
- * @return JsonNode of file.
- */
- private static JsonNode openJson(File file)
- {
- try
- {
- return new ObjectMapper().readTree(file);
- } catch (IOException e)
- {
- throw new RuntimeException(e);
- }
- }
-
- /**
- * Check directory structure.
- *
- * @param directory JSON Configuration Directory
- */
- private static void checkDirectory(File directory)
- {
- assert new File(directory, "swerve.json").exists();
- assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory();
- assert new File(directory, "modules/back").exists() && new File(directory, "modules/back").isDirectory();
- assert new File(directory, "modules/front").exists() && new File(directory, "modules/front").isDirectory();
- assert new File(directory, "modules/back/left.json").exists();
- assert new File(directory, "modules/back/right.json").exists();
- assert new File(directory, "modules/front/left.json").exists();
- assert new File(directory, "modules/front/right.json").exists();
- }
-
- /**
- * Create the motor controller based off the config.
- *
- * @param motorConfig JSON Motor config object.
- * @return Motor Controller
- */
- private static MotorController createMotor(JsonNode motorConfig)
- {
- int motorID = motorConfig.get("ID").asInt();
- switch (motorConfig.get("Type").asText())
- {
- case "Neo":
- return new CANSparkMax(motorID, MotorType.kBrushless);
- case "Falcon":
- return motorConfig.has("CANBus") ? new WPI_TalonFX(motorID, motorConfig.get("CANBus").asText())
- : new WPI_TalonFX(motorID);
- }
- return null;
- }
-
- /**
- * Create SwerveEncoder from JSON config.
- *
- * @param encoderConfig Swerve Encoder JSON config.
- * @return SwerveEncoder
- */
- private static Object createEncoder(JsonNode encoderConfig)
- {
- int encoderID = encoderConfig.get("ID").asInt();
- switch (encoderConfig.get("Type").asText())
- {
- case "CANCoder":
- return encoderConfig.has("CANBus") ? new CANCoder(encoderID, encoderConfig.get("CANBus").asText())
- : new CANCoder(encoderID);
- case "ThriftyBot":
- case "AnalogEncoder":
- return new AnalogEncoder(encoderID);
- case "SRXMagEncoder":
- case "REVThroughBore":
- return new DutyCycleEncoder(encoderID); // This probably doesn't work
- }
- return null;
- }
-
- /**
- * Create the gyro object based off the configuration.
- *
- * @param gyroConfig Gyro configuration.
- * @return Gyro object.
- */
- private static WPI_Pigeon2 createGyro(JsonNode gyroConfig)
- {
- int gyroID = gyroConfig.get("ID").asInt();
-
- if (gyroConfig.get("Type").asText().equals("Pigeon2"))
- {
- return gyroConfig.has("CANBus") ? new WPI_Pigeon2(gyroID, gyroConfig.get("CANBus").asText())
- : new WPI_Pigeon2(gyroID);
-// case "Pigeon":
-// return new WPI_PigeonIMU(gyroID);
- }
- return null;
- }
-
- /**
- * Check module JSON config for existing values.
- *
- * @param moduleJson Module JSON object.
- */
- private static void checkModule(JsonNode moduleJson)
- {
- assert moduleJson.has("Motor");
- assert moduleJson.get("Motor").has("Drive");
- assert moduleJson.get("Motor").get("Drive").has("Type") && moduleJson.get("Motor").get("Drive").has("ID");
- assert moduleJson.get("Motor").has("Steer");
- assert moduleJson.get("Motor").get("Steer").has("Type") && moduleJson.get("Motor").get("Steer").has("ID") &&
- moduleJson.get("Motor").get("Steer").has("FreeSpeedRPM");
- assert moduleJson.has("AbsoluteEncoder");
- assert moduleJson.get("AbsoluteEncoder").has("Type") &&
- moduleJson.get("AbsoluteEncoder").has("ID") &&
- moduleJson.get("AbsoluteEncoder").has("Offset");
- }
-
- /**
- * Check the main json for correct attributes.
- *
- * @param mainJson Main JSON.
- */
- private static void checkMain(JsonNode mainJson)
- {
- assert mainJson.has("WheelBase");
- assert mainJson.has("TrackWidth");
- assert mainJson.has("WheelDiameter");
- assert mainJson.has("Speed");
- assert mainJson.get("Speed").has("MetersPerSecond") && mainJson.get("Speed").has("RadianPerSecond") &&
- mainJson.get("Speed").has("PhysicalMetersPerSecond");
- assert mainJson.has("Acceleration");
- assert mainJson.get("Acceleration").has("MetersPerSecond") && mainJson.get("Acceleration").has("RadianPerSecond");
- assert mainJson.has("Drive");
- assert mainJson.get("Drive").has("Inverted") && mainJson.get("Drive").has("GearRatio") &&
- mainJson.get("Drive").has("MaxPower");
- assert mainJson.has("Steer");
- assert mainJson.get("Steer").has("Inverted") && mainJson.get("Steer").has("GearRatio") &&
- mainJson.get("Steer").has("MaxPower");
- assert mainJson.has("Gyro");
- assert mainJson.get("Gyro").has("Inverted") && mainJson.get("Gyro").has("Type") && mainJson.get("Gyro").has("ID");
- assert mainJson.has("Initial Pose");
- assert mainJson.get("Initial Pose").has("X") && mainJson.get("Initial Pose").has("Y") &&
- mainJson.get("Initial Pose").has("Rotation");
- }
-
- /**
- * Create SwerveModule from JSON configuration file.
- *
- * @param mainJson Main JSON to pull from.
- * @param moduleFile Module specific JSON data to pull from.
- * @param moduleLocation Swerve module location that is being created.
- * @return SwerveModule
- */
- private static SwerveModule createModule(JsonNode mainJson, File moduleFile,
- SwerveModuleLocation moduleLocation)
- {
- JsonNode moduleJson = openJson(moduleFile);
- checkModule(moduleJson);
- MotorController driveMotor = createMotor(moduleJson.get("Motor").get("Drive")),
- steerMotor = createMotor(moduleJson.get("Motor").get("Steer"));
- Object encoder = createEncoder(moduleJson.get("AbsoluteEncoder"));
- if (encoder == null)
- {
- if (moduleJson.get("IntegratedAbsoluteEncoder").asBoolean() && (steerMotor instanceof CANSparkMax))
- {
- encoder = ((CANSparkMax) steerMotor).getAbsoluteEncoder(Type.kDutyCycle);
- }
- }
-
- SwerveModule module = new SwerveModule<>(driveMotor,
- steerMotor,
- encoder,
- moduleLocation,
- mainJson.get("Drive").get("GearRatio").asDouble(),
- mainJson.get("Steer").get("GearRatio").asDouble(),
- moduleJson.get("AbsoluteEncoder").get("Offset").asDouble(),
- Units.inchesToMeters(mainJson.get("WheelDiameter").asDouble()),
- Units.inchesToMeters(mainJson.get("WheelBase").asDouble()),
- Units.inchesToMeters(mainJson.get("TrackWidth").asDouble()),
- moduleJson.get("Motor").get("Steer").get("FreeSpeedRPM")
- .asDouble(),
- mainJson.get("Speed").get("MetersPerSecond").asDouble(),
- mainJson.get("Acceleration").get("MetersPerSecond").asDouble(),
- mainJson.get("Drive").get("MaxPower").asDouble(),
- mainJson.get("Steer").get("MaxPower").asDouble(),
- mainJson.get("Steer").get("Inverted").asBoolean(),
- mainJson.get("Drive").get("Inverted").asBoolean());
-
- if (moduleJson.get("Motor").get("Steer").has("PID"))
- {
- JsonNode jsonPIDF = moduleJson.get("Motor").get("Steer").get("PID");
- setModulePIDF(module.turningMotor, jsonPIDF);
- } else if (mainJson.get("Steer").has("PID"))
- {
- JsonNode jsonPIDF = mainJson.get("Steer").get("PID");
- setModulePIDF(module.turningMotor, jsonPIDF);
- }
-
- if (moduleJson.get("Motor").get("Drive").has("PID"))
- {
- JsonNode jsonPIDF = moduleJson.get("Motor").get("Drive").get("PID");
- setModulePIDF(module.driveMotor, jsonPIDF);
- } else if (mainJson.get("Steer").has("PID"))
- {
- JsonNode jsonPIDF = mainJson.get("Drive").get("PID");
- setModulePIDF(module.turningMotor, jsonPIDF);
- }
-
- if (moduleJson.get("Motor").get("Steer").has("CurrentLimit"))
- {
- module.turningMotor.setCurrentLimit(moduleJson.get("Motor").get("Steer").get("CurrentLimit")
- .asInt());
- } else if (mainJson.get("Steer").has("CurrentLimit"))
- {
- module.turningMotor.setCurrentLimit(mainJson.get("Steer").get("CurrentLimit").asInt());
- }
-
- if (moduleJson.get("Motor").get("Drive").has("CurrentLimit"))
- {
- module.driveMotor.setCurrentLimit(moduleJson.get("Motor").get("Drive").get("CurrentLimit")
- .asInt());
- } else if (mainJson.get("Drive").has("CurrentLimit"))
- {
- module.turningMotor.setCurrentLimit(mainJson.get("Drive").get("CurrentLimit").asInt());
- }
-
- return module;
- }
-
- /**
- * Set the PIDF for the module if the given JSON has the paramters.
- *
- * @param motor Motor to configure.
- * @param jsonPIDF JSON ndoe with values.
- */
- private static void setModulePIDF(SwerveMotor motor, JsonNode jsonPIDF)
- {
- if (jsonPIDF.has("P") && jsonPIDF.has("I") && jsonPIDF.has("D") &&
- jsonPIDF.has("F") && jsonPIDF.has("IntegralZone"))
- {
- motor.setPIDF(jsonPIDF.get("P").asDouble(),
- jsonPIDF.get("I").asDouble(),
- jsonPIDF.get("D").asDouble(),
- jsonPIDF.get("F").asDouble(),
- jsonPIDF.get("IntegralZone").asDouble());
- }
- }
-
- /**
- * Create SwerveDrive from JSON configuration directory.
- *
- * @param directory JSON Configuration directory.
- * @return SwerveDrive
- */
- public static SwerveDrive fromJSONDirectory(File directory)
- {
- checkDirectory(directory);
- JsonNode swerveJson = openJson(new File(directory, "swerve.json"));
- checkMain(swerveJson);
- return new SwerveDrive(createModule(swerveJson,
- new File(directory, "modules/front/left.json"),
- SwerveModuleLocation.FrontLeft),
- createModule(swerveJson,
- new File(directory, "modules/front/right.json"),
- SwerveModuleLocation.FrontRight),
- createModule(swerveJson,
- new File(directory, "modules/back/left.json"),
- SwerveModuleLocation.BackLeft),
- createModule(swerveJson,
- new File(directory, "modules/back/right.json"),
- SwerveModuleLocation.BackRight),
- createGyro(swerveJson.get("Gyro")),
- swerveJson.get("Speed").get("MetersPerSecond").asDouble(),
- swerveJson.get("Speed").get("RadianPerSecond").asDouble() * Math.PI,
- swerveJson.get("Acceleration").get("MetersPerSecond").asDouble(),
- swerveJson.get("Acceleration").get("RadianPerSecond").asDouble() * Math.PI,
- swerveJson.get("Speed").get("PhysicalMetersPerSecond").asDouble(),
- swerveJson.get("Gyro").get("Inverted").asBoolean(),
- new Pose2d(Units.inchesToMeters(swerveJson.get("Initial Pose").get("X").asDouble()),
- Units.inchesToMeters(swerveJson.get("Initial Pose").get("Y").asDouble()),
- Rotation2d.fromDegrees(swerveJson.get("Initial Pose").get("Rotation")
- .asDouble())));
-
- }
-}
diff --git a/swervelib/commands/CustomSwerveControllerCommand.java b/swervelib/commands/CustomSwerveControllerCommand.java
deleted file mode 100644
index 4261a95..0000000
--- a/swervelib/commands/CustomSwerveControllerCommand.java
+++ /dev/null
@@ -1,244 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.subsystems.swervedrive.swerve.commands;
-
-import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
-
-import edu.wpi.first.math.controller.HolonomicDriveController;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.Subsystem;
-import java.util.function.Consumer;
-import java.util.function.Supplier;
-
-/**
- * A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
- * ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a swerve drive.
- *
- * This command outputs the raw desired Swerve Module States ({@link SwerveModuleState}) in an
- * array. The desired wheel and module rotation velocities should be taken from those and used in velocity PIDs.
- *
- *
The robot angle controller does not follow the angle given by the trajectory but rather goes
- * to the angle given in the final state of the trajectory.
- *
- *
This class is provided by the NewCommands VendorDep
- */
-public class CustomSwerveControllerCommand extends CommandBase
-{
-
- private final Timer m_timer = new Timer();
- private final Trajectory m_trajectory;
- private final Supplier m_pose;
- private final SwerveDriveKinematics m_kinematics;
- private final HolonomicDriveController m_controller;
- private final Consumer m_outputModuleStates;
- private final Supplier m_desiredRotation;
-
- /**
- * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will
- * not return output voltages but rather raw module states from the position controllers which need to be put into a
- * velocity PID.
- *
- * Note: The controllers will *not* set the outputVolts to zero upon completion of the path.
- * This is left to the user to do since it is not appropriate for paths with nonstationary endstates.
- *
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of the odometry classes to provide
- * this.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param xController The Trajectory Tracker PID controller for the robot's x position.
- * @param yController The Trajectory Tracker PID controller for the robot's y position.
- * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
- * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each time step.
- * @param outputModuleStates The raw output module states from the position controllers.
- * @param requirements The subsystems to require.
- */
- public CustomSwerveControllerCommand(
- Trajectory trajectory,
- Supplier pose,
- SwerveDriveKinematics kinematics,
- PIDController xController,
- PIDController yController,
- ProfiledPIDController thetaController,
- Supplier desiredRotation,
- Consumer outputModuleStates,
- Subsystem... requirements)
- {
- this(
- trajectory,
- pose,
- kinematics,
- new HolonomicDriveController(
- requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
- requireNonNullParam(yController, "yController", "SwerveControllerCommand"),
- requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")),
- desiredRotation,
- outputModuleStates,
- requirements);
- }
-
- /**
- * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will
- * not return output voltages but rather raw module states from the position controllers which need to be put into a
- * velocity PID.
- *
- * Note: The controllers will *not* set the outputVolts to zero upon completion of the path.
- * This is left to the user since it is not appropriate for paths with nonstationary endstates.
- *
- *
Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
- * trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior
- * is desired, the other constructor with a supplier for rotation should be used.
- *
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of the odometry classes to provide
- * this.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param xController The Trajectory Tracker PID controller for the robot's x position.
- * @param yController The Trajectory Tracker PID controller for the robot's y position.
- * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
- * @param outputModuleStates The raw output module states from the position controllers.
- * @param requirements The subsystems to require.
- */
- public CustomSwerveControllerCommand(
- Trajectory trajectory,
- Supplier pose,
- SwerveDriveKinematics kinematics,
- PIDController xController,
- PIDController yController,
- ProfiledPIDController thetaController,
- Consumer outputModuleStates,
- Subsystem... requirements)
- {
- this(
- trajectory,
- pose,
- kinematics,
- xController,
- yController,
- thetaController,
- () ->
- trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
- outputModuleStates,
- requirements);
- }
-
- /**
- * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will
- * not return output voltages but rather raw module states from the position controllers which need to be put into a
- * velocity PID.
- *
- * Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
- * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
- *
- *
Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
- * trajectory. The robot will not follow the rotations from the poses at each timestep. If alternate rotation behavior
- * is desired, the other constructor with a supplier for rotation should be used.
- *
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of the odometry classes to provide
- * this.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param controller The HolonomicDriveController for the drivetrain.
- * @param outputModuleStates The raw output module states from the position controllers.
- * @param requirements The subsystems to require.
- */
- public CustomSwerveControllerCommand(
- Trajectory trajectory,
- Supplier pose,
- SwerveDriveKinematics kinematics,
- HolonomicDriveController controller,
- Consumer outputModuleStates,
- Subsystem... requirements)
- {
- this(
- trajectory,
- pose,
- kinematics,
- controller,
- () ->
- trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
- outputModuleStates,
- requirements);
- }
-
- /**
- * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will
- * not return output voltages but rather raw module states from the position controllers which need to be put into a
- * velocity PID.
- *
- * Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
- * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
- *
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of the odometry classes to provide
- * this.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param controller The HolonomicDriveController for the drivetrain.
- * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each time step.
- * @param outputModuleStates The raw output module states from the position controllers.
- * @param requirements The subsystems to require.
- */
- public CustomSwerveControllerCommand(
- Trajectory trajectory,
- Supplier pose,
- SwerveDriveKinematics kinematics,
- HolonomicDriveController controller,
- Supplier desiredRotation,
- Consumer outputModuleStates,
- Subsystem... requirements)
- {
- m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
- m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
- m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
- m_controller = requireNonNullParam(controller, "controller", "SwerveControllerCommand");
-
- m_desiredRotation =
- requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
-
- m_outputModuleStates =
- requireNonNullParam(outputModuleStates, "outputModuleStates", "SwerveControllerCommand");
-
- addRequirements(requirements);
- }
-
- @Override
- public void initialize()
- {
- m_timer.reset();
- m_timer.start();
- }
-
- @Override
- public void execute()
- {
- double curTime = m_timer.get();
- var desiredState = m_trajectory.sample(curTime);
-
- var targetChassisSpeeds =
- m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
-
- m_outputModuleStates.accept(targetChassisSpeeds);
- }
-
- @Override
- public void end(boolean interrupted)
- {
- m_timer.stop();
- }
-
- @Override
- public boolean isFinished()
- {
- return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
- }
-}
diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java
new file mode 100644
index 0000000..1e5e0f5
--- /dev/null
+++ b/swervelib/encoders/CANCoderSwerve.java
@@ -0,0 +1,96 @@
+package frc.robot.subsystems.swervedrive2.swervelib.encoders;
+
+import com.ctre.phoenix.sensors.AbsoluteSensorRange;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+import com.ctre.phoenix.sensors.SensorInitializationStrategy;
+import com.ctre.phoenix.sensors.SensorTimeBase;
+import com.ctre.phoenix.sensors.WPI_CANCoder;
+
+/**
+ * Swerve Absolute Encoder for CTRE CANCoders.
+ */
+public class CANCoderSwerve extends SwerveAbsoluteEncoder
+{
+
+ /**
+ * CANCoder with WPILib sendable and support.
+ */
+ public WPI_CANCoder encoder;
+
+ /**
+ * Initialize the CANCoder on the standard CANBus.
+ *
+ * @param id CAN ID.
+ */
+ public CANCoderSwerve(int id)
+ {
+ encoder = new WPI_CANCoder(id);
+ }
+
+ /**
+ * Initialize the CANCoder on the CANivore.
+ *
+ * @param id CAN ID.
+ * @param canbus CAN bus to initialize it on.
+ */
+ public CANCoderSwerve(int id, String canbus)
+ {
+ encoder = new WPI_CANCoder(id, canbus);
+ }
+
+ /**
+ * Reset the encoder to factory defaults.
+ */
+ @Override
+ public void factoryDefault()
+ {
+ encoder.configFactoryDefault();
+ }
+
+ /**
+ * Clear sticky faults on the encoder.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ encoder.clearStickyFaults();
+ }
+
+ /**
+ * Configure the absolute encoder to read from [0, 360) per second.
+ *
+ * @param inverted Whether the encoder is inverted.
+ */
+ @Override
+ public void configure(boolean inverted)
+ {
+ CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
+ canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
+ canCoderConfiguration.sensorDirection = inverted;
+ canCoderConfiguration.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition;
+ canCoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond;
+ encoder.configAllSettings(canCoderConfiguration);
+ }
+
+ /**
+ * Get the absolute position of the encoder.
+ *
+ * @return Absolute position in degrees from [0, 360).
+ */
+ @Override
+ public double getAbsolutePosition()
+ {
+ return encoder.getAbsolutePosition();
+ }
+
+ /**
+ * Get the instantiated absolute encoder Object.
+ *
+ * @return Absolute encoder object.
+ */
+ @Override
+ public Object getAbsoluteEncoder()
+ {
+ return encoder;
+ }
+}
diff --git a/swervelib/encoders/CTRECANCoder.java b/swervelib/encoders/CTRECANCoder.java
deleted file mode 100644
index 7f34fd6..0000000
--- a/swervelib/encoders/CTRECANCoder.java
+++ /dev/null
@@ -1,111 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.encoders;
-
-import com.ctre.phoenix.sensors.AbsoluteSensorRange;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;
-import com.ctre.phoenix.sensors.MagnetFieldStrength;
-import com.ctre.phoenix.sensors.SensorInitializationStrategy;
-import com.ctre.phoenix.sensors.SensorTimeBase;
-import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder;
-
-public class CTRECANCoder extends SwerveEncoder
-{
-
- /**
- * Create SwerveEncoder based off CANCoder.
- *
- * @param encoder CANCoder to use.
- */
- public CTRECANCoder(CANCoder encoder)
- {
- m_encoder = encoder;
- }
-
- /**
- * Configure the absolute encoder if possible.
- */
- public void configure()
- {
- CANCoderConfiguration sensorConfig = new CANCoderConfiguration();
-
- sensorConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
- sensorConfig.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition;
- sensorConfig.sensorTimeBase = SensorTimeBase.PerSecond;
- m_encoder.configAllSettings(sensorConfig);
- }
-
- /**
- * Get the magnetic field strength, if available.
- *
- * @return CTRE MagneticFieldStrength Enum.
- */
- @Override
- public MagnetFieldStrength getMagnetFieldStrength()
- {
- return m_encoder.getMagnetFieldStrength();
- }
-
- /**
- * Get the absolute position in degrees.
- *
- * @return Absolute position (0, 360]
- */
- @Override
- public double getAbsolutePosition()
- {
- return m_encoder.getAbsolutePosition();
- }
-
- /**
- * Get the velocity of the absolute encoder in degrees per second.
- *
- * @return Velocity in degrees per second.
- */
- @Override
- public double getVelocity()
- {
- return m_encoder.getVelocity();
- }
-
- /**
- * Configure the magnetic offset for the AbsoluteEncoder.
- *
- * @param offset Offset in degrees.
- */
- @Override
- public void setOffset(double offset)
- {
- m_encoder.configMagnetOffset(offset);
- }
-
- /**
- * Is the encoder reachable?
- *
- * @return True if reachable, false otherwise.
- */
- @Override
- public boolean reachable()
- {
- return m_encoder.getFirmwareVersion() > 0;
- }
-
- /**
- * Configure the sensor direction
- *
- * @param isInverted Inverted or not.
- */
- @Override
- public void setInverted(boolean isInverted)
- {
- m_encoder.configSensorDirection(isInverted);
- }
-
- /**
- * Reset encoder to factory default settings, if possible.
- */
- @Override
- public void factoryDefault()
- {
- m_encoder.configFactoryDefault();
- }
-}
diff --git a/swervelib/encoders/PWMAnalogEncoder.java b/swervelib/encoders/PWMAnalogEncoder.java
deleted file mode 100644
index 4d4dc89..0000000
--- a/swervelib/encoders/PWMAnalogEncoder.java
+++ /dev/null
@@ -1,128 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.encoders;
-
-import com.ctre.phoenix.sensors.MagnetFieldStrength;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.AnalogEncoder;
-import edu.wpi.first.wpilibj.Timer;
-import frc.robot.Robot;
-import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder;
-
-public class PWMAnalogEncoder extends SwerveEncoder
-{
-
- private final Timer m_timer;
- private boolean m_inverted = false;
- private double m_lastAngle, m_velocity, m_lastTime;
-
- /**
- * Constructor for analog encoder Swerve Encoder.
- *
- * @param encoder AnalogEncoder.
- */
- public PWMAnalogEncoder(AnalogEncoder encoder)
- {
- m_encoder = encoder;
- m_timer = new Timer();
- m_timer.start();
- configure();
-
- m_lastTime = m_timer.get();
- m_lastAngle = m_encoder.getDistance();
- Robot.getInstance().addPeriodic(this::updateVelocity, 1);
- }
-
- /**
- * Updates the velocity every second.
- */
- public void updateVelocity()
- {
- double currentTime = m_timer.get(), currentAngle = m_encoder.getDistance();
- m_velocity = (currentAngle - m_lastAngle) / (currentTime - m_lastTime);
- m_lastTime = currentTime;
- m_lastAngle = currentAngle;
- }
-
- /**
- * Configure the absolute encoder if possible.
- */
- @Override
- public void configure()
- {
- m_encoder.setDistancePerRotation(360);
- }
-
- /**
- * Reset encoder to factory default settings, if possible.
- */
- @Override
- public void factoryDefault()
- {
- m_encoder.reset();
- }
-
- /**
- * Get the magnetic field strength, if available.
- *
- * @return CTRE MagneticFieldStrength Enum.
- */
- @Override
- public MagnetFieldStrength getMagnetFieldStrength()
- {
- return MagnetFieldStrength.Good_GreenLED;
- }
-
- /**
- * Get the absolute position in degrees.
- *
- * @return Absolute position (0, 360]
- */
- @Override
- public double getAbsolutePosition()
- {
- return m_encoder.getDistance() * (m_inverted ? -1 : 1);
- }
-
- /**
- * Get the velocity of the absolute encoder in degrees per second.
- *
- * @return Velocity in degrees per second.
- */
- @Override
- public double getVelocity()
- {
- return m_velocity;
- }
-
- /**
- * Configure the magnetic offset for the AbsoluteEncoder.
- *
- * @param offset Offset in degrees.
- */
- @Override
- public void setOffset(double offset)
- {
- m_encoder.setPositionOffset((Rotation2d.fromDegrees(offset).getDegrees() + 180) / 360);
- }
-
- /**
- * Is the encoder reachable?
- *
- * @return True if reachable, false otherwise.
- */
- @Override
- public boolean reachable()
- {
- return true;
- }
-
- /**
- * Configure the sensor direction
- *
- * @param isInverted Inverted or not.
- */
- @Override
- public void setInverted(boolean isInverted)
- {
- m_inverted = isInverted;
- }
-}
diff --git a/swervelib/encoders/PWMDutyCycleEncoder.java b/swervelib/encoders/PWMDutyCycleEncoder.java
deleted file mode 100644
index 2a3291b..0000000
--- a/swervelib/encoders/PWMDutyCycleEncoder.java
+++ /dev/null
@@ -1,129 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.encoders;
-
-import com.ctre.phoenix.sensors.MagnetFieldStrength;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.Timer;
-import frc.robot.Robot;
-import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder;
-
-public class PWMDutyCycleEncoder extends SwerveEncoder
-{
-
- private final Timer m_timer;
- private boolean m_inverted = false;
- private double m_lastAngle, m_velocity, m_lastTime;
-
- /**
- * Construct PWM DutyCycleEncoder class.
- *
- * @param encoder Encoder class.
- */
- public PWMDutyCycleEncoder(DutyCycleEncoder encoder)
- {
- m_timer = new Timer();
- m_timer.start();
-
- m_encoder = encoder;
- configure();
-
- m_lastTime = m_timer.get();
- m_lastAngle = m_encoder.getDistance();
- Robot.getInstance().addPeriodic(this::updateVelocity, 1);
- }
-
- /**
- * Updates the velocity every second.
- */
- public void updateVelocity()
- {
- double currentTime = m_timer.get(), currentAngle = m_encoder.getDistance();
- m_velocity = (currentAngle - m_lastAngle) / (currentTime - m_lastTime);
- m_lastTime = currentTime;
- m_lastAngle = currentAngle;
- }
-
- /**
- * Configure the absolute encoder if possible.
- */
- @Override
- public void configure()
- {
- m_encoder.setDistancePerRotation(360);
- }
-
- /**
- * Reset encoder to factory default settings, if possible.
- */
- @Override
- public void factoryDefault()
- {
- m_encoder.reset();
- }
-
- /**
- * Get the magnetic field strength, if available.
- *
- * @return CTRE MagneticFieldStrength Enum.
- */
- @Override
- public MagnetFieldStrength getMagnetFieldStrength()
- {
- return MagnetFieldStrength.Good_GreenLED;
- }
-
- /**
- * Get the absolute position in degrees.
- *
- * @return Absolute position (0, 360]
- */
- @Override
- public double getAbsolutePosition()
- {
- return m_encoder.getDistance() * (m_inverted ? -1 : 1);
- }
-
- /**
- * Get the velocity of the absolute encoder in degrees per second.
- *
- * @return Velocity in degrees per second.
- */
- @Override
- public double getVelocity()
- {
- return m_velocity;
- }
-
- /**
- * Configure the magnetic offset for the AbsoluteEncoder.
- *
- * @param offset Offset in degrees.
- */
- @Override
- public void setOffset(double offset)
- {
- m_encoder.setPositionOffset((Rotation2d.fromDegrees(offset).getDegrees() + 180) / 360);
- }
-
- /**
- * Is the encoder reachable?
- *
- * @return True if reachable, false otherwise.
- */
- @Override
- public boolean reachable()
- {
- return true;
- }
-
- /**
- * Configure the sensor direction
- *
- * @param isInverted Inverted or not.
- */
- @Override
- public void setInverted(boolean isInverted)
- {
- m_inverted = isInverted;
- }
-}
diff --git a/swervelib/encoders/REVAbsoluteEncoder.java b/swervelib/encoders/REVAbsoluteEncoder.java
deleted file mode 100644
index 39536d9..0000000
--- a/swervelib/encoders/REVAbsoluteEncoder.java
+++ /dev/null
@@ -1,107 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.encoders;
-
-import com.ctre.phoenix.sensors.MagnetFieldStrength;
-import com.revrobotics.AbsoluteEncoder;
-import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder;
-
-public class REVAbsoluteEncoder extends SwerveEncoder
-{
-
- /**
- * Constructor for AbsoluteEncoder class.
- *
- * @param encoder Encoder to wrap around.
- */
- public REVAbsoluteEncoder(AbsoluteEncoder encoder)
- {
- m_encoder = encoder;
- configure();
- }
-
- /**
- * Configure the absolute encoder if possible.
- */
- @Override
- public void configure()
- {
- // Redundant, but ensures everything is always correct.
- m_encoder.setPositionConversionFactor(360);
- m_encoder.setVelocityConversionFactor(360.0 / 60.0);
- }
-
- /**
- * Reset encoder to factory default settings, if possible.
- */
- @Override
- public void factoryDefault()
- {
- // Nothing to do here.
- }
-
- /**
- * Get the magnetic field strength, if available.
- *
- * @return CTRE MagneticFieldStrength Enum.
- */
- @Override
- public MagnetFieldStrength getMagnetFieldStrength()
- {
- // Unable to check, assume green.
- return MagnetFieldStrength.Good_GreenLED;
- }
-
- /**
- * Get the absolute position in degrees.
- *
- * @return Absolute position (0, 360]
- */
- @Override
- public double getAbsolutePosition()
- {
- return m_encoder.getPosition();
- }
-
- /**
- * Get the velocity of the absolute encoder in degrees per second.
- *
- * @return Velocity in degrees per second.
- */
- @Override
- public double getVelocity()
- {
- return m_encoder.getVelocity();
- }
-
- /**
- * Configure the magnetic offset for the AbsoluteEncoder.
- *
- * @param offset Offset in degrees.
- */
- @Override
- public void setOffset(double offset)
- {
- m_encoder.setZeroOffset(offset);
- }
-
- /**
- * Is the encoder reachable?
- *
- * @return True if reachable, false otherwise.
- */
- @Override
- public boolean reachable()
- {
- return true;
- }
-
- /**
- * Configure the sensor direction.
- *
- * @param isInverted Inverted or not.
- */
- @Override
- public void setInverted(boolean isInverted)
- {
- m_encoder.setInverted(isInverted);
- }
-}
diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java
new file mode 100644
index 0000000..5e61227
--- /dev/null
+++ b/swervelib/encoders/SparkMaxEncoderSwerve.java
@@ -0,0 +1,85 @@
+package frc.robot.subsystems.swervedrive2.swervelib.encoders;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+
+/**
+ * SparkMax absolute encoder, attached through the data port.
+ */
+public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
+{
+
+ /**
+ * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
+ */
+ public AbsoluteEncoder encoder;
+
+ /**
+ * Create the {@link AbsoluteEncoder} object as a duty cycle. from the {@link CANSparkMax} motor.
+ *
+ * @param motor Motor to create the encoder from.
+ */
+ public SparkMaxEncoderSwerve(SwerveMotor motor)
+ {
+ if (motor.getMotor() instanceof CANSparkMax)
+ {
+ encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle);
+ } else
+ {
+ throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
+ }
+ }
+
+ /**
+ * Reset the encoder to factory defaults.
+ */
+ @Override
+ public void factoryDefault()
+ {
+ // Do nothing
+ }
+
+ /**
+ * Clear sticky faults on the encoder.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ // Do nothing
+ }
+
+ /**
+ * Configure the absolute encoder to read from [0, 360) per second.
+ *
+ * @param inverted Whether the encoder is inverted.
+ */
+ @Override
+ public void configure(boolean inverted)
+ {
+ encoder.setInverted(inverted);
+ }
+
+ /**
+ * Get the absolute position of the encoder.
+ *
+ * @return Absolute position in degrees from [0, 360).
+ */
+ @Override
+ public double getAbsolutePosition()
+ {
+ return encoder.getPosition();
+ }
+
+ /**
+ * Get the instantiated absolute encoder Object.
+ *
+ * @return Absolute encoder object.
+ */
+ @Override
+ public Object getAbsoluteEncoder()
+ {
+ return encoder;
+ }
+}
diff --git a/swervelib/encoders/SwerveAbsoluteEncoder.java b/swervelib/encoders/SwerveAbsoluteEncoder.java
new file mode 100644
index 0000000..f63086c
--- /dev/null
+++ b/swervelib/encoders/SwerveAbsoluteEncoder.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems.swervedrive2.swervelib.encoders;
+
+/**
+ * Swerve abstraction class to define a standard interface with absolute encoders for swerve modules..
+ */
+public abstract class SwerveAbsoluteEncoder
+{
+
+ /**
+ * Reset the encoder to factory defaults.
+ */
+ public abstract void factoryDefault();
+
+ /**
+ * Clear sticky faults on the encoder.
+ */
+ public abstract void clearStickyFaults();
+
+ /**
+ * Configure the absolute encoder to read from [0, 360) per second.
+ *
+ * @param inverted Whether the encoder is inverted.
+ */
+ public abstract void configure(boolean inverted);
+
+ /**
+ * Get the absolute position of the encoder.
+ *
+ * @return Absolute position in degrees from [0, 360).
+ */
+ public abstract double getAbsolutePosition();
+
+ /**
+ * Get the instantiated absolute encoder Object.
+ *
+ * @return Absolute encoder object.
+ */
+ public abstract Object getAbsoluteEncoder();
+}
diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java
new file mode 100644
index 0000000..0f4cbeb
--- /dev/null
+++ b/swervelib/imu/NavXSwerve.java
@@ -0,0 +1,97 @@
+package frc.robot.subsystems.swervedrive2.swervelib.imu;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Communicates with the NavX as the IMU.
+ */
+public class NavXSwerve extends SwerveIMU
+{
+
+ /**
+ * NavX IMU.
+ */
+ private AHRS gyro;
+ /**
+ * Offset for the NavX yaw reading.
+ */
+ private double yawOffset = 0;
+
+ /**
+ * Constructor for the NavX swerve.
+ */
+ public NavXSwerve()
+ {
+ try
+ {
+ /* Communicate w/navX-MXP via the MXP SPI Bus. */
+ /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
+ /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
+ gyro = new AHRS(SerialPort.Port.kMXP);
+ SmartDashboard.putData(gyro);
+ } catch (RuntimeException ex)
+ {
+ DriverStation.reportError("Error instantiating navX-MXP: " + ex.getMessage(), true);
+ }
+ }
+
+ /**
+ * Reset IMU to factory default.
+ */
+ @Override
+ public void factoryDefault()
+ {
+ // gyro.reset(); // Reported to be slow
+ yawOffset = gyro.getYaw();
+ }
+
+ /**
+ * Clear sticky faults on IMU.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ }
+
+ /**
+ * Set the yaw in degrees.
+ *
+ * @param yaw Yaw angle in degrees.
+ */
+ @Override
+ public void setYaw(double yaw)
+ {
+ // gyro.reset(); // Reported to be slow using the offset.
+ if (yaw != 0)
+ {
+ yawOffset = yaw + gyro.getYaw();
+ }
+ }
+
+ /**
+ * Fetch the yaw/pitch/roll from the IMU.
+ *
+ * @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
+ */
+ @Override
+ public void getYawPitchRoll(double[] yprArray)
+ {
+ yprArray[0] = gyro.getYaw() - yawOffset;
+ yprArray[1] = gyro.getPitch();
+ yprArray[2] = gyro.getRoll();
+ }
+
+ /**
+ * Get the instantiated IMU object.
+ *
+ * @return IMU object.
+ */
+ @Override
+ public Object getIMU()
+ {
+ return gyro;
+ }
+}
diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java
new file mode 100644
index 0000000..f811d33
--- /dev/null
+++ b/swervelib/imu/Pigeon2Swerve.java
@@ -0,0 +1,93 @@
+package frc.robot.subsystems.swervedrive2.swervelib.imu;
+
+import com.ctre.phoenix.sensors.Pigeon2Configuration;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * SwerveIMU interface for the Pigeon2
+ */
+public class Pigeon2Swerve extends SwerveIMU
+{
+
+ /**
+ * Pigeon2 IMU device.
+ */
+ WPI_Pigeon2 imu;
+
+
+ /**
+ * Generate the SwerveIMU for pigeon.
+ *
+ * @param canid CAN ID for the pigeon
+ * @param canbus CAN Bus name the pigeon resides on.
+ */
+ public Pigeon2Swerve(int canid, String canbus)
+ {
+ imu = new WPI_Pigeon2(canid, canbus);
+ Pigeon2Configuration config = new Pigeon2Configuration();
+ imu.configAllSettings(config);
+ SmartDashboard.putData(imu);
+ }
+
+ /**
+ * Generate the SwerveIMU for pigeon.
+ *
+ * @param canid CAN ID for the pigeon
+ */
+ public Pigeon2Swerve(int canid)
+ {
+ this(canid, "");
+ }
+
+ /**
+ * Reset IMU to factory default.
+ */
+ @Override
+ public void factoryDefault()
+ {
+ imu.configFactoryDefault();
+ }
+
+ /**
+ * Clear sticky faults on IMU.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ imu.clearStickyFaults();
+ }
+
+ /**
+ * Set the yaw in degrees.
+ *
+ * @param yaw Angle in degrees.
+ */
+ @Override
+ public void setYaw(double yaw)
+ {
+ imu.setYaw(yaw);
+ }
+
+ /**
+ * Fetch the yaw/pitch/roll from the IMU, inverts them all if SwerveIMU is inverted.
+ *
+ * @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
+ */
+ @Override
+ public void getYawPitchRoll(double[] yprArray)
+ {
+ imu.getYawPitchRoll(yprArray);
+ }
+
+ /**
+ * Get the instantiated IMU object.
+ *
+ * @return IMU object.
+ */
+ @Override
+ public Object getIMU()
+ {
+ return imu;
+ }
+}
diff --git a/swervelib/imu/PigeonSwerve.java b/swervelib/imu/PigeonSwerve.java
new file mode 100644
index 0000000..e94a62f
--- /dev/null
+++ b/swervelib/imu/PigeonSwerve.java
@@ -0,0 +1,75 @@
+package frc.robot.subsystems.swervedrive2.swervelib.imu;
+
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * SwerveIMU interface for the Pigeon.
+ */
+public class PigeonSwerve extends SwerveIMU
+{
+
+ WPI_PigeonIMU imu;
+
+ /**
+ * Generate the SwerveIMU for pigeon.
+ *
+ * @param canid CAN ID for the pigeon, does not support CANBus.
+ */
+ public PigeonSwerve(int canid)
+ {
+ imu = new WPI_PigeonIMU(canid);
+ SmartDashboard.putData(imu);
+ }
+
+ /**
+ * Reset IMU to factory default.
+ */
+ @Override
+ public void factoryDefault()
+ {
+ imu.configFactoryDefault();
+ }
+
+ /**
+ * Clear sticky faults on IMU.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ imu.clearStickyFaults();
+ }
+
+ /**
+ * Set the yaw in degrees.
+ *
+ * @param yaw Angle in degrees.
+ */
+ @Override
+ public void setYaw(double yaw)
+ {
+ imu.setYaw(yaw);
+ }
+
+ /**
+ * Fetch the yaw/pitch/roll from the IMU, inverts them all if SwerveIMU is inverted.
+ *
+ * @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
+ */
+ @Override
+ public void getYawPitchRoll(double[] yprArray)
+ {
+ imu.getYawPitchRoll(yprArray);
+ }
+
+ /**
+ * Get the instantiated IMU object.
+ *
+ * @return IMU object.
+ */
+ @Override
+ public Object getIMU()
+ {
+ return imu;
+ }
+}
diff --git a/swervelib/imu/SwerveIMU.java b/swervelib/imu/SwerveIMU.java
new file mode 100644
index 0000000..a6ad31e
--- /dev/null
+++ b/swervelib/imu/SwerveIMU.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems.swervedrive2.swervelib.imu;
+
+/**
+ * Swerve IMU abstraction to define a standard interface with a swerve drive.
+ */
+public abstract class SwerveIMU
+{
+
+ /**
+ * Reset IMU to factory default.
+ */
+ public abstract void factoryDefault();
+
+ /**
+ * Clear sticky faults on IMU.
+ */
+ public abstract void clearStickyFaults();
+
+ /**
+ * Set the yaw in degrees.
+ *
+ * @param yaw Yaw angle in degrees.
+ */
+ public abstract void setYaw(double yaw);
+
+ /**
+ * Fetch the yaw/pitch/roll from the IMU.
+ *
+ * @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
+ */
+ public abstract void getYawPitchRoll(double[] yprArray);
+
+ /**
+ * Get the instantiated IMU object.
+ *
+ * @return IMU object.
+ */
+ public abstract Object getIMU();
+}
diff --git a/swervelib/kinematics/SwerveModuleState2.java b/swervelib/kinematics/SwerveModuleState2.java
deleted file mode 100644
index f891acf..0000000
--- a/swervelib/kinematics/SwerveModuleState2.java
+++ /dev/null
@@ -1,119 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.kinematics;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-
-// Copied from team3181's REVSwerve2023 repo
-public class SwerveModuleState2 extends edu.wpi.first.math.kinematics.SwerveModuleState
-{
-
- /**
- * Angular velocity in radians per second. Angular Velocity = omega.
- */
- public double angularVelocityRadPerSecond = 0;
-
- /**
- * Constructs a SwerveModuleState with zeros for speed and angle.
- */
- public SwerveModuleState2()
- {
- }
-
- /**
- * Constructs a SwerveModuleState with zeros for speed and angle.
- */
- public SwerveModuleState2(SwerveModuleState self)
- {
- super(self.speedMetersPerSecond, self.angle);
- this.angularVelocityRadPerSecond = 0;
- }
-
- /**
- * Constructs a SwerveModuleState.
- *
- * @param speedMetersPerSecond The speed of the wheel of the module.
- * @param angle The angle of the module.
- */
- public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle)
- {
- super(speedMetersPerSecond, angle);
- this.angularVelocityRadPerSecond = 0;
- }
-
- /**
- * Constructs a SwerveModuleState.
- *
- * @param speedMetersPerSecond The speed of the wheel of the module.
- * @param angle The angle of the module.
- * @param angularVelocityRadPerSecond The angular velocity of the module.
- */
- public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle, double angularVelocityRadPerSecond)
- {
- super(speedMetersPerSecond, angle);
- this.angularVelocityRadPerSecond = angularVelocityRadPerSecond;
- }
-
- /**
- * Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
- * the wheel spins. Customized from WPILib's version to include placing in appropriate scope for CTRE and REV onboard
- * control as both controllers as of writing don't have support for continuous input.
- *
- * @param desiredState The desired state.
- * @param currentAngle The current module angle.
- */
- public static SwerveModuleState2 optimize(
- SwerveModuleState2 desiredState, Rotation2d currentAngle)
- {
- SwerveModuleState2 state;
- Rotation2d delta = desiredState.angle.minus(currentAngle);
- if (Math.abs(delta.getDegrees()) > 90.0)
- {
- state = new SwerveModuleState2(
- -desiredState.speedMetersPerSecond,
- desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
- } else
- {
- state = new SwerveModuleState2(desiredState.speedMetersPerSecond, desiredState.angle);
- }
- // state.angle = Rotation2d.fromDegrees(
- // placeInAppropriate0To360Scope(currentAngle.getDegrees(), state.angle.getDegrees()));
- return state;
- }
-
- /**
- * @param scopeReference Current Angle
- * @param newAngle Target Angle
- * @return Closest angle within scope
- */
- public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
- {
- double lowerBound;
- double upperBound;
- double lowerOffset = scopeReference % 360;
- if (lowerOffset >= 0)
- {
- lowerBound = scopeReference - lowerOffset;
- upperBound = scopeReference + (360 - lowerOffset);
- } else
- {
- upperBound = scopeReference - lowerOffset;
- lowerBound = scopeReference - (360 + lowerOffset);
- }
- while (newAngle < lowerBound)
- {
- newAngle += 360;
- }
- while (newAngle > upperBound)
- {
- newAngle -= 360;
- }
- if (newAngle - scopeReference > 180)
- {
- newAngle -= 360;
- } else if (newAngle - scopeReference < -180)
- {
- newAngle += 360;
- }
- return newAngle;
- }
-}
\ No newline at end of file
diff --git a/swervelib/kinematics/SwerveDriveKinematics2.java b/swervelib/math/SwerveKinematics2.java
similarity index 91%
rename from swervelib/kinematics/SwerveDriveKinematics2.java
rename to swervelib/math/SwerveKinematics2.java
index 96dd1a3..533ef41 100644
--- a/swervelib/kinematics/SwerveDriveKinematics2.java
+++ b/swervelib/math/SwerveKinematics2.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.swervedrive.swerve.kinematics;
+package frc.robot.subsystems.swervedrive2.swervelib.math;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
@@ -6,22 +6,21 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
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 java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
-// Taken from https://github.com/FRC2539/javabot-2023/tree/main/src/main/java/frc/lib/swerve
-// and https://github.com/first95/FRC2022/tree/feature/secondOrderKinematics/SwervyBot/src/main/java/frc/lib
-
/**
* Clone of WPI SwerveKinematics, which implements second order kinematics when calculating modules states from chassis
* speed.
- * Taken from https://github.com/FRC2539/javabot-2023/tree/main/src/main/java/frc/lib/swerve
- * https://github.com/first95/FRC2022/tree/feature/secondOrderKinematics/SwervyBot/src/main/java/frc/lib
- * Makes use of {@link SwerveDriveKinematics2} to add the angular velocity that is required of the module as an output.
+ *
+ *
+ * Makes use of {@link SwerveModuleState2} to add the angular velocity that is required of the module as an output.
*/
-public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.SwerveDriveKinematics
+public class SwerveKinematics2 extends SwerveDriveKinematics
{
private final SimpleMatrix m_inverseKinematics;
@@ -34,14 +33,14 @@ public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.Swerve
private Translation2d m_prevCoR = new Translation2d();
/**
- * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as
- * {@link Translation2d}s. The order in which you pass in the wheel locations is the same order that you will receive
- * the module states when performing inverse kinematics. It is also expected that you pass in the module states in the
- * same order when calling the forward kinematics methods.
+ * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as Translation2ds.
+ * The order in which you pass in the wheel locations is the same order that you will receive the module states when
+ * performing inverse kinematics. It is also expected that you pass in the module states in the same order when
+ * calling the forward kinematics methods.
*
* @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
*/
- public SwerveDriveKinematics2(Translation2d... wheelsMeters)
+ public SwerveKinematics2(Translation2d... wheelsMeters)
{
super(wheelsMeters);
if (wheelsMeters.length < 2)
@@ -85,7 +84,7 @@ public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.Swerve
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond)
{
- for (SwerveModuleState2 moduleState : moduleStates)
+ for (SwerveModuleState moduleState : moduleStates)
{
moduleState.speedMetersPerSecond =
moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
@@ -133,7 +132,7 @@ public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.Swerve
/ attainableMaxRotationalVelocityRadiansPerSecond;
double k = Math.max(translationalK, rotationalK);
double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
- for (SwerveModuleState2 moduleState : moduleStates)
+ for (SwerveModuleState moduleState : moduleStates)
{
moduleState.speedMetersPerSecond *= scale;
}
@@ -266,7 +265,7 @@ public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.Swerve
var omegaVector = trigThetaAngle.mult(accelVector);
- double omega = omegaVector.get(1, 0) / speed;
+ double omega = (omegaVector.get(1, 0) / speed) - chassisSpeeds.omegaRadiansPerSecond;
m_moduleStates[i] = new SwerveModuleState2(speed, angle, omega);
}
@@ -350,4 +349,4 @@ public class SwerveDriveKinematics2 extends edu.wpi.first.math.kinematics.Swerve
return new Twist2d(
chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
}
-}
\ No newline at end of file
+}
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
new file mode 100644
index 0000000..66e8737
--- /dev/null
+++ b/swervelib/math/SwerveMath.java
@@ -0,0 +1,234 @@
+package frc.robot.subsystems.swervedrive2.swervelib.math;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveController;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveDriveConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModuleConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveParser;
+
+/**
+ * Mathematical functions which pertain to swerve drive.
+ */
+public class SwerveMath
+{
+
+ /**
+ * Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
+ * degree <=> (maxVolts) / (maxSpeed)
+ *
+ * @param optimalVoltage Optimal voltage to use when calculating the angle kV.
+ * @param motorFreeSpeedRPM Motor free speed in Rotations per Minute.
+ * @param angleGearRatio Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.
+ * @return angle kV for feedforward.
+ */
+ public static double calculateAngleKV(double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
+ {
+ double maxAngularVelocity = 360 * (motorFreeSpeedRPM / angleGearRatio) / 60; // deg/s
+ return optimalVoltage / maxAngularVelocity;
+ }
+
+ /**
+ * Calculate the meters per rotation for the integrated encoder. Calculation: 4in diameter wheels * pi [circumfrence]
+ * / gear ratio.
+ *
+ * @param wheelDiameter Wheel diameter in meters.
+ * @param driveGearRatio The gear ratio of the drive motor.
+ * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder.
+ * @return Meters per rotation for the drive motor.
+ */
+ public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation)
+ {
+ return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation);
+ }
+
+ /**
+ * Algebraically apply a deadband using a piece wise function.
+ *
+ * @param value value to apply deadband to.
+ * @param scaled Use algebra to determine deadband by starting the value at 0 past deadband.
+ * @param deadband The deadbnad to apply.
+ * @return Value with deadband applied.
+ */
+ public static double applyDeadband(double value, boolean scaled, double deadband)
+ {
+ value = Math.abs(value) > deadband ? value : 0;
+ return scaled ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) : value;
+ }
+
+ /**
+ * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts
+ * motor rotations to linear wheel distance and steering converts motor rotations to module azimuth.
+ *
+ * @param angleGearRatio The gear ratio of the steering motor.
+ * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated.
+ * @return Degrees per steering rotation for the angle motor.
+ */
+ public static double calculateDegreesPerSteeringRotation(double angleGearRatio, double pulsePerRotation)
+ {
+ return 360 / (angleGearRatio * pulsePerRotation);
+ }
+
+ /**
+ * Calculate the maximum angular velocity.
+ *
+ * @param maxSpeed Max speed of the robot in meters per second.
+ * @param furthestModuleX X of the furthest module in meters.
+ * @param furthestModuleY Y of the furthest module in meters.
+ * @return Maximum angular velocity in rad/s.
+ */
+ public static double calculateMaxAngularVelocity(double maxSpeed, double furthestModuleX, double furthestModuleY)
+ {
+ return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
+ }
+
+ /**
+ * Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction.
+ *
+ * @param cof Coefficient of Friction of the wheel grip tape.
+ * @return Practical maximum acceleration in m/s/s.
+ */
+ public static double calculateMaxAcceleration(double cof)
+ {
+ return cof * 9.81;
+ }
+
+ /**
+ * Calculate the maximum theoretical acceleration without friction.
+ *
+ * @param stallTorqueNm Stall torque of driving motor in nM.
+ * @param gearRatio Gear ratio for driving motor number of motor rotations until one wheel rotation.
+ * @param moduleCount Number of swerve modules.
+ * @param wheelDiameter Wheel diameter in meters.
+ * @param robotMass Mass of the robot in kg.
+ * @return Theoretical maximum acceleration in m/s/s.
+ */
+ public static double calculateMaxAcceleration(double stallTorqueNm, double gearRatio, double moduleCount,
+ double wheelDiameter, double robotMass)
+ {
+ return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass);
+ }
+
+ /**
+ * Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm position from
+ * NetworkTables and is passed the direction in question.
+ *
+ * @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that this
+ * is robot-relative.
+ * @param chassisMass Chassis mass in kg. (The weight of just the chassis not anything else)
+ * @param robotMass The weight of the robot in kg. (Including manipulators, etc).
+ * @param chassisCenterOfGravity Chassis center of gravity.
+ * @param config The swerve drive configuration.
+ * @return Maximum acceleration allowed in the robot direction.
+ */
+ private static double calcMaxAccel(Rotation2d angle, double chassisMass, double robotMass,
+ Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
+ {
+ double xMoment = (chassisCenterOfGravity.getX() * chassisMass);
+ double yMoment = (chassisCenterOfGravity.getY() * chassisMass);
+ // Calculate the vertical mass moment using the floor as the datum. This will be used later to calculate max
+ // acceleration
+ double zMoment = (chassisCenterOfGravity.getZ() * (chassisMass));
+ Translation3d robotCG = new Translation3d(xMoment, yMoment, zMoment).div(robotMass);
+ Translation2d horizontalCG = robotCG.toTranslation2d();
+
+ Translation2d projectedHorizontalCg = new Translation2d(
+ (angle.getSin() * angle.getCos() * horizontalCG.getY()) + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
+ (angle.getSin() * angle.getCos() * horizontalCG.getX()) + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())
+ );
+
+ // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is rectangular.
+ // Because a line is being projected, rather than a point, one of the coordinates of the projected point is
+ // already known.
+ Translation2d projectedWheelbaseEdge;
+ double angDeg = angle.getDegrees();
+ if (angDeg <= 45 && angDeg >= -45)
+ {
+ SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("frontleft", config).configuration;
+ projectedWheelbaseEdge = new Translation2d(conf.moduleLocation.getX(),
+ conf.moduleLocation.getX() * angle.getTan());
+ } else if (135 >= angDeg && angDeg > 45)
+ {
+ SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("frontleft", config).configuration;
+
+ projectedWheelbaseEdge = new Translation2d(
+ conf.moduleLocation.getY() / angle.getTan(),
+ conf.moduleLocation.getY());
+ } else if (-135 <= angDeg && angDeg < -45)
+ {
+ SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("frontright", config).configuration;
+ projectedWheelbaseEdge = new Translation2d(
+ conf.moduleLocation.getY() / angle.getTan(),
+ conf.moduleLocation.getY());
+ } else
+ {
+ SwerveModuleConfiguration conf = SwerveParser.getModuleConfigurationByName("backleft", config).configuration;
+ projectedWheelbaseEdge = new Translation2d(
+ conf.moduleLocation.getX(),
+ conf.moduleLocation.getX() * angle.getTan());
+ }
+
+ double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm();
+ double maxAccel = 9.81 * horizontalDistance / robotCG.getZ();
+
+ SmartDashboard.putNumber("calcMaxAccel", maxAccel);
+ return maxAccel;
+ }
+
+ /**
+ * Limits a commanded velocity to prevent exceeding the maximum acceleration given by
+ * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that
+ * this takes and returns field-relative velocities.
+ *
+ * @param commandedVelocity The desired velocity
+ * @param fieldVelocity The velocity of the robot within a field relative state.
+ * @param robotPose The current pose of the robot.
+ * @param loopTime The time it takes to update the velocity in seconds. Note: this should include the
+ * 100ms that it takes for a SparkMax velocity to update.
+ * @param chassisMass Chassis mass in kg. (The weight of just the chassis not anything else)
+ * @param robotMass The weight of the robot in kg. (Including manipulators, etc).
+ * @param chassisCenterOfGravity Chassis center of gravity.
+ * @param config The swerve drive configuration.
+ * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable
+ * velocity.
+ */
+ public static Translation2d limitVelocity(Translation2d commandedVelocity, ChassisSpeeds fieldVelocity,
+ Pose2d robotPose, double loopTime, double chassisMass, double robotMass,
+ Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
+ {
+ // Get the robot's current field-relative velocity
+ Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity);
+ SmartDashboard.putNumber("currentVelocity", currentVelocity.getX());
+
+ // Calculate the commanded change in velocity by subtracting current velocity
+ // from commanded velocity
+ Translation2d deltaV = commandedVelocity.minus(currentVelocity);
+ SmartDashboard.putNumber("deltaV", deltaV.getX());
+
+ // Creates an acceleration vector with the direction of delta V and a magnitude
+ // of the maximum allowed acceleration in that direction
+ Translation2d maxAccel = new Translation2d(
+ calcMaxAccel(deltaV
+ // Rotates the velocity vector to convert from field-relative to robot-relative
+ .rotateBy(robotPose.getRotation().unaryMinus())
+ .getAngle(), chassisMass, robotMass, chassisCenterOfGravity, config),
+ deltaV.getAngle());
+
+ // Calculate the maximum achievable velocity by the next loop cycle.
+ // delta V = Vf - Vi = at
+ Translation2d maxAchievableDeltaVelocity = maxAccel.times(loopTime);
+
+ if (deltaV.getNorm() > maxAchievableDeltaVelocity.getNorm())
+ {
+ return maxAchievableDeltaVelocity.plus(currentVelocity);
+ } else
+ {
+ // If the commanded velocity is attainable, use that.
+ return commandedVelocity;
+ }
+ }
+}
diff --git a/swervelib/math/SwerveModuleState2.java b/swervelib/math/SwerveModuleState2.java
new file mode 100644
index 0000000..143af0e
--- /dev/null
+++ b/swervelib/math/SwerveModuleState2.java
@@ -0,0 +1,41 @@
+package frc.robot.subsystems.swervedrive2.swervelib.math;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+/**
+ * Second order kinematics swerve module state.
+ */
+public class SwerveModuleState2 extends SwerveModuleState
+{
+
+ public double speedMetersPerSecond;
+
+ /**
+ * Rad per sec
+ */
+ public double omegaRadPerSecond = 0;
+
+ public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+ /**
+ * Constructs a SwerveModuleState with zeros for speed and angle.
+ */
+ public SwerveModuleState2()
+ {
+ }
+
+ /**
+ * Constructs a SwerveModuleState.
+ *
+ * @param speedMetersPerSecond The speed of the wheel of the module.
+ * @param angle The angle of the module.
+ * @param omegaRadPerSecond The angular velocity of the module.
+ */
+ public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
+ {
+ this.speedMetersPerSecond = speedMetersPerSecond;
+ this.angle = angle;
+ this.omegaRadPerSecond = omegaRadPerSecond;
+ }
+}
diff --git a/swervelib/motors/CTRESwerveMotor.java b/swervelib/motors/CTRESwerveMotor.java
deleted file mode 100644
index eff6322..0000000
--- a/swervelib/motors/CTRESwerveMotor.java
+++ /dev/null
@@ -1,503 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.motors;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.AbsoluteSensorRange;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.SensorInitializationStrategy;
-import edu.wpi.first.math.util.Units;
-import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder;
-import frc.robot.subsystems.swervedrive.swerve.SwerveMotor;
-import java.util.function.Supplier;
-
-public class CTRESwerveMotor extends SwerveMotor
-{
-
- private final SwerveEncoder m_angleEncoder;
- private final boolean m_integratedAbsEncoder;
- private final double m_allowableClosedLoopError;
-
- private final ControlMode m_controlMode;
- private final int m_mainPIDSlotId;
- private final int m_mainPidId;
- private final Supplier m_encoderRet;
- private final WPI_TalonFX m_motor;
-
- /**
- * kV feed forward for PID
- */
- private final double m_moduleRadkV;
-
- // TODO: Finish this based off of BaseFalconSwerve
- public CTRESwerveMotor(WPI_TalonFX motor, SwerveEncoder encoder, ModuleMotorType type, double gearRatio,
- double wheelDiameter,
- double freeSpeedRPM, double powerLimit)
- {
- TalonFXConfiguration config = new TalonFXConfiguration();
-
- m_integratedAbsEncoder = encoder.m_encoder instanceof CANCoder;
- // Inspired by the following sources
- // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/rev/NeoDriveControllerFactoryBuilder.java#L38
- // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L68
- // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/Constants.java#L89
- // https://github.com/AusTINCANsProgrammingTeam/2022Swerve/blob/main/2022Swerve/src/main/java/frc/robot/Constants.java
- motor.clearStickyFaults();
- motor.configFactoryDefault();
-
- m_angleEncoder = encoder;
- m_motorType = type;
- m_motor = motor;
-
- m_motor.setNeutralMode(NeutralMode.Brake);
- m_motor.setSensorPhase(true);
-
- m_motor.enableVoltageCompensation(true);
-
- m_mainPidId = CTRE_pidIdx.PRIMARY_PID.ordinal();
- if (type == ModuleMotorType.DRIVE)
- {
- m_moduleRadkV = 1;
- m_allowableClosedLoopError = Units.inchesToMeters(1) * 60;
-
- m_mainPIDSlotId = CTRE_slotIdx.Velocity.ordinal();
- m_controlMode = ControlMode.Velocity;
- m_encoderRet = m_motor::getSelectedSensorVelocity;
-
- setCurrentLimit(40);
-
- setConversionFactor(((Math.PI * wheelDiameter) / gearRatio) * 10); // Convert units to MPS.
- } else
- {
- m_moduleRadkV = (12 * 60) / (freeSpeedRPM * Math.toRadians(360 / (m_integratedAbsEncoder ? 1 : gearRatio)));
- m_allowableClosedLoopError = 5;
-
- // Configure the CANCoder as the remote sensor.
- if (m_integratedAbsEncoder)
- {
- m_motor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0);
- m_motor.configRemoteFeedbackFilter((CANCoder) m_angleEncoder.m_encoder,
- CTRE_remoteSensor.REMOTE_SENSOR_0.ordinal());
- ((CANCoder) m_angleEncoder.m_encoder).configAbsoluteSensorRange(AbsoluteSensorRange.Signed_PlusMinus180);
- } else
- {
- m_motor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor);
- m_motor.configIntegratedSensorInitializationStrategy(SensorInitializationStrategy.BootToZero);
- m_motor.configIntegratedSensorAbsoluteRange(AbsoluteSensorRange.Signed_PlusMinus180);
- setEncoder(0);
- }
-
- m_mainPIDSlotId = CTRE_slotIdx.Distance.ordinal();
- m_controlMode = ControlMode.Position;
- m_encoderRet = m_motor::getSelectedSensorPosition;
-
- setCurrentLimit(20);
-
- m_motor.configFeedbackNotContinuous(false, 100);
- }
-
- setPIDOutputRange(-powerLimit, powerLimit);
- setVoltageCompensation(12);
-
- optimizeCANFrames();
-
- }
-
- /**
- * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base.
- *
- * @param min Minimum output.
- * @param max Maximum output.
- */
- @Override
- public void setPIDOutputRange(double min, double max)
- {
- m_motor.configPeakOutputReverse(min);
- m_motor.configPeakOutputForward(max);
- }
-
- /**
- * Set the PIDF coefficients for the closed loop PID onboard the SparkMax.
- *
- * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
- * Default is 1.0
- * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
- * PID Loop.
- * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID
- * loop). Default is 0.1
- * @param F Feed Fwd gain for Closed loop.
- * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far
- * from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor
- * units.
- */
- @Override
- public void setPIDF(double P, double I, double D, double F, double integralZone)
- {
- m_motor.selectProfileSlot(m_mainPIDSlotId, m_mainPidId);
- // More Closed-Loop Configs at
- // https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#closed-loop-configs-per-slot-four-slots-available
- // Example at
- // https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20General/VelocityClosedLoop_ArbFeedForward/src/main/java/frc/robot/Robot.java
- m_motor.config_kP(m_mainPIDSlotId, P);
- m_motor.config_kI(m_mainPIDSlotId, I);
- m_motor.config_kD(m_mainPIDSlotId, D);
- m_motor.config_kF(m_mainPIDSlotId, F);
- m_motor.config_IntegralZone(m_mainPIDSlotId, integralZone);
- m_motor.configAllowableClosedloopError(m_mainPIDSlotId, m_allowableClosedLoopError);
-
- // If the closed loop error is within this threshold, the motor output will be neutral. Set to 0 to disable.
- // Value is in sensor units.
- }
-
- /**
- * Configures the conversion factor based upon which motor.
- *
- * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning
- * motor.
- */
- @Override
- public void setConversionFactor(double conversionFactor)
- {
- m_motor.configSelectedFeedbackCoefficient(conversionFactor);
- }
-
- /**
- * Set the target for the PID loop.
- *
- * @param target The PID target to aim for.
- * @param feedforward The feedforward for this target.
- */
- @Override
- public void setTarget(double target, double feedforward)
- {
- m_motor.set(m_controlMode, target, DemandType.ArbitraryFeedForward, feedforward);
- }
-
- /**
- * Stop the motor by sending 0 volts to it.
- */
- @Override
- public void stop()
- {
- m_motor.set(ControlMode.PercentOutput, 0);
- }
-
- /**
- * Set the speed of the drive motor from -1 to 1.
- *
- * @param speed Speed from -1 to 1.
- */
- @Override
- public void set(double speed)
- {
- m_motor.set(ControlMode.PercentOutput, speed);
- }
-
- /**
- * Set the voltage of the motor.
- *
- * @param voltage Voltage to output.
- */
- @Override
- public void setVoltage(double voltage)
- {
- m_motor.setVoltage(voltage);
- }
-
- /**
- * Get the current value of the encoder corresponding to the PID.
- *
- * @return Current value of the encoder.
- */
- @Override
- public double get()
- {
- return m_encoderRet.get();
- }
-
- /**
- * Get the current value of the encoder.
- *
- * @return Current value of the encoder.
- */
- @Override
- public double getPosition()
- {
- return m_motor.getSelectedSensorPosition();
- }
-
- /**
- * Set the voltage compensation for the swerve module motor.
- *
- * @param nominalVoltage Nominal voltage for operation to output to.
- */
- @Override
- public void setVoltageCompensation(double nominalVoltage)
- {
- m_motor.configVoltageCompSaturation(nominalVoltage);
- }
-
- /**
- * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
- * voltage compensation. This is useful to protect the motor from current spikes.
- *
- * @param currentLimit Current limit in AMPS at free speed.
- */
- @Override
- public void setCurrentLimit(int currentLimit)
- {
- m_motor.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, currentLimit, currentLimit, 1));
- }
-
- /**
- * Set the encoder value.
- *
- * @param value Value to set the encoder to.
- */
- @Override
- public void setEncoder(double value)
- {
- m_motor.setSelectedSensorPosition(value, m_mainPidId, 100);
- }
-
- /**
- * Check that the link is good on the swerve module and CAN bus is able to retrieve data.
- *
- * @return true on all devices are accessible over CAN.
- */
- @Override
- public boolean reachable()
- {
- return m_motor.getFirmwareVersion() != 0;
- }
-
- /**
- * Check if the absolute encoder is used inplace of the integrated encoder.
- *
- * @return true, if the absolute encoder is being used as the integrated controller.
- */
- @Override
- public boolean remoteIntegratedEncoder()
- {
- return m_integratedAbsEncoder;
- }
-
- /**
- * Optimize the CAN status frames to reduce utilization.
- */
- @Override
- public void optimizeCANFrames()
- {
-
- }
-
- /**
- * Save configuration data to the motor controller so it is persistent on reboot.
- */
- @Override
- public void saveConfig()
- {
- // Config is on the fly for falcons.
- }
-
- /**
- * Invert the motor.
- *
- * @param inverted Set the motor as inverted.
- */
- @Override
- public void setInverted(boolean inverted)
- {
- m_motor.setInverted(inverted);
- }
-
- @Override
- public double getAmps()
- {
- return m_motor.getStatorCurrent();
- }
-
- /**
- * The Talon SRX Slot profile used to configure the motor to use for the PID.
- */
- enum CTRE_slotIdx
- {
- Distance, Turning, Velocity, MotionProfile
- }
-
- /**
- * The TalonSRX PID to use onboard.
- */
- enum CTRE_pidIdx
- {
- PRIMARY_PID, AUXILIARY_PID, THIRD_PID, FOURTH_PID
- }
-
- enum CTRE_remoteSensor
- {
- REMOTE_SENSOR_0, REMOTE_SENSOR_1
- }
-
-// /**
-// * Set the PIDF coefficients for the closed loop PID onboard the TalonSRX.
-// *
-// * @param profile The {@link CTRE_slotIdx} to use.
-// * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor
-// units.
-// * Note the closed loop output interprets a final value of 1023 as full output. So use a gain
-// * of '0.25' to get full output if err is 4096u (Mag Encoder 1 rotation)
-// * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units
-// every
-// * PID Loop. Note the closed loop output interprets a final value of 1023 as full output. So
-// * use a gain of '0.00025' to get full output if err is 4096u (Mag Encoder 1 rotation) after
-// * 1000 loops
-// * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per
-// * PID loop). Note the closed loop output interprets a final value of 1023 as full output. So
-// * use a gain of '250' to get full output if derr is 4096u per (Mag Encoder 1 rotation) per
-// * 1000 loops (typ 1 sec)
-// * @param F Feed Fwd gain for Closed loop. See documentation for calculation details. If using
-// velocity,
-// * motion magic, or motion profile, use (1023 * duty-cycle /
-// * sensor-velocity-sensor-units-per-100ms)
-// * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too
-// * far from the target. This prevents unstable oscillation if the kI is too large. Value
-// is in
-// * sensor units. (ticks per 100ms)
-// * @param moduleMotorType Motor Type for swerve module.
-// */
-// private void setCTREPIDF(CTRE_slotIdx profile, double P, double I, double D, double F, double integralZone,
-// ModuleMotorType moduleMotorType)
-// {
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor
-// : m_turningMotor)).selectProfileSlot(
-// profile.ordinal(), CTRE_pidIdx.PRIMARY_PID.ordinal());
-// // More Closed-Loop Configs at
-// // https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#closed-loop-configs-per-slot-four-slots-available
-// // Example at
-// // https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20General/VelocityClosedLoop_ArbFeedForward/src/main/java/frc/robot/Robot.java
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kP(
-// profile.ordinal(), P);
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kI(
-// profile.ordinal(), I);
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kD(
-// profile.ordinal(), D);
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor : m_turningMotor)).config_kF(
-// profile.ordinal(), F);
-//
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor
-// : m_turningMotor)).config_IntegralZone(
-// profile.ordinal(), integralZone);
-//
-// // If the closed loop error is within this threshold, the motor output will be neutral. Set to 0 to disable.
-// // Value is in sensor units.
-//
-// ((BaseTalon) (moduleMotorType == ModuleMotorType.DRIVE ? m_driveMotor
-// : m_turningMotor)).configAllowableClosedloopError(
-// profile.ordinal(), 0);
-//
-// }
-//
-// /**
-// * Set the angle using the onboard controller when working with CTRE Talons
-// *
-// * @param angle Angle in degrees
-// */
-// private void setCTREAngle(double angle)
-// {
-// ((BaseTalon) m_turningMotor).set(ControlMode.Position, angle);
-// // TODO: Pass feedforward down.
-// }
-//
-// /**
-// * Set the velocity of the drive motor.
-// *
-// * @param velocity Velocity in meters per second.
-// */
-// private void setCTREDrive(double velocity)
-// {
-// ((BaseTalon) m_driveMotor).set(ControlMode.Velocity, driveFeedforward.calculate(velocity));
-// }
-//
-// /**
-// * Set up the CTRE motors and configure class attributes correspondingly
-// *
-// * @param motor Motor controller to configure.
-// * @param moduleMotorType Motor type to configure
-// * @param gearRatio Gear ratio of the motor for one revolution.
-// */
-// private void setupCTREMotor(BaseTalon motor, ModuleMotorType moduleMotorType, double gearRatio)
-// {
-//
-// // Purposely did not configure status frames since CTRE motors should be on a CANivore
-//
-// motor.setSensorPhase(true);
-// motor.setNeutralMode(NeutralMode.Brake);
-// // Unable to use TalonFX configs since this should support both TalonSRX's and TalonFX's
-// setVoltageCompensation(12, moduleMotorType);
-// // Code is based off of.
-// // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500SteerControllerFactoryBuilder.java#L91
-// // and here
-// // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L52
-//
-// if (moduleMotorType == ModuleMotorType.DRIVE)
-// {
-// setCurrentLimit(80, moduleMotorType);
-// // Math set's the coefficient to the OUTPUT of the ENCODER (ticks/100ms) which is the INPUT to the PID.
-// // We want to set the PID to use MPS == meters/second :)
-// // Dimensional analysis, solve for K
-// // ticks/100ms * K = meters/second
-// // ticks/100ms * 100ms/(1s=1000ms) * (pi*diameter)meters/(ticks[4096]*gearRatio)ticks = meters/second
-// // ticks/100ms * 1/10 * (pi*diameter)/(ticks[4096]*gearRatio)ticks = meters/second
-// // ticks/100ms * (pi*diameter)/((ticks[4096]*gearRatio)*10) = meters/second
-// // K = (pi*diameter)/((ticks[4096]*gearRatio)*10)
-// // Set the feedback sensor up earlier in setCANRemoteFeedbackSensor()
-// motor.configSelectedFeedbackCoefficient(((Math.PI * wheelDiameter) / ((4096 / gearRatio)) * 10));
-// } else
-// {
-// setCurrentLimit(20, moduleMotorType);
-// setPIDF(0.2, 0, 0.1, 0, 100, moduleMotorType);
-// }
-// }
-//
-// /**
-// * Set the CANCoder to be the primary PID on the motor controller and configure the PID to accept inputs in degrees.
-// * The talon will communicate independently of the roboRIO to fetch the current CANCoder position (which will result
-// * in PID adjustments when using a CANivore).
-// *
-// * @param motor Talon Motor controller to configure.
-// * @param encoder CANCoder to use as the remote sensor.
-// */
-// private void setupCTRECANCoderRemoteSensor(BaseTalon motor, CANCoder encoder)
-// {
-// motor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0);
-// motor.configRemoteFeedbackFilter(encoder, CTRE_remoteSensor.REMOTE_SENSOR_0.ordinal());
-// motor.configSelectedFeedbackCoefficient((double) 360 / 4096); // Degrees/Ticks
-// // The CANCoder has 4096 ticks per 1 revolution.
-// }
-//
-// /**
-// * Returns whether the turning motor is a CTRE motor.
-// *
-// * @return is the turning motor a CTRE motor?
-// */
-// private boolean isCTRETurningMotor()
-// {
-// return m_turningMotor instanceof BaseMotorController;
-// }
-//
-// /**
-// * Returns whether the drive motor is a CTRE motor. All CTRE motors implement the {@link BaseMotorController} class.
-// * We will only support the TalonSRX and TalonFX.
-// *
-// * @return is the drive motor a CTRE motor?
-// */
-// private boolean isCTREDriveMotor()
-// {
-// return m_driveMotor instanceof TalonFX || m_driveMotor instanceof TalonSRX;
-// }
-
-}
diff --git a/swervelib/motors/REVSwerveMotor.java b/swervelib/motors/REVSwerveMotor.java
deleted file mode 100644
index a088a15..0000000
--- a/swervelib/motors/REVSwerveMotor.java
+++ /dev/null
@@ -1,406 +0,0 @@
-package frc.robot.subsystems.swervedrive.swerve.motors;
-
-import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.ControlType;
-import com.revrobotics.CANSparkMax.IdleMode;
-import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
-import com.revrobotics.MotorFeedbackSensor;
-import com.revrobotics.REVPhysicsSim;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkMaxPIDController;
-import com.revrobotics.SparkMaxPIDController.ArbFFUnits;
-import frc.robot.Robot;
-import frc.robot.subsystems.swervedrive.swerve.SwerveEncoder;
-import frc.robot.subsystems.swervedrive.swerve.SwerveMotor;
-import java.util.function.Supplier;
-
-public class REVSwerveMotor extends SwerveMotor
-{
-
- private final int m_mainPidSlot;
- private final int m_secondaryPidSlot;
- private final ControlType m_pidControlType;
- private final CANSparkMax m_motor;
- private final MotorFeedbackSensor m_encoder;
- private final boolean m_integratedAbsEncoder;
- private final SparkMaxPIDController m_pid;
- private final ArbFFUnits m_feedforwardUnits;
- private final Supplier m_encoderRet, m_encoderPosRet;
- /**
- * kV feed forward for PID
- */
- private final double m_moduleRadkV;
-
- /**
- * Constructor for REV Swerve Motor, expecting CANSparkMax. Clears sticky faults and restores factory defaults.
- *
- * @param motor SparkMAX motor controller.
- * @param absoluteEncoder The absolute encoder used for the module, if the motor is a turning motor and the encoder is
- * compatible it will set the encoder as the remote integrated encoder and does not need
- * periodic synchronization.
- * @param type Swerve module motor type.
- * @param gearRatio Gear ratio.
- * @param wheelDiameter Wheel diameter in meters.
- * @param freeSpeedRPM Free speed RPM of the motor.
- * @param powerLimit Power limit for the motor.
- */
- public REVSwerveMotor(CANSparkMax motor, SwerveEncoder absoluteEncoder, ModuleMotorType type, double gearRatio,
- double wheelDiameter, double freeSpeedRPM, double powerLimit)
- {
- m_integratedAbsEncoder = absoluteEncoder.m_encoder instanceof AbsoluteEncoder && type == ModuleMotorType.TURNING;
-
- // Inspired by the following sources
- // https://github.com/SwerveDriveSpecialties/swerve-lib-2022-unmaintained/blob/55f3f1ad9e6bd81e56779d022a40917aacf8d3b3/src/main/java/com/swervedrivespecialties/swervelib/rev/NeoDriveControllerFactoryBuilder.java#L38
- // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L68
- // https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/Constants.java#L89
- // https://github.com/AusTINCANsProgrammingTeam/2022Swerve/blob/main/2022Swerve/src/main/java/frc/robot/Constants.java
- motor.restoreFactoryDefaults();
-
- m_motor = motor;
- m_motorType = type;
- m_encoder = m_integratedAbsEncoder ? (AbsoluteEncoder) absoluteEncoder.m_encoder : motor.getEncoder();
- m_pid = motor.getPIDController();
-
- m_pid.setFeedbackDevice(m_encoder);
-
- motor.clearFaults();
- motor.setIdleMode(IdleMode.kBrake);
-
-// double powerDriving = .6, powerTurning = .4; // TODO: Change from magic numbers.
-
- if (type == ModuleMotorType.DRIVE)
- {
- m_moduleRadkV = 1;
-
- m_encoderRet = ((RelativeEncoder) m_encoder)::getVelocity;
- m_encoderPosRet = ((RelativeEncoder) m_encoder)::getPosition;
- m_feedforwardUnits = ArbFFUnits.kPercentOut;
- m_pidControlType = ControlType.kVelocity;
- m_mainPidSlot = REV_slotIdx.Velocity.ordinal();
- m_secondaryPidSlot = REV_slotIdx.Position.ordinal();
-
-// setPIDF(0.1, 0, 0, 0, 1);
- setPIDF(0.01, 0, 0.005, 0, 0);
-
- setConversionFactor(((Math.PI * wheelDiameter) / gearRatio) / 60);
-
- setCurrentLimit(50);
- } else
- {
- m_moduleRadkV = (12 * 60) / (freeSpeedRPM * Math.toRadians(360 / (m_integratedAbsEncoder ? 1 : gearRatio)));
-
- if (m_integratedAbsEncoder)
- {
- m_encoderPosRet = m_encoderRet = ((AbsoluteEncoder) m_encoder)::getPosition;
- } else
- {
- m_encoderPosRet = m_encoderRet = ((RelativeEncoder) m_encoder)::getPosition;
- }
-
- m_feedforwardUnits = ArbFFUnits.kVoltage;
- m_pidControlType = ControlType.kPosition;
- m_mainPidSlot = REV_slotIdx.Position.ordinal();
- m_secondaryPidSlot = REV_slotIdx.Velocity.ordinal();
-
- setPIDF(0.01, 0, 0.005, 0, 0);
-
- setConversionFactor(m_integratedAbsEncoder ? 360 : 360 / gearRatio);
-
- m_pid.setPositionPIDWrappingEnabled(true);
- m_pid.setPositionPIDWrappingMinInput(-180);
- m_pid.setPositionPIDWrappingMaxInput(180);
-
- setCurrentLimit(20);
- }
-
- setPIDOutputRange(-powerLimit, powerLimit);
- setVoltageCompensation(12);
-
- setEncoder(0);
-
- optimizeCANFrames();
-
- m_motor.setCANTimeout(0); // Spin off configurations in a different thread.
- if (!Robot.isReal())
- {
- REVPhysicsSim.getInstance().addSparkMax(m_motor, 2.6f, 5676);
- }
- }
-
- /**
- * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base.
- *
- * @param min Minimum output.
- * @param max Maximum output.
- */
- @Override
- public void setPIDOutputRange(double min, double max)
- {
- m_pid.setOutputRange(min, max, m_mainPidSlot);
- m_pid.setOutputRange(min, max, m_secondaryPidSlot);
- }
-
- /**
- * Set the PIDF coefficients for the closed loop PID onboard the SparkMax.
- *
- * @param P Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
- * Default is 1.0
- * @param I Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
- * PID Loop.
- * @param D Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID
- * loop). Default is 0.1
- * @param F Feed Fwd gain for Closed loop.
- * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far
- * from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor
- * units.
- */
- @Override
- public void setPIDF(double P, double I, double D, double F, double integralZone)
- {
- // Example at
- // https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Velocity%20Closed%20Loop%20Control/src/main/java/frc/robot/Robot.java#L65-L71
- kP = P;
- kI = I;
- kD = D;
- kF = F;
- kIZ = integralZone;
- m_pid.setP(P, m_mainPidSlot);
- m_pid.setI(I, m_mainPidSlot);
- m_pid.setD(D, m_mainPidSlot);
- m_pid.setFF(F, m_mainPidSlot);
- m_pid.setIZone(integralZone, m_mainPidSlot);
- // m_pid.setOutputRange(-1, 1, m_mainPidSlot);
- }
-
- /**
- * Configures the conversion factor based upon which motor.
- *
- * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning
- * motor.
- */
- @Override
- public void setConversionFactor(double conversionFactor)
- {
- if (m_motorType == ModuleMotorType.TURNING)
- {
- if (m_encoder instanceof AbsoluteEncoder)
- {
- ((AbsoluteEncoder) m_encoder).setPositionConversionFactor(conversionFactor);
- ((AbsoluteEncoder) m_encoder).setVelocityConversionFactor(conversionFactor / 60);
- } else
- {
- ((RelativeEncoder) m_encoder).setPositionConversionFactor(conversionFactor);
- ((RelativeEncoder) m_encoder).setVelocityConversionFactor(conversionFactor / 60);
- }
- } else
- {
- ((RelativeEncoder) m_encoder).setVelocityConversionFactor(conversionFactor);
- ((RelativeEncoder) m_encoder).setPositionConversionFactor(
- conversionFactor * 60 * (Robot.isReal() ? 1 : 42 * 60)); // RPS -> RPM sim
- // SparkMaxSimProfile assumes the velocity is in RPM and multiplies it by 60, in our use case velocity is in RPS
- // The Sim profile also neglects to take into account that there are 42 ticks per rotation.
- }
- }
-
- /**
- * Set the target for the PID loop.
- *
- * @param target The PID target to aim for.
- * @param feedforward feedForward value.
- */
- @Override
- public void setTarget(double target, double feedforward)
- {
- this.target = target;
- m_pid.setReference(target, m_pidControlType, m_mainPidSlot, feedforward * m_moduleRadkV, m_feedforwardUnits);
- }
-
- /**
- * Stop the motor by sending 0 volts to it.
- */
- @Override
- public void stop()
- {
- m_motor.set(0);
- }
-
- /**
- * Set the speed of the drive motor from -1 to 1.
- *
- * @param speed Speed from -1 to 1.
- */
- @Override
- public void set(double speed)
- {
- m_motor.set(speed);
- }
-
- /**
- * Set the voltage of the motor.
- *
- * @param voltage Voltage to output.
- */
- @Override
- public void setVoltage(double voltage)
- {
- m_motor.setVoltage(voltage);
- }
-
- /**
- * Get the current value of the encoder corresponding to the PID.
- *
- * @return Current value of the encoder.
- */
- @Override
- public double get()
- {
- return m_encoderRet.get();
- }
-
- /**
- * Get the current output.
- *
- * @return Output amps.
- */
- @Override
- public double getAmps()
- {
- return m_motor.getOutputCurrent();
- }
-
- /**
- * Get the current value of the encoder.
- *
- * @return Current value of the encoder.
- */
- @Override
- public double getPosition()
- {
- return m_encoderPosRet.get();
- }
-
- /**
- * Set the voltage compensation for the swerve module motor.
- *
- * @param nominalVoltage Nominal voltage for operation to output to.
- */
- @Override
- public void setVoltageCompensation(double nominalVoltage)
- {
- m_motor.enableVoltageCompensation(nominalVoltage);
- }
-
- /**
- * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
- * voltage compensation. This is useful to protect the motor from current spikes.
- *
- * @param currentLimit Current limit in AMPS at free speed.
- */
- @Override
- public void setCurrentLimit(int currentLimit)
- {
- m_motor.setSmartCurrentLimit(currentLimit);
- }
-
- /**
- * Set the encoder value.
- *
- * @param value Value to set the encoder to.
- */
- @Override
- public void setEncoder(double value)
- {
- if (m_encoder instanceof RelativeEncoder)
- {
- ((RelativeEncoder) m_encoder).setPosition(value);
- }
- }
-
- /**
- * Set the CAN status frames.
- *
- * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
- * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
- * @param CANStatus2 Motor Position
- * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
- * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
- */
- public void setCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
- {
- m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
- m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
- m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2);
- m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3);
- m_motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4);
- // TODO: Configure Status Frame 5 and 6 if necessary
- // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
- }
-
- /**
- * Check that the link is good on the swerve module and CAN bus is able to retrieve data.
- *
- * @return true on all devices are accessible over CAN.
- */
- @Override
- public boolean reachable()
- {
- // Based off of https://github.com/DigitalDislocators/SDS-MK4i-NEO-Swerve-Template/blob/main/src/main/java/frc/robot/subsystems/SwerveModuleSparkMax.java#L490
- return m_motor.getFirmwareVersion() != 0;
- }
-
- /**
- * Check if the absolute encoder is used inplace of the integrated encoder.
- *
- * @return true, if the absolute encoder is being used as the integrated controller.
- */
- @Override
- public boolean remoteIntegratedEncoder()
- {
- return m_integratedAbsEncoder;
- }
-
- /**
- * Optimize the CAN status frames to reduce utilization.
- */
- @Override
- public void optimizeCANFrames()
- {
- // Taken from https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
- if (m_motorType == ModuleMotorType.DRIVE)
- {
- setCANStatusFrames(10, 20, 500, 500, 500);
- } else
- {
- setCANStatusFrames(10, 500, 20, 500, 500);
- }
- }
-
- /**
- * Save configuration data to the motor controller so it is persistent on reboot.
- */
- @Override
- public void saveConfig()
- {
- m_motor.burnFlash();
- }
-
- /**
- * Invert the motor.
- *
- * @param inverted Set the motor as inverted.
- */
- @Override
- public void setInverted(boolean inverted)
- {
- m_motor.setInverted(inverted);
- }
-
- /**
- * REV Slots for PID configuration.
- */
- enum REV_slotIdx
- {
- Position, Velocity, Simulation
- }
-}
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
new file mode 100644
index 0000000..b78e9de
--- /dev/null
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -0,0 +1,340 @@
+package frc.robot.subsystems.swervedrive2.swervelib.motors;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+
+/**
+ * An implementation of {@link CANSparkMax} as a {@link SwerveMotor}.
+ */
+public class SparkMaxSwerve extends SwerveMotor
+{
+
+ /**
+ * SparkMAX Instance.
+ */
+ public CANSparkMax motor;
+ /**
+ * Integrated encoder.
+ */
+ public RelativeEncoder encoder;
+ /**
+ * Absolute encoder attached to the SparkMax (if exists)
+ */
+ public AbsoluteEncoder absoluteEncoder;
+ /**
+ * Closed-loop PID controller.
+ */
+ public SparkMaxPIDController pid;
+ /**
+ * Factory default already occurred.
+ */
+ private boolean factoryDefaultOccurred = false;
+
+
+ /**
+ * Initialize the swerve motor.
+ *
+ * @param motor The SwerveMotor as a SparkMax object.
+ * @param isDriveMotor Is the motor being initialized a drive motor?
+ */
+ public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor)
+ {
+ this.motor = motor;
+ this.isDriveMotor = isDriveMotor;
+ factoryDefaults();
+ clearStickyFaults();
+
+ encoder = motor.getEncoder();
+ pid = motor.getPIDController();
+ pid.setFeedbackDevice(encoder); // Configure feedback of the PID controller as the integrated encoder.
+
+ motor.setCANTimeout(0); // Spin off configurations in a different thread.
+ }
+
+ /**
+ * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
+ *
+ * @param id CAN ID of the SparkMax.
+ * @param isDriveMotor Is the motor being initialized a drive motor?
+ */
+ public SparkMaxSwerve(int id, boolean isDriveMotor)
+ {
+ this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor);
+ }
+
+ /**
+ * Set the voltage compensation for the swerve module motor.
+ *
+ * @param nominalVoltage Nominal voltage for operation to output to.
+ */
+ @Override
+ public void setVoltageCompensation(double nominalVoltage)
+ {
+ motor.enableVoltageCompensation(nominalVoltage);
+ }
+
+ /**
+ * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+ * voltage compensation. This is useful to protect the motor from current spikes.
+ *
+ * @param currentLimit Current limit in AMPS at free speed.
+ */
+ @Override
+ public void setCurrentLimit(int currentLimit)
+ {
+ motor.setSmartCurrentLimit(currentLimit);
+ }
+
+ /**
+ * Set the maximum rate the open/closed loop output can change by.
+ *
+ * @param rampRate Time in seconds to go from 0 to full throttle.
+ */
+ @Override
+ public void setLoopRampRate(double rampRate)
+ {
+ motor.setOpenLoopRampRate(rampRate);
+ motor.setClosedLoopRampRate(rampRate);
+ }
+
+ /**
+ * Get the motor object from the module.
+ *
+ * @return Motor object.
+ */
+ @Override
+ public Object getMotor()
+ {
+ return motor;
+ }
+
+ /**
+ * Queries whether the absolute encoder is directly attached to the motor controller.
+ *
+ * @return connected absolute encoder state.
+ */
+ @Override
+ public boolean isAttachedAbsoluteEncoder()
+ {
+ return absoluteEncoder != null;
+ }
+
+ /**
+ * Configure the factory defaults.
+ */
+ @Override
+ public void factoryDefaults()
+ {
+ if (!factoryDefaultOccurred)
+ {
+ motor.restoreFactoryDefaults();
+ factoryDefaultOccurred = true;
+ }
+ }
+
+ /**
+ * Clear the sticky faults on the motor controller.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ motor.clearFaults();
+ }
+
+ /**
+ * Set the absolute encoder to be a compatible absolute encoder.
+ *
+ * @param encoder The encoder to use.
+ * @return The {@link SwerveMotor} for easy instantiation.
+ */
+ @Override
+ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
+ {
+ if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
+ pid.setFeedbackDevice(absoluteEncoder);
+ }
+ return this;
+ }
+
+ /**
+ * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
+ *
+ * @param positionConversionFactor The conversion factor to apply.
+ */
+ @Override
+ public void configureIntegratedEncoder(double positionConversionFactor)
+ {
+ if (absoluteEncoder == null)
+ {
+ encoder.setPositionConversionFactor(positionConversionFactor);
+ encoder.setVelocityConversionFactor(positionConversionFactor / 60);
+
+ // Taken from https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+ configureCANStatusFrames(10, 20, 20, 500, 500);
+ } else
+ {
+ absoluteEncoder.setPositionConversionFactor(positionConversionFactor);
+ absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60);
+ }
+ }
+
+ /**
+ * Configure the PIDF values for the closed loop controller.
+ *
+ * @param config Configuration class holding the PIDF values.
+ */
+ @Override
+ public void configurePIDF(PIDFConfig config)
+ {
+ int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ pid.setP(config.p, pidSlot);
+ pid.setI(config.i, pidSlot);
+ pid.setD(config.d, pidSlot);
+ pid.setFF(config.f, pidSlot);
+ pid.setIZone(config.iz, pidSlot);
+ pid.setOutputRange(config.output.min, config.output.max, pidSlot);
+ }
+
+ /**
+ * Configure the PID wrapping for the position closed loop controller.
+ *
+ * @param minInput Minimum PID input.
+ * @param maxInput Maximum PID input.
+ */
+ @Override
+ public void configurePIDWrapping(double minInput, double maxInput)
+ {
+ pid.setPositionPIDWrappingEnabled(true);
+ pid.setPositionPIDWrappingMinInput(minInput);
+ pid.setPositionPIDWrappingMaxInput(maxInput);
+ }
+
+ /**
+ * Set the CAN status frames.
+ *
+ * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
+ * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
+ * @param CANStatus2 Motor Position
+ * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
+ * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
+ */
+ public void configureCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
+ {
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4);
+ // TODO: Configure Status Frame 5 and 6 if necessary
+ // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
+ }
+
+ /**
+ * Set the idle mode.
+ *
+ * @param isBrakeMode Set the brake mode.
+ */
+ @Override
+ public void setMotorBrake(boolean isBrakeMode)
+ {
+ motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+
+ /**
+ * Set the motor to be inverted.
+ *
+ * @param inverted State of inversion.
+ */
+ @Override
+ public void setInverted(boolean inverted)
+ {
+ motor.setInverted(inverted);
+ }
+
+ /**
+ * Save the configurations from flash to EEPROM.
+ */
+ @Override
+ public void burnFlash()
+ {
+ motor.burnFlash();
+ }
+
+ /**
+ * Set the percentage output.
+ *
+ * @param percentOutput percent out for the motor controller.
+ */
+ @Override
+ public void set(double percentOutput)
+ {
+ motor.set(percentOutput);
+ }
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in MPS or Angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward)
+ {
+ int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ pid.setReference(setpoint, isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, pidSlot, feedforward);
+ }
+
+ /**
+ * Get the velocity of the integrated encoder.
+ *
+ * @return velocity
+ */
+ @Override
+ public double getVelocity()
+ {
+ return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+ }
+
+ /**
+ * Get the position of the integrated encoder.
+ *
+ * @return Position
+ */
+ @Override
+ public double getPosition()
+ {
+ return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition();
+ }
+
+ /**
+ * Set the integrated encoder position.
+ *
+ * @param position Integrated encoder position.
+ */
+ @Override
+ public void setPosition(double position)
+ {
+ if (absoluteEncoder == null)
+ {
+ encoder.setPosition(position);
+ }
+ }
+
+ /**
+ * REV Slots for PID configuration.
+ */
+ enum SparkMAX_slotIdx
+ {
+ Position, Velocity, Simulation
+ }
+}
diff --git a/swervelib/motors/SwerveMotor.java b/swervelib/motors/SwerveMotor.java
new file mode 100644
index 0000000..fd014f5
--- /dev/null
+++ b/swervelib/motors/SwerveMotor.java
@@ -0,0 +1,146 @@
+package frc.robot.subsystems.swervedrive2.swervelib.motors;
+
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+
+/**
+ * Swerve motor abstraction which defines a standard interface for motors within a swerve module.
+ */
+public abstract class SwerveMotor
+{
+
+ /**
+ * Whether the swerve motor is a drive motor.
+ */
+ protected boolean isDriveMotor;
+
+ /**
+ * Configure the factory defaults.
+ */
+ public abstract void factoryDefaults();
+
+ /**
+ * Clear the sticky faults on the motor controller.
+ */
+ public abstract void clearStickyFaults();
+
+ /**
+ * Set the absolute encoder to be a compatible absolute encoder.
+ *
+ * @param encoder The encoder to use.
+ */
+ public abstract SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder);
+
+ /**
+ * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
+ *
+ * @param positionConversionFactor The conversion factor to apply for position.
+ */
+ public abstract void configureIntegratedEncoder(double positionConversionFactor);
+
+ /**
+ * Configure the PIDF values for the closed loop controller. 0 is disabled or off.
+ *
+ * @param config Configuration class holding the PIDF values.
+ */
+ public abstract void configurePIDF(PIDFConfig config);
+
+ /**
+ * Configure the PID wrapping for the position closed loop controller.
+ *
+ * @param minInput Minimum PID input.
+ * @param maxInput Maximum PID input.
+ */
+ public abstract void configurePIDWrapping(double minInput, double maxInput);
+
+ /**
+ * Set the idle mode.
+ *
+ * @param isBrakeMode Set the brake mode.
+ */
+ public abstract void setMotorBrake(boolean isBrakeMode);
+
+ /**
+ * Set the motor to be inverted.
+ *
+ * @param inverted State of inversion.
+ */
+ public abstract void setInverted(boolean inverted);
+
+ /**
+ * Save the configurations from flash to EEPROM.
+ */
+ public abstract void burnFlash();
+
+ /**
+ * Set the percentage output.
+ *
+ * @param percentOutput percent out for the motor controller.
+ */
+ public abstract void set(double percentOutput);
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in MPS or Angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ */
+ public abstract void setReference(double setpoint, double feedforward);
+
+ /**
+ * Get the velocity of the integrated encoder.
+ *
+ * @return velocity
+ */
+ public abstract double getVelocity();
+
+ /**
+ * Get the position of the integrated encoder.
+ *
+ * @return Position
+ */
+ public abstract double getPosition();
+
+ /**
+ * Set the integrated encoder position.
+ *
+ * @param position Integrated encoder position. Should be angle in degrees or meters per second.
+ */
+ public abstract void setPosition(double position);
+
+ /**
+ * Set the voltage compensation for the swerve module motor.
+ *
+ * @param nominalVoltage Nominal voltage for operation to output to.
+ */
+ public abstract void setVoltageCompensation(double nominalVoltage);
+
+ /**
+ * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+ * voltage compensation. This is useful to protect the motor from current spikes.
+ *
+ * @param currentLimit Current limit in AMPS at free speed.
+ */
+ public abstract void setCurrentLimit(int currentLimit);
+
+ /**
+ * Set the maximum rate the open/closed loop output can change by.
+ *
+ * @param rampRate Time in seconds to go from 0 to full throttle.
+ */
+ public abstract void setLoopRampRate(double rampRate);
+
+ /**
+ * Get the motor object from the module.
+ *
+ * @return Motor object.
+ */
+ public abstract Object getMotor();
+
+ /**
+ * Queries whether the absolute encoder is directly attached to the motor controller.
+ *
+ * @return connected absolute encoder state.
+ */
+ public abstract boolean isAttachedAbsoluteEncoder();
+}
diff --git a/swervelib/parser/PIDFConfig.java b/swervelib/parser/PIDFConfig.java
new file mode 100644
index 0000000..49e2c90
--- /dev/null
+++ b/swervelib/parser/PIDFConfig.java
@@ -0,0 +1,109 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.deserializer.PIDFRange;
+
+/**
+ * Hold the PIDF and Integral Zone values for a PID.
+ */
+public class PIDFConfig
+{
+
+ /**
+ * Proportional Gain for PID.
+ */
+ public double p;
+ /**
+ * Integral Gain for PID.
+ */
+ public double i;
+ /**
+ * Derivative Gain for PID.
+ */
+ public double d;
+ /**
+ * Feedforward value for PID.
+ */
+ public double f;
+ /**
+ * Integral zone of the PID.
+ */
+ public double iz;
+
+ /**
+ * The PIDF output range.
+ */
+ public PIDFRange output = new PIDFRange();
+
+ /**
+ * Used when parsing PIDF values from JSON.
+ */
+ public PIDFConfig()
+ {
+ }
+
+ /**
+ * PIDF Config constructor to contain the values.
+ *
+ * @param p P gain.
+ * @param i I gain.
+ * @param d D gain.
+ * @param f F gain.
+ * @param iz Intergral zone.
+ */
+ public PIDFConfig(double p, double i, double d, double f, double iz)
+ {
+ this.p = p;
+ this.i = i;
+ this.d = d;
+ this.f = f;
+ this.iz = iz;
+ }
+
+ /**
+ * PIDF Config constructor to contain the values.
+ *
+ * @param p P gain.
+ * @param i I gain.
+ * @param d D gain.
+ * @param f F gain.
+ */
+ public PIDFConfig(double p, double i, double d, double f)
+ {
+ this(p, i, d, f, 0);
+ }
+
+ /**
+ * PIDF Config constructor to contain the values.
+ *
+ * @param p P gain.
+ * @param i I gain.
+ * @param d D gain.
+ */
+ public PIDFConfig(double p, double i, double d)
+ {
+ this(p, i, d, 0, 0);
+ }
+
+ /**
+ * PIDF Config constructor to contain the values.
+ *
+ * @param p P gain.
+ * @param d D gain.
+ */
+ public PIDFConfig(double p, double d)
+ {
+ this(p, 0, d, 0, 0);
+ }
+
+ /**
+ * Create a PIDController from the PID values.
+ *
+ * @return PIDController.
+ */
+ public PIDController createPIDController()
+ {
+ return new PIDController(p, i, d);
+ }
+}
+
diff --git a/swervelib/parser/SwerveControllerConfiguration.java b/swervelib/parser/SwerveControllerConfiguration.java
new file mode 100644
index 0000000..4e78c6a
--- /dev/null
+++ b/swervelib/parser/SwerveControllerConfiguration.java
@@ -0,0 +1,59 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateMaxAngularVelocity;
+
+/**
+ * Swerve Controller configuration class which is used to configure
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveController}.
+ */
+public class SwerveControllerConfiguration
+{
+
+ /**
+ * Maximum robot speed in meters per second.
+ */
+ public final double maxSpeed;
+ /**
+ * Maximum angular velocity in rad/s
+ */
+ public final double maxAngularVelocity;
+ /**
+ * PIDF for the heading of the robot.
+ */
+ public final PIDFConfig headingPIDF;
+ /**
+ * hypotenuse deadband for the robot angle control joystick.
+ */
+ public final double angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
+
+ /**
+ * Construct the swerve controller configuration.
+ *
+ * @param driveCfg Drive configuration.
+ * @param headingPIDF Heading PIDF configuration.
+ * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick.
+ */
+ public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF,
+ double angleJoyStickRadiusDeadband)
+ {
+ this.maxSpeed = driveCfg.maxSpeed;
+ this.maxAngularVelocity = calculateMaxAngularVelocity(driveCfg.maxSpeed,
+ Math.abs(driveCfg.moduleLocationsMeters[0].getX()),
+ Math.abs(driveCfg.moduleLocationsMeters[0].getY()));
+ this.headingPIDF = headingPIDF;
+ this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband;
+ }
+
+ /**
+ * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum radius for angle to be
+ * set on angle joystick is .5 of the controller).
+ *
+ * @param driveCfg Drive configuration.
+ * @param headingPIDF Heading PIDF configuration.
+ */
+ public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF)
+ {
+ this(driveCfg, headingPIDF, 0.5);
+ }
+
+}
diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java
new file mode 100644
index 0000000..74717d1
--- /dev/null
+++ b/swervelib/parser/SwerveDriveConfiguration.java
@@ -0,0 +1,76 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveModule;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.SwerveIMU;
+
+/**
+ * Swerve drive configurations used during SwerveDrive construction.
+ */
+public class SwerveDriveConfiguration
+{
+
+ /**
+ * Swerve Module locations.
+ */
+ public Translation2d[] moduleLocationsMeters;
+ /**
+ * Swerve IMU
+ */
+ public SwerveIMU imu;
+ /**
+ * Invert the imu measurements.
+ */
+ public boolean invertedIMU = false;
+ /**
+ * Max speed in meters per second.
+ */
+ public double maxSpeed;
+ /**
+ * Number of modules on the robot.
+ */
+ public int moduleCount;
+ /**
+ * Swerve Modules.
+ */
+ public SwerveModule[] modules;
+
+ /**
+ * Create swerve drive configuration.
+ *
+ * @param moduleConfigs Module configuration.
+ * @param swerveIMU Swerve IMU.
+ * @param maxSpeed Max speed of the robot in meters per second.
+ * @param invertedIMU Invert the IMU.
+ */
+ public SwerveDriveConfiguration(SwerveModuleConfiguration[] moduleConfigs, SwerveIMU swerveIMU, double maxSpeed,
+ boolean invertedIMU)
+ {
+ this.moduleCount = moduleConfigs.length;
+ this.imu = swerveIMU;
+ this.maxSpeed = maxSpeed;
+ this.invertedIMU = invertedIMU;
+ this.modules = createModules(moduleConfigs);
+ this.moduleLocationsMeters = new Translation2d[moduleConfigs.length];
+ for (SwerveModule module : modules)
+ {
+ this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation;
+ }
+ }
+
+ /**
+ * Create modules based off of the SwerveModuleConfiguration.
+ *
+ * @param swerves Swerve constants.
+ * @return Swerve Modules.
+ */
+ public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves)
+ {
+ SwerveModule[] modArr = new SwerveModule[swerves.length];
+ for (int i = 0; i < swerves.length; i++)
+ {
+ modArr[i] = new SwerveModule(i, swerves[i]);
+ }
+ return modArr;
+ }
+}
diff --git a/swervelib/parser/SwerveModuleConfiguration.java b/swervelib/parser/SwerveModuleConfiguration.java
new file mode 100644
index 0000000..7f670e9
--- /dev/null
+++ b/swervelib/parser/SwerveModuleConfiguration.java
@@ -0,0 +1,179 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateAngleKV;
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation;
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateMaxAcceleration;
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateMetersPerRotation;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+
+/**
+ * Swerve Module configuration class which is used to configure
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveModule}.
+ */
+public class SwerveModuleConfiguration
+{
+
+ /**
+ * Angle offset in degrees for the Swerve Module.
+ */
+ public final double angleOffset;
+ /**
+ * Whether the absolute encoder is inverted.
+ */
+ public final boolean absoluteEncoderInverted;
+ /**
+ * State of inversion of the drive motor.
+ */
+ public final boolean driveMotorInverted;
+ /**
+ * State of inversion of the angle motor.
+ */
+ public final boolean angleMotorInverted;
+ /**
+ * Maximum robot speed in meters per second.
+ */
+ public final double maxSpeed;
+ /**
+ * PIDF configuration options for the angle motor closed-loop PID controller.
+ */
+ public PIDFConfig anglePIDF;
+ /**
+ * PIDF configuration options for the drive motor closed-loop PID controller.
+ */
+ public PIDFConfig velocityPIDF;
+ /**
+ * Angle volt-meter-per-second.
+ */
+ public double angleKV;
+ /**
+ * Swerve module location relative to the robot.
+ */
+ public Translation2d moduleLocation;
+ /**
+ * Physical characteristics of the swerve module.
+ */
+ public SwerveModulePhysicalCharacteristics physicalCharacteristics;
+ /**
+ * The drive motor and angle motor of this swerve module.
+ */
+ public SwerveMotor driveMotor, angleMotor;
+ /**
+ * The Absolute Encoder for the swerve module.
+ */
+ public SwerveAbsoluteEncoder absoluteEncoder;
+
+
+ /**
+ * Construct a configuration object for swerve modules.
+ *
+ * @param driveMotor Drive {@link SwerveMotor}.
+ * @param angleMotor Angle {@link SwerveMotor}
+ * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}.
+ * @param angleOffset Absolute angle offset to 0.
+ * @param absoluteEncoderInverted Absolute encoder inverted.
+ * @param angleMotorInverted State of inversion of the angle motor.
+ * @param driveMotorInverted Drive motor inverted.
+ * @param xMeters Module location in meters from the center horizontally.
+ * @param yMeters Module location in meters from center vertically.
+ * @param anglePIDF Angle PIDF configuration.
+ * @param velocityPIDF Velocity PIDF configuration.
+ * @param maxSpeed Maximum speed in meters per second.
+ * @param physicalCharacteristics Physical characteristics of the swerve module.
+ */
+ public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
+ SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
+ double xMeters,
+ double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, double maxSpeed,
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ boolean absoluteEncoderInverted, boolean driveMotorInverted,
+ boolean angleMotorInverted)
+ {
+ this.driveMotor = driveMotor;
+ this.angleMotor = angleMotor;
+ this.absoluteEncoder = absoluteEncoder;
+ this.angleOffset = angleOffset;
+ this.absoluteEncoderInverted = absoluteEncoderInverted;
+ this.driveMotorInverted = driveMotorInverted;
+ this.angleMotorInverted = angleMotorInverted;
+ this.moduleLocation = new Translation2d(xMeters, yMeters);
+ this.anglePIDF = anglePIDF;
+ this.velocityPIDF = velocityPIDF;
+ this.maxSpeed = maxSpeed;
+ this.angleKV = calculateAngleKV(physicalCharacteristics.optimalVoltage,
+ physicalCharacteristics.angleMotorFreeSpeedRPM,
+ physicalCharacteristics.angleGearRatio);
+ this.physicalCharacteristics = physicalCharacteristics;
+
+ }
+
+ /**
+ * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not
+ * inverted.
+ *
+ * @param driveMotor Drive {@link SwerveMotor}.
+ * @param angleMotor Angle {@link SwerveMotor}
+ * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}.
+ * @param angleOffset Absolute angle offset to 0.
+ * @param xMeters Module location in meters from the center horizontally.
+ * @param yMeters Module location in meters from center vertically.
+ * @param anglePIDF Angle PIDF configuration.
+ * @param velocityPIDF Velocity PIDF configuration.
+ * @param maxSpeed Maximum robot speed in meters per second.
+ * @param physicalCharacteristics Physical characteristics of the swerve module.
+ */
+ public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
+ SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
+ double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
+ double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ {
+ this(driveMotor,
+ angleMotor,
+ absoluteEncoder,
+ angleOffset,
+ xMeters,
+ yMeters,
+ anglePIDF,
+ velocityPIDF,
+ maxSpeed,
+ physicalCharacteristics,
+ false,
+ false,
+ false);
+ }
+
+
+ /**
+ * Create the drive feedforward for swerve modules.
+ *
+ * @return Drive feedforward for drive motor on a swerve module.
+ */
+ public SimpleMotorFeedforward createDriveFeedforward()
+ {
+ double kv = physicalCharacteristics.optimalVoltage / maxSpeed;
+ ///^ Volt-seconds per meter (max voltage divided by max speed)
+ double ka = physicalCharacteristics.optimalVoltage /
+ calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
+ ///^ Volt-seconds^2 per meter (max voltage divided by max accel)
+ return new SimpleMotorFeedforward(0, kv, ka);
+ }
+
+ /**
+ * Get the encoder conversion for position encoders.
+ *
+ * @param isDriveMotor For the drive motor.
+ * @return Position encoder conversion factor.
+ */
+ public double getPositionEncoderConversion(boolean isDriveMotor)
+ {
+ return isDriveMotor ? calculateMetersPerRotation(physicalCharacteristics.wheelDiameter,
+ physicalCharacteristics.driveGearRatio,
+ physicalCharacteristics.driveEncoderPulsePerRotation)
+ : calculateDegreesPerSteeringRotation(
+ angleMotor.isAttachedAbsoluteEncoder() ? 1 : physicalCharacteristics.angleGearRatio,
+ physicalCharacteristics.angleEncoderPulsePerRotation);
+ }
+}
diff --git a/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/swervelib/parser/SwerveModulePhysicalCharacteristics.java
new file mode 100644
index 0000000..e49dc70
--- /dev/null
+++ b/swervelib/parser/SwerveModulePhysicalCharacteristics.java
@@ -0,0 +1,117 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+/**
+ * Configuration class which stores physical characteristics shared between every swerve module.
+ */
+public class SwerveModulePhysicalCharacteristics
+{
+
+ /**
+ * Wheel diameter in meters.
+ */
+ public final double wheelDiameter;
+ /**
+ * Drive gear ratio. How many times the motor has to spin before the wheel makes a complete rotation.
+ */
+ public final double driveGearRatio;
+ /**
+ * Angle gear ratio. How many times the motor has to spin before the wheel makes a full rotation.
+ */
+ public final double angleGearRatio;
+ /**
+ * Drive motor encoder pulse per rotation. 1 if integrated encoder.
+ */
+ public final int driveEncoderPulsePerRotation;
+ /**
+ * Angle motor encoder pulse per rotation. 1 if integrated encoder.
+ */
+ public final int angleEncoderPulsePerRotation;
+ /**
+ * Optimal voltage of the robot.
+ */
+ public final double optimalVoltage;
+ /**
+ * Current limits for the Swerve Module.
+ */
+ public final int driveMotorCurrentLimit, angleMotorCurrentLimit;
+ /**
+ * The time it takes for the motor to go from 0 to full throttle in seconds.
+ */
+ public final double driveMotorRampRate, angleMotorRampRate;
+ /**
+ * Wheel grip tape coefficient of friction on carpet, as described by the vendor.
+ */
+ public final double wheelGripCoefficientOfFriction;
+ /**
+ * Free speed rotations per minute of the motor, as described by the vendor.
+ */
+ public final double angleMotorFreeSpeedRPM;
+
+ /**
+ * Construct the swerve module physical characteristics.
+ *
+ * @param driveGearRatio Gear ratio of the drive motor. Number of motor rotations to rotate the
+ * wheel.
+ * @param angleGearRatio Gear ratio of the angle motor. Number of motor rotations to spin the wheel.
+ * @param angleMotorFreeSpeedRPM Motor free speed rotation per minute.
+ * @param wheelDiameter Wheel diameter in meters.
+ * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by manufacturer.
+ * @param optimalVoltage Optimal robot voltage.
+ * @param driveMotorCurrentLimit Current limit for the drive motor.
+ * @param angleMotorCurrentLimit Current limit for the angle motor.
+ * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
+ * over drawing power from battery)
+ * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
+ * overdrawing power and power loss).
+ * @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
+ * @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
+ */
+ public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio,
+ double angleMotorFreeSpeedRPM,
+ double wheelDiameter, double wheelGripCoefficientOfFriction,
+ double optimalVoltage, int driveMotorCurrentLimit,
+ int angleMotorCurrentLimit, double driveMotorRampRate,
+ double angleMotorRampRate, int driveEncoderPulsePerRotation,
+ int angleEncoderPulsePerRotation)
+ {
+ this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
+ this.optimalVoltage = optimalVoltage;
+
+ this.angleMotorFreeSpeedRPM = angleMotorFreeSpeedRPM;
+ this.angleGearRatio = angleGearRatio;
+ this.driveGearRatio = driveGearRatio;
+ this.angleEncoderPulsePerRotation = angleEncoderPulsePerRotation;
+ this.driveEncoderPulsePerRotation = driveEncoderPulsePerRotation;
+ this.wheelDiameter = wheelDiameter;
+
+ this.driveMotorCurrentLimit = driveMotorCurrentLimit;
+ this.angleMotorCurrentLimit = angleMotorCurrentLimit;
+ this.driveMotorRampRate = driveMotorRampRate;
+ this.angleMotorRampRate = angleMotorRampRate;
+ }
+
+ /**
+ * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 (taken from blue
+ * nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive motor current limit is 40A, and the
+ * angle motor current limit is 20A.
+ *
+ * @param driveGearRatio Gear ratio of the drive motor. Number of motor rotations to rotate the wheel.
+ * @param angleGearRatio Gear ratio of the angle motor. Number of motor rotations to spin the wheel.
+ * @param angleMotorFreeSpeedRPM Motor free speed rotation per minute.
+ * @param wheelDiameter Wheel diameter in meters.
+ * @param driveMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents over
+ * drawing power from battery)
+ * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
+ * overdrawing power and power loss).
+ * @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
+ * @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
+ */
+ public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio,
+ double angleMotorFreeSpeedRPM,
+ double wheelDiameter, double driveMotorRampRate, double angleMotorRampRate,
+ int driveEncoderPulsePerRotation, int angleEncoderPulsePerRotation)
+ {
+ this(driveGearRatio, angleGearRatio, angleMotorFreeSpeedRPM, wheelDiameter, 1.19, 12,
+ 40, 20, driveMotorRampRate, angleMotorRampRate, driveEncoderPulsePerRotation, angleEncoderPulsePerRotation);
+ }
+}
diff --git a/swervelib/parser/SwerveParser.java b/swervelib/parser/SwerveParser.java
new file mode 100644
index 0000000..ae05146
--- /dev/null
+++ b/swervelib/parser/SwerveParser.java
@@ -0,0 +1,140 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveDrive;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveModule;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.ControllerPropertiesJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.ModuleJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.PIDFPropertiesJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.PhysicalPropertiesJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.SwerveDriveJson;
+import java.io.File;
+import java.io.IOException;
+import java.util.HashMap;
+
+/**
+ * Helper class used to parse the JSON directory with specified configuration options.
+ */
+public class SwerveParser
+{
+
+ private static final HashMap moduleConfigs = new HashMap<>();
+ /**
+ * Parsed swervedrive.json
+ */
+ public static SwerveDriveJson swerveDriveJson;
+ /**
+ * Parsed controllerproperties.json
+ */
+ public static ControllerPropertiesJson controllerPropertiesJson;
+ /**
+ * Parsed modules/pidfproperties.json
+ */
+ public static PIDFPropertiesJson pidfPropertiesJson;
+ /**
+ * Parsed modules/physicalproperties.json
+ */
+ public static PhysicalPropertiesJson physicalPropertiesJson;
+ /**
+ * Array holding the module jsons given in {@link SwerveDriveJson}.
+ */
+ public static ModuleJson[] moduleJsons;
+
+
+ /**
+ * Construct a swerve parser. Will throw an error if there is a missing file.
+ *
+ * @param directory Directory with swerve configurations.
+ */
+ public SwerveParser(File directory) throws IOException
+ {
+ checkDirectory(directory);
+ swerveDriveJson = new ObjectMapper().readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
+ controllerPropertiesJson = new ObjectMapper().readValue(new File(directory, "controllerproperties.json"),
+ ControllerPropertiesJson.class);
+ pidfPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/pidfproperties.json"),
+ PIDFPropertiesJson.class);
+ physicalPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/physicalproperties.json"),
+ PhysicalPropertiesJson.class);
+ moduleJsons = new ModuleJson[swerveDriveJson.modules.length];
+ for (int i = 0; i < moduleJsons.length; i++)
+ {
+ moduleConfigs.put(swerveDriveJson.modules[i], i);
+ File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]);
+ assert moduleFile.exists();
+ moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class);
+ }
+ }
+
+ /**
+ * Get the swerve module by the json name.
+ *
+ * @param name JSON name.
+ * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from.
+ * @return {@link SwerveModuleConfiguration} based on the file.
+ */
+ public static SwerveModule getModuleConfigurationByName(String name,
+ SwerveDriveConfiguration driveConfiguration)
+ {
+ return driveConfiguration.modules[moduleConfigs.get(name + ".json")];
+ }
+
+ /**
+ * Open JSON file.
+ *
+ * @param file JSON File to open.
+ * @return JsonNode of file.
+ */
+ private JsonNode openJson(File file)
+ {
+ try
+ {
+ return new ObjectMapper().readTree(file);
+ } catch (IOException e)
+ {
+ throw new RuntimeException(e);
+ }
+ }
+
+ /**
+ * Check directory structure.
+ *
+ * @param directory JSON Configuration Directory
+ */
+ private void checkDirectory(File directory)
+ {
+ assert new File(directory, "swervedrive.json").exists();
+ assert new File(directory, "controllerproperties.json").exists();
+ assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory();
+ assert new File(directory, "modules/pidfproperties.json").exists();
+ assert new File(directory, "modules/physicalproperties.json").exists();
+ }
+
+ /**
+ * Create {@link SwerveDrive} from JSON configuration directory.
+ *
+ * @return {@link SwerveDrive} instance.
+ */
+ public SwerveDrive createSwerveDrive()
+ {
+ double maxSpeedMPS = Units.feetToMeters(swerveDriveJson.maxSpeed);
+ SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length];
+ for (int i = 0; i < moduleConfigurations.length; i++)
+ {
+ ModuleJson module = moduleJsons[i];
+ moduleConfigurations[i] = module.createModuleConfiguration(pidfPropertiesJson.angle, pidfPropertiesJson.drive,
+ maxSpeedMPS,
+ physicalPropertiesJson.createPhysicalProperties(
+ swerveDriveJson.optimalVoltage));
+ }
+ SwerveDriveConfiguration swerveDriveConfiguration = new SwerveDriveConfiguration(moduleConfigurations,
+ swerveDriveJson.imu.createIMU(),
+ maxSpeedMPS,
+ swerveDriveJson.invertedIMU);
+
+ return new SwerveDrive(swerveDriveConfiguration,
+ controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration));
+ }
+}
diff --git a/swervelib/parser/deserializer/PIDFRange.java b/swervelib/parser/deserializer/PIDFRange.java
new file mode 100644
index 0000000..3df05d1
--- /dev/null
+++ b/swervelib/parser/deserializer/PIDFRange.java
@@ -0,0 +1,17 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.deserializer;
+
+/**
+ * Class to hold the minimum and maximum input or output of the PIDF.
+ */
+public class PIDFRange
+{
+
+ /**
+ * Minimum value.
+ */
+ public double min = -1;
+ /**
+ * Maximum value.
+ */
+ public double max = 1;
+}
diff --git a/swervelib/parser/json/ControllerPropertiesJson.java b/swervelib/parser/json/ControllerPropertiesJson.java
new file mode 100644
index 0000000..58453fb
--- /dev/null
+++ b/swervelib/parser/json/ControllerPropertiesJson.java
@@ -0,0 +1,32 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveControllerConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveDriveConfiguration;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveController} parsed class. Used to access the JSON data.
+ */
+public class ControllerPropertiesJson
+{
+
+ /**
+ * The minimum radius of the angle control joystick to allow for heading adjustment of the robot.
+ */
+ public double angleJoystickRadiusDeadband;
+ /**
+ * The PID used to control the robot heading.
+ */
+ public PIDFConfig heading;
+
+ /**
+ * Create the {@link SwerveControllerConfiguration} based on parsed and given data.
+ *
+ * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration.
+ * @return {@link SwerveControllerConfiguration} object based on parsed data.
+ */
+ public SwerveControllerConfiguration createControllerConfiguration(SwerveDriveConfiguration driveConfiguration)
+ {
+ return new SwerveControllerConfiguration(driveConfiguration, heading, angleJoystickRadiusDeadband);
+ }
+}
diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java
new file mode 100644
index 0000000..2bd2673
--- /dev/null
+++ b/swervelib/parser/json/DeviceJson.java
@@ -0,0 +1,100 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.CANCoderSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SparkMaxEncoderSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.NavXSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.Pigeon2Swerve;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.PigeonSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.SwerveIMU;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SparkMaxSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+
+/**
+ * Device JSON parsed class. Used to access the JSON data.
+ */
+public class DeviceJson
+{
+
+ /**
+ * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx
+ */
+ public String type;
+ /**
+ * The CAN ID or pin ID of the device.
+ */
+ public int id;
+ /**
+ * The CAN bus name which the device resides on if using CAN.
+ */
+ public String canbus = "";
+
+ /**
+ * Create a {@link SwerveAbsoluteEncoder} from the current configuration.
+ *
+ * @return {@link SwerveAbsoluteEncoder} given.
+ */
+ public SwerveAbsoluteEncoder createEncoder()
+ {
+ switch (type)
+ {
+ case "integrated":
+ case "attached":
+ return null;
+ case "cancoder":
+ return new CANCoderSwerve(id, canbus != null ? canbus : "");
+ default:
+ throw new RuntimeException(type + " is not a recognized absolute encoder type.");
+ }
+ }
+
+ /**
+ * Create a {@link SwerveIMU} from the given configuration.
+ *
+ * @return {@link SwerveIMU} given.
+ */
+ public SwerveIMU createIMU()
+ {
+ switch (type)
+ {
+ case "navx":
+ return new NavXSwerve();
+ case "pigeon":
+ return new PigeonSwerve(id);
+ case "pigeon2":
+ return new Pigeon2Swerve(id, canbus != null ? canbus : "");
+ default:
+ throw new RuntimeException(type + " is not a recognized absolute encoder type.");
+ }
+ }
+
+ /**
+ * Create a {@link SwerveMotor} from the given configuration.
+ *
+ * @param isDriveMotor If the motor being generated is a drive motor.
+ * @return {@link SwerveMotor} given.
+ */
+ public SwerveMotor createMotor(boolean isDriveMotor)
+ {
+ if (type.equals("sparkmax"))
+ {
+ return new SparkMaxSwerve(id, isDriveMotor);
+ }
+ throw new RuntimeException(type + " is not a recognized absolute encoder type.");
+ }
+
+ /**
+ * Create a {@link SwerveAbsoluteEncoder} from the data port on the motor controller.
+ *
+ * @param motor The motor to create the absolute encoder from.
+ * @return {@link SwerveAbsoluteEncoder} from the motor controller.
+ */
+ public SwerveAbsoluteEncoder createIntegratedEncoder(SwerveMotor motor)
+ {
+ if (type.equals("sparkmax"))
+ {
+ return new SparkMaxEncoderSwerve(motor);
+ }
+ throw new RuntimeException("Could not create absolute encoder from data port of " + type + " id " + id);
+ }
+}
diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java
new file mode 100644
index 0000000..1b52a00
--- /dev/null
+++ b/swervelib/parser/json/ModuleJson.java
@@ -0,0 +1,76 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModuleConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModulePhysicalCharacteristics;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules.BoolMotorJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules.LocationJson;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveModule} JSON parsed class. Used to access the JSON data.
+ */
+public class ModuleJson
+{
+
+ /**
+ * Drive motor device configuration.
+ */
+ public DeviceJson drive;
+ /**
+ * Angle motor device configuration.
+ */
+ public DeviceJson angle;
+ /**
+ * Absolute encoder device configuration.
+ */
+ public DeviceJson encoder;
+ /**
+ * Defines which motors are inverted.
+ */
+ public BoolMotorJson inverted;
+ /**
+ * Absolute encoder offset from 0 in degrees.
+ */
+ public double absoluteEncoderOffset;
+ /**
+ * Absolute encoder inversion state.
+ */
+ public boolean absoluteEncoderInverted = false;
+ /**
+ * The location of the swerve module from the center of the robot in inches.
+ */
+ public LocationJson location;
+
+ /**
+ * Create the swerve module configuration based off of parsed data.
+ *
+ * @param anglePIDF The PIDF values for the angle motor.
+ * @param velocityPIDF The velocity PIDF values for the drive motor.
+ * @param maxSpeed The maximum speed of the robot in meters per second.
+ * @param physicalCharacteristics Physical characteristics of the swerve module.
+ * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data.
+ */
+ public SwerveModuleConfiguration createModuleConfiguration(PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
+ double maxSpeed,
+ SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ {
+ SwerveMotor angleMotor = angle.createMotor(false);
+ SwerveAbsoluteEncoder absEncoder = encoder.createEncoder();
+
+ // If the absolute encoder is attached.
+ if (absEncoder == null)
+ {
+ absEncoder = angle.createIntegratedEncoder(angleMotor);
+ angleMotor.setAbsoluteEncoder(absEncoder);
+ }
+
+ return new SwerveModuleConfiguration(drive.createMotor(true), angleMotor, absEncoder,
+ absoluteEncoderOffset, Units.inchesToMeters(location.x),
+ Units.inchesToMeters(location.y), anglePIDF, velocityPIDF, maxSpeed,
+ physicalCharacteristics, absoluteEncoderInverted, inverted.drive,
+ inverted.angle);
+ }
+}
diff --git a/swervelib/parser/json/PIDFPropertiesJson.java b/swervelib/parser/json/PIDFPropertiesJson.java
new file mode 100644
index 0000000..589a7e6
--- /dev/null
+++ b/swervelib/parser/json/PIDFPropertiesJson.java
@@ -0,0 +1,20 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveModule} PID with Feedforward for the drive motor and angle
+ * motor.
+ */
+public class PIDFPropertiesJson
+{
+
+ /**
+ * The PIDF with Integral Zone used for the drive motor.
+ */
+ public PIDFConfig drive;
+ /**
+ * The PIDF with Integral Zone used for the angle motor.
+ */
+ public PIDFConfig angle;
+}
diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java
new file mode 100644
index 0000000..52553b2
--- /dev/null
+++ b/swervelib/parser/json/PhysicalPropertiesJson.java
@@ -0,0 +1,128 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModulePhysicalCharacteristics;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to
+ * configure the SwerveModule.
+ */
+public class PhysicalPropertiesJson
+{
+
+ /**
+ * Wheel diameter in inches.
+ */
+ public double wheelDiameter;
+ /**
+ * Gear ratio for the motors, number of times the motor has to spin before the wheel rotates a single time.
+ */
+ public MotorConfigDouble gearRatio;
+ /**
+ * Encoder pulse per rotation for non-integrated encoders. 1 for integrated encoders.
+ */
+ public MotorConfigInt encoderPulsePerRotation = new MotorConfigInt(1, 1);
+ /**
+ * The current limit in AMPs to apply to the motors.
+ */
+ public MotorConfigInt currentLimit = new MotorConfigInt(40, 20);
+ /**
+ * The minimum number of seconds to take for the motor to go from 0 to full throttle.
+ */
+ public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25);
+ /**
+ * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum acceleration.
+ */
+ public double wheelGripCoefficientOfFriction = 1.19;
+ /**
+ * Angle motor free speed rotations per minute.
+ */
+ public double angleMotorFreeSpeedRPM;
+
+ /**
+ * Create the physical characteristics based off the parsed data.
+ *
+ * @param optimalVoltage Optimal voltage to compensate for and use to calculate drive motor feedforward.
+ * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data.
+ */
+ public SwerveModulePhysicalCharacteristics createPhysicalProperties(double optimalVoltage)
+ {
+ return new SwerveModulePhysicalCharacteristics(gearRatio.drive, gearRatio.angle, angleMotorFreeSpeedRPM,
+ Units.inchesToMeters(wheelDiameter), wheelGripCoefficientOfFriction,
+ optimalVoltage,
+ currentLimit.drive, currentLimit.angle, rampRate.drive,
+ rampRate.angle, encoderPulsePerRotation.drive,
+ encoderPulsePerRotation.angle);
+ }
+}
+
+/**
+ * Used to store doubles for motor configuration.
+ */
+class MotorConfigDouble
+{
+
+ /**
+ * Drive motor.
+ */
+ public double drive;
+ /**
+ * Angle motor.
+ */
+ public double angle;
+
+ /**
+ * Default constructor.
+ */
+ public MotorConfigDouble()
+ {
+ }
+
+ /**
+ * Default constructor.
+ *
+ * @param angle Angle data.
+ * @param drive Drive data.
+ */
+ public MotorConfigDouble(double angle, double drive)
+ {
+ this.angle = angle;
+ this.drive = drive;
+ }
+
+}
+
+/**
+ * Used to store ints for motor configuration.
+ */
+class MotorConfigInt
+{
+
+ /**
+ * Drive motor.
+ */
+ public int drive;
+ /**
+ * Angle motor.
+ */
+ public int angle;
+
+ /**
+ * Default constructor.
+ */
+ public MotorConfigInt()
+ {
+ }
+
+ /**
+ * Default constructor with values.
+ *
+ * @param drive Drive data.
+ * @param angle Angle data.
+ */
+ public MotorConfigInt(int drive, int angle)
+ {
+ this.angle = angle;
+ this.drive = drive;
+ }
+}
\ No newline at end of file
diff --git a/swervelib/parser/json/SwerveDriveJson.java b/swervelib/parser/json/SwerveDriveJson.java
new file mode 100644
index 0000000..a3f0d35
--- /dev/null
+++ b/swervelib/parser/json/SwerveDriveJson.java
@@ -0,0 +1,30 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from
+ * the swervedrive.json file.
+ */
+public class SwerveDriveJson
+{
+
+ /**
+ * Maximum robot speed in feet per second.
+ */
+ public double maxSpeed;
+ /**
+ * Optimal voltage to compensate to and base feedforward calculations off of.
+ */
+ public double optimalVoltage;
+ /**
+ * Robot IMU used to determine heading of the robot.
+ */
+ public DeviceJson imu;
+ /**
+ * Invert the IMU of the robot.
+ */
+ public boolean invertedIMU;
+ /**
+ * Module JSONs in order clockwise order starting from front left.
+ */
+ public String[] modules;
+}
diff --git a/swervelib/parser/json/modules/BoolMotorJson.java b/swervelib/parser/json/modules/BoolMotorJson.java
new file mode 100644
index 0000000..b37b4cc
--- /dev/null
+++ b/swervelib/parser/json/modules/BoolMotorJson.java
@@ -0,0 +1,17 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules;
+
+/**
+ * Inverted motor JSON parsed class. Used to access the JSON data.
+ */
+public class BoolMotorJson
+{
+
+ /**
+ * Drive motor inversion state.
+ */
+ public boolean drive;
+ /**
+ * Angle motor inversion state.
+ */
+ public boolean angle;
+}
diff --git a/swervelib/parser/json/modules/LocationJson.java b/swervelib/parser/json/modules/LocationJson.java
new file mode 100644
index 0000000..50f214d
--- /dev/null
+++ b/swervelib/parser/json/modules/LocationJson.java
@@ -0,0 +1,19 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules;
+
+/**
+ * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as distances to the center of
+ * the robot. Positive x is torwards the robot front, and * +y is torwards robot left.
+ */
+
+public class LocationJson
+{
+
+ /**
+ * Location of the swerve module in inches from the center of the robot horizontally.
+ */
+ public double x;
+ /**
+ * Location of the swerve module in inches from the center of the robot vertically.
+ */
+ public double y;
+}