@@ -104,7 +114,7 @@ loadScripts(document, 'script');
double
-
Max speed in meters per second.
+
Max module speed in meters per second.
int
@@ -200,7 +210,21 @@ loadScripts(document, 'script');
maxSpeed
public double maxSpeed
-Max speed in meters per second.
+Max module speed in meters per second.
+
+
+
+
+attainableMaxTranslationalSpeedMetersPerSecond
+public double attainableMaxTranslationalSpeedMetersPerSecond
+Max module speed in meters per second.
+
+
+
+
+attainableMaxRotationalVelocityRadiansPerSecond
+public double attainableMaxRotationalVelocityRadiansPerSecond
+Max module speed in meters per second.
diff --git a/docs/swervelib/parser/SwerveModuleConfiguration.html b/docs/swervelib/parser/SwerveModuleConfiguration.html
index 396063f..547a18f 100644
--- a/docs/swervelib/parser/SwerveModuleConfiguration.html
+++ b/docs/swervelib/parser/SwerveModuleConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleConfiguration
-
+
diff --git a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
index d069df1..82fb68c 100644
--- a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
+++ b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
@@ -1,11 +1,11 @@
-
+
SwerveModulePhysicalCharacteristics
-
+
diff --git a/docs/swervelib/parser/SwerveParser.html b/docs/swervelib/parser/SwerveParser.html
index 4de8f82..153a69a 100644
--- a/docs/swervelib/parser/SwerveParser.html
+++ b/docs/swervelib/parser/SwerveParser.html
@@ -1,11 +1,11 @@
-
+
SwerveParser
-
+
diff --git a/docs/swervelib/parser/deserializer/PIDFRange.html b/docs/swervelib/parser/deserializer/PIDFRange.html
index b90de74..1b41a70 100644
--- a/docs/swervelib/parser/deserializer/PIDFRange.html
+++ b/docs/swervelib/parser/deserializer/PIDFRange.html
@@ -1,11 +1,11 @@
-
+
PIDFRange
-
+
diff --git a/docs/swervelib/parser/deserializer/package-summary.html b/docs/swervelib/parser/deserializer/package-summary.html
index 63c50d0..f45bc05 100644
--- a/docs/swervelib/parser/deserializer/package-summary.html
+++ b/docs/swervelib/parser/deserializer/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.deserializer
-
+
diff --git a/docs/swervelib/parser/deserializer/package-tree.html b/docs/swervelib/parser/deserializer/package-tree.html
index 82a7cf4..ca1dd74 100644
--- a/docs/swervelib/parser/deserializer/package-tree.html
+++ b/docs/swervelib/parser/deserializer/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.deserializer Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/json/ControllerPropertiesJson.html b/docs/swervelib/parser/json/ControllerPropertiesJson.html
index 0ec5939..81b44b6 100644
--- a/docs/swervelib/parser/json/ControllerPropertiesJson.html
+++ b/docs/swervelib/parser/json/ControllerPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
ControllerPropertiesJson
-
+
diff --git a/docs/swervelib/parser/json/DeviceJson.html b/docs/swervelib/parser/json/DeviceJson.html
index 68766df..fb6f43f 100644
--- a/docs/swervelib/parser/json/DeviceJson.html
+++ b/docs/swervelib/parser/json/DeviceJson.html
@@ -1,11 +1,11 @@
-
+
DeviceJson
-
+
diff --git a/docs/swervelib/parser/json/ModuleJson.html b/docs/swervelib/parser/json/ModuleJson.html
index 43d9a4a..d3bc148 100644
--- a/docs/swervelib/parser/json/ModuleJson.html
+++ b/docs/swervelib/parser/json/ModuleJson.html
@@ -1,11 +1,11 @@
-
+
ModuleJson
-
+
diff --git a/docs/swervelib/parser/json/MotorConfigDouble.html b/docs/swervelib/parser/json/MotorConfigDouble.html
index cf232e5..936ad50 100644
--- a/docs/swervelib/parser/json/MotorConfigDouble.html
+++ b/docs/swervelib/parser/json/MotorConfigDouble.html
@@ -1,11 +1,11 @@
-
+
MotorConfigDouble
-
+
diff --git a/docs/swervelib/parser/json/MotorConfigInt.html b/docs/swervelib/parser/json/MotorConfigInt.html
index cdb9719..28ca83d 100644
--- a/docs/swervelib/parser/json/MotorConfigInt.html
+++ b/docs/swervelib/parser/json/MotorConfigInt.html
@@ -1,11 +1,11 @@
-
+
MotorConfigInt
-
+
diff --git a/docs/swervelib/parser/json/PIDFPropertiesJson.html b/docs/swervelib/parser/json/PIDFPropertiesJson.html
index e9c4093..d6f9f58 100644
--- a/docs/swervelib/parser/json/PIDFPropertiesJson.html
+++ b/docs/swervelib/parser/json/PIDFPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
PIDFPropertiesJson
-
+
diff --git a/docs/swervelib/parser/json/PhysicalPropertiesJson.html b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
index b608fbc..5806db5 100644
--- a/docs/swervelib/parser/json/PhysicalPropertiesJson.html
+++ b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
PhysicalPropertiesJson
-
+
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index 21b7c18..d470998 100644
--- a/docs/swervelib/parser/json/SwerveDriveJson.html
+++ b/docs/swervelib/parser/json/SwerveDriveJson.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveJson
-
+
diff --git a/docs/swervelib/parser/json/modules/BoolMotorJson.html b/docs/swervelib/parser/json/modules/BoolMotorJson.html
index 230aa59..44653e3 100644
--- a/docs/swervelib/parser/json/modules/BoolMotorJson.html
+++ b/docs/swervelib/parser/json/modules/BoolMotorJson.html
@@ -1,11 +1,11 @@
-
+
BoolMotorJson
-
+
diff --git a/docs/swervelib/parser/json/modules/LocationJson.html b/docs/swervelib/parser/json/modules/LocationJson.html
index 43d812f..3dc8d1d 100644
--- a/docs/swervelib/parser/json/modules/LocationJson.html
+++ b/docs/swervelib/parser/json/modules/LocationJson.html
@@ -1,11 +1,11 @@
-
+
LocationJson
-
+
diff --git a/docs/swervelib/parser/json/modules/package-summary.html b/docs/swervelib/parser/json/modules/package-summary.html
index 88d6ff0..b125e47 100644
--- a/docs/swervelib/parser/json/modules/package-summary.html
+++ b/docs/swervelib/parser/json/modules/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json.modules
-
+
diff --git a/docs/swervelib/parser/json/modules/package-tree.html b/docs/swervelib/parser/json/modules/package-tree.html
index 261f70c..bdc5791 100644
--- a/docs/swervelib/parser/json/modules/package-tree.html
+++ b/docs/swervelib/parser/json/modules/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json.modules Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/json/package-summary.html b/docs/swervelib/parser/json/package-summary.html
index b1c4ace..7380795 100644
--- a/docs/swervelib/parser/json/package-summary.html
+++ b/docs/swervelib/parser/json/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json
-
+
diff --git a/docs/swervelib/parser/json/package-tree.html b/docs/swervelib/parser/json/package-tree.html
index fc26f0c..d75fe86 100644
--- a/docs/swervelib/parser/json/package-tree.html
+++ b/docs/swervelib/parser/json/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser.json Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/package-summary.html b/docs/swervelib/parser/package-summary.html
index 13a7782..d66b4b5 100644
--- a/docs/swervelib/parser/package-summary.html
+++ b/docs/swervelib/parser/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser
-
+
diff --git a/docs/swervelib/parser/package-tree.html b/docs/swervelib/parser/package-tree.html
index f46d51b..bc03acf 100644
--- a/docs/swervelib/parser/package-tree.html
+++ b/docs/swervelib/parser/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser Class Hierarchy
-
+
diff --git a/docs/swervelib/simulation/SwerveIMUSimulation.html b/docs/swervelib/simulation/SwerveIMUSimulation.html
index a99d8c9..74668e6 100644
--- a/docs/swervelib/simulation/SwerveIMUSimulation.html
+++ b/docs/swervelib/simulation/SwerveIMUSimulation.html
@@ -1,11 +1,11 @@
-
+
SwerveIMUSimulation
-
+
diff --git a/docs/swervelib/simulation/SwerveModuleSimulation.html b/docs/swervelib/simulation/SwerveModuleSimulation.html
index 9a86828..b854a67 100644
--- a/docs/swervelib/simulation/SwerveModuleSimulation.html
+++ b/docs/swervelib/simulation/SwerveModuleSimulation.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleSimulation
-
+
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
index 4b21171..5031b7c 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
@@ -1,11 +1,11 @@
-
+
PhysicsSim.SimProfile
-
+
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.html b/docs/swervelib/simulation/ctre/PhysicsSim.html
index 1b16b6b..84b51c1 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.html
@@ -1,11 +1,11 @@
-
+
PhysicsSim
-
+
diff --git a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
index aa76054..425b48c 100644
--- a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
@@ -1,11 +1,11 @@
-
+
TalonFXSimProfile
-
+
diff --git a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
index 3da1e79..acddad8 100644
--- a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
@@ -1,11 +1,11 @@
-
+
TalonSRXSimProfile
-
+
diff --git a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
index f8091fa..4e63fa3 100644
--- a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
@@ -1,11 +1,11 @@
-
+
VictorSPXSimProfile
-
+
diff --git a/docs/swervelib/simulation/ctre/package-summary.html b/docs/swervelib/simulation/ctre/package-summary.html
index 817a5b7..71c94d5 100644
--- a/docs/swervelib/simulation/ctre/package-summary.html
+++ b/docs/swervelib/simulation/ctre/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.simulation.ctre
-
+
diff --git a/docs/swervelib/simulation/ctre/package-tree.html b/docs/swervelib/simulation/ctre/package-tree.html
index d5a4bdc..1794908 100644
--- a/docs/swervelib/simulation/ctre/package-tree.html
+++ b/docs/swervelib/simulation/ctre/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.simulation.ctre Class Hierarchy
-
+
diff --git a/docs/swervelib/simulation/package-summary.html b/docs/swervelib/simulation/package-summary.html
index e37cacf..0ef4ed7 100644
--- a/docs/swervelib/simulation/package-summary.html
+++ b/docs/swervelib/simulation/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.simulation
-
+
diff --git a/docs/swervelib/simulation/package-tree.html b/docs/swervelib/simulation/package-tree.html
index 1a6b6e1..0e18cbb 100644
--- a/docs/swervelib/simulation/package-tree.html
+++ b/docs/swervelib/simulation/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.simulation Class Hierarchy
-
+
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
index 04f419a..44dcaf3 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveTelemetry.TelemetryVerbosity
-
+
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
index 27af883..d738d75 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveTelemetry
-
+
diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index 214c3e7..d0eb83d 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry
-
+
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index 624dd3c..842078e 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry Class Hierarchy
-
+
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index eb1ecac..343e357 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -5,12 +5,7 @@ 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.Rotation3d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
@@ -20,9 +15,6 @@ import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Optional;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveKinematics2;
import swervelib.math.SwerveMath;
@@ -33,6 +25,10 @@ import swervelib.simulation.SwerveIMUSimulation;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
/**
* Swerve Drive class representing and controlling the swerve drive.
*/
@@ -253,34 +249,54 @@ public class SwerveDrive
setRawModuleStates(swerveModuleStates, isOpenLoop);
}
+ /**
+ * Set the maximum speeds for desaturation.
+ *
+ * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach in meters per second.
+ * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while
+ * translating in meters per second.
+ * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in radians per second.
+ */
+ public void setMaximumSpeeds(
+ double attainableMaxModuleSpeedMetersPerSecond,
+ double attainableMaxTranslationalSpeedMetersPerSecond,
+ double attainableMaxRotationalVelocityRadiansPerSecond) {
+ setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond);
+ swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
+ swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
+ }
+
/**
* 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.
*/
- private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
- {
+ private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop) {
// Desaturates wheel speeds
- SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
+ if (swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond != 0 ||
+ swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond != 0)
+ SwerveKinematics2.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
+ swerveDriveConfiguration.maxSpeed,
+ swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
+ swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
+ else
+ SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
// Sets states
- for (SwerveModule module : swerveModules)
- {
+ for (SwerveModule module : swerveModules) {
module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
- if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
- {
+ if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
SwerveDriveTelemetry.desiredStates[module.moduleNumber *
- 2] = module.lastState.angle.getDegrees();
+ 2] = module.lastState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) +
- 1] = module.lastState.speedMetersPerSecond;
+ 1] = module.lastState.speedMetersPerSecond;
}
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
- {
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) {
SmartDashboard.putNumber(
- "Module[" + module.moduleNumber + "] Speed Setpoint: ",
- module.lastState.speedMetersPerSecond);
+ "Module[" + module.moduleNumber + "] Speed Setpoint: ",
+ module.lastState.speedMetersPerSecond);
SmartDashboard.putNumber(
"Module[" + module.moduleNumber + "] Angle Setpoint: ",
module.lastState.angle.getDegrees());
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index dd5be9c..0caa5c8 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -194,7 +194,6 @@ public class SwerveModule
{
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
double feedforward = Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV;
- System.out.println((float) (Math.toDegrees(0) * configuration.angleKV));
if (absoluteEncoder != null && synchronizeEncoderQueued)
{
double absoluteEncoderPosition = getAbsolutePosition();
diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java
index 24bf4c0..d1673ce 100644
--- a/swervelib/parser/SwerveDriveConfiguration.java
+++ b/swervelib/parser/SwerveDriveConfiguration.java
@@ -7,73 +7,70 @@ import swervelib.imu.SwerveIMU;
/**
* Swerve drive configurations used during SwerveDrive construction.
*/
-public class SwerveDriveConfiguration
-{
+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;
+ /**
+ * Swerve Module locations.
+ */
+ public Translation2d[] moduleLocationsMeters;
+ /**
+ * Swerve IMU
+ */
+ public SwerveIMU imu;
+ /**
+ * Invert the imu measurements.
+ */
+ public boolean invertedIMU = false;
+ /**
+ * Max module speed in meters per second.
+ */
+ public double maxSpeed, attainableMaxTranslationalSpeedMetersPerSecond, attainableMaxRotationalVelocityRadiansPerSecond;
+ /**
+ * 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 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.attainableMaxRotationalVelocityRadiansPerSecond = 0;
+ this.attainableMaxTranslationalSpeedMetersPerSecond = 0;
+ 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]);
+ /**
+ * 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;
}
- return modArr;
- }
}