Trustworthiness of the internal model of how motors should be moving Measured in expected standard deviation
- (meters of position and degrees of rotation)
Trustworthiness of the internal model of how motors should be moving Measured in expected standard deviation
+ (meters of position and degrees of rotation)
Set the desired state of the swerve module. WARNING: If you are not using one of the functions from
+ SwerveDrive you may screw up SwerveDrive.kinematics
Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as Translation2ds.
+
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.
Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used
+
Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often
+ used
to convert joystick values into module speeds and angles.
This function also supports variable centers of rotation. During normal operations, the
- center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to
+ center of rotation is usually the same as the physical center of the robot; therefore, the argument is
+ defaulted to
that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or
for any other use case, you can do so.
@@ -361,8 +391,10 @@ loadScripts(document, 'script');
toSwerveModuleStates in class edu.wpi.first.math.kinematics.SwerveDriveKinematics
Parameters:
chassisSpeeds - The desired chassis speed.
-
centerOfRotationMeters - The center of rotation. For example, if you set the center of rotation at one corner
- of the robot and provide a chassis speed that only has a dtheta component, the robot
+
centerOfRotationMeters - The center of rotation. For example, if you set the center of rotation at one
+ corner
+ of the robot and provide a chassis speed that only has a dtheta component, the
+ robot
will rotate around that corner.
Returns:
An array containing the module states. Use caution because these module states are not normalized.
diff --git a/docs/swervelib/math/SwerveMath.html b/docs/swervelib/math/SwerveMath.html
index be9e889..f0ec8a1 100644
--- a/docs/swervelib/math/SwerveMath.html
+++ b/docs/swervelib/math/SwerveMath.html
@@ -1,11 +1,11 @@
-
+
SwerveMath
-
+
diff --git a/docs/swervelib/math/SwerveModuleState2.html b/docs/swervelib/math/SwerveModuleState2.html
index 6ac0ef4..c9701c3 100644
--- a/docs/swervelib/math/SwerveModuleState2.html
+++ b/docs/swervelib/math/SwerveModuleState2.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleState2
-
+
diff --git a/docs/swervelib/math/package-summary.html b/docs/swervelib/math/package-summary.html
index d8a7110..98bfed8 100644
--- a/docs/swervelib/math/package-summary.html
+++ b/docs/swervelib/math/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.math
-
+
diff --git a/docs/swervelib/math/package-tree.html b/docs/swervelib/math/package-tree.html
index 15b1e06..0ad2371 100644
--- a/docs/swervelib/math/package-tree.html
+++ b/docs/swervelib/math/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.math Class Hierarchy
-
+
diff --git a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
index 18ac3d5..6f98f9d 100644
--- a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
+++ b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
@@ -1,11 +1,11 @@
-
+
SparkMaxBrushedMotorSwerve
-
+
@@ -265,8 +265,15 @@ loadScripts(document, 'script');
Set the closed loop PID controller reference point.
setpoint - Setpoint in meters per second or angle in degrees.
+
feedforward - Feedforward in volt-meter-per-second or kV.
+
position - Only used on the angle motor, the position of the motor in degrees.
+
+
+
+
getVelocity
publicdoublegetVelocity()
diff --git a/docs/swervelib/motors/package-summary.html b/docs/swervelib/motors/package-summary.html
index 5194eaf..c8fe521 100644
--- a/docs/swervelib/motors/package-summary.html
+++ b/docs/swervelib/motors/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.motors
-
+
diff --git a/docs/swervelib/motors/package-tree.html b/docs/swervelib/motors/package-tree.html
index 8a97276..3e157a5 100644
--- a/docs/swervelib/motors/package-tree.html
+++ b/docs/swervelib/motors/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.motors Class Hierarchy
-
+
diff --git a/docs/swervelib/package-summary.html b/docs/swervelib/package-summary.html
index 9ea949a..3ee6f61 100644
--- a/docs/swervelib/package-summary.html
+++ b/docs/swervelib/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib
-
+
diff --git a/docs/swervelib/package-tree.html b/docs/swervelib/package-tree.html
index 7f3fcdb..05a011f 100644
--- a/docs/swervelib/package-tree.html
+++ b/docs/swervelib/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib Class Hierarchy
-
+
diff --git a/docs/swervelib/parser/PIDFConfig.html b/docs/swervelib/parser/PIDFConfig.html
index 0e08822..baa0f02 100644
--- a/docs/swervelib/parser/PIDFConfig.html
+++ b/docs/swervelib/parser/PIDFConfig.html
@@ -1,11 +1,11 @@
-
+
PIDFConfig
-
+
diff --git a/docs/swervelib/parser/SwerveControllerConfiguration.html b/docs/swervelib/parser/SwerveControllerConfiguration.html
index 9c7e1de..0943893 100644
--- a/docs/swervelib/parser/SwerveControllerConfiguration.html
+++ b/docs/swervelib/parser/SwerveControllerConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveControllerConfiguration
-
+
diff --git a/docs/swervelib/parser/SwerveDriveConfiguration.html b/docs/swervelib/parser/SwerveDriveConfiguration.html
index 2cec65a..e691297 100644
--- a/docs/swervelib/parser/SwerveDriveConfiguration.html
+++ b/docs/swervelib/parser/SwerveDriveConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveDriveConfiguration
-
+
diff --git a/docs/swervelib/parser/SwerveModuleConfiguration.html b/docs/swervelib/parser/SwerveModuleConfiguration.html
index 4c31506..8c5f136 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 aef720d..1724957 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 17e60ea..7b99da2 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 c0f8950..a1271d6 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 20d3180..78ec31c 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 4725744..12d982a 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 232c510..2182854 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 8d652ce..621cd3f 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 6615c93..32eca3a 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 e687f65..9238420 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 c616825..169d1ed 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 a1606e5..7bf42d9 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 ad35961..6a82645 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 21ac2b3..e581fc1 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 2f412a2..fc3526e 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 62de372..9cdaed5 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 07546bc..3fc70df 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 554673b..a9db522 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 5316462..8084ba5 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 4f218c4..e2987f6 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 efd5139..c246a78 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 7ea4871..bd4400e 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 bc5a7de..80fef4a 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 a6d0c1b..d88ee54 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 91dbf7a..301d303 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 7ce4b0d..757a2fc 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 aaf0d9c..8797801 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 7855863..4b3c236 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 a81e0a0..9bd7046 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 a51f6d2..56f5221 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 d3883fa..59632e3 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 1e8e196..bf74169 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 a4bb99b..890673c 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 0821281..4e3e3cc 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 8a16b2e..889875f 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 a9fba98..0e10cf5 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 84f83d7..d9561ce 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 e4718a8..2be9550 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -95,9 +95,9 @@ public class SwerveDrive
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
- * {@link SwerveDrive#setModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it takes
- * a translation and rotation, as well as parameters for field-centric and closed-loop velocity control.
- * {@link SwerveDrive#setModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules.
+ * {@link SwerveDrive#setRawModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it
+ * takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control.
+ * {@link SwerveDrive#setRawModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules.
* This subsystem also handles odometry.
*
* @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off of.
@@ -246,7 +246,7 @@ public class SwerveDrive
// Calculate required module states via kinematics
SwerveModuleState2[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity);
- setModuleStates(swerveModuleStates, isOpenLoop);
+ setRawModuleStates(swerveModuleStates, isOpenLoop);
}
/**
@@ -255,7 +255,7 @@ public class SwerveDrive
* @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)
+ private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
{
// Desaturates wheel speeds
SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
@@ -284,6 +284,17 @@ public class SwerveDrive
}
}
+ /**
+ * Set the module states (azimuth and velocity) directly. Used primarily for auto paths.
+ *
+ * @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)
+ {
+ setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), isOpenLoop);
+ }
+
/**
* Set chassis speeds with closed-loop velocity control.
*
@@ -295,7 +306,7 @@ public class SwerveDrive
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
- setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
+ setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
}
/**
@@ -539,6 +550,9 @@ public class SwerveDrive
swerveModule.setDesiredState(desiredState, false, true);
}
+
+ // Update kinematics because we are not using setModuleStates
+ kinematics.toSwerveModuleStates(new ChassisSpeeds());
}
/**
@@ -649,7 +663,7 @@ public class SwerveDrive
{
for (SwerveModule module : swerveModules)
{
- module.synchronizeEncoders();
+ module.queueSynchronizeEncoders();
}
}
@@ -679,25 +693,17 @@ public class SwerveDrive
swerveDrivePoseEstimator.resetPosition(
robotPose.getRotation(), getModulePositions(), robotPose);
}
-
- if (!SwerveDriveTelemetry.isSimulation)
- {
- imu.setOffset(new Rotation3d(0, 0, swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians()));
- // Yaw reset recommended by Team 1622
- } else
- {
- simIMU.setAngle(swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getRadians());
- }
}
/**
- * Set the Gyroscope offset using a {@link Rotation3d} object.
+ * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new
+ * {@link Rotation3d}.
*
- * @param gyro Gyroscope offset.
+ * @param gyro expected gyroscope angle.
*/
public void setGyro(Rotation3d gyro)
{
- imu.setOffset(gyro);
+ imu.setOffset(imu.getRawRotation3d().minus(gyro));
}
/**
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 0f25819..0d3b1e2 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -55,6 +55,10 @@ public class SwerveModule
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
+ /**
+ * Encoder synchronization queued.
+ */
+ private boolean synchronizeEncoderQueued = false;
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
@@ -95,7 +99,7 @@ public class SwerveModule
{
absoluteEncoder.factoryDefault();
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
- angleMotor.setPosition(getAbsolutePosition() - angleOffset);
+ angleMotor.setPosition(getAbsolutePosition());
}
// Config angle motor/controller
@@ -125,18 +129,19 @@ public class SwerveModule
}
/**
- * Synchronize the integrated angle encoder with the absolute encoder.
+ * Queue synchronization of the integrated angle encoder with the absolute encoder.
*/
- public void synchronizeEncoders()
+ public void queueSynchronizeEncoders()
{
if (absoluteEncoder != null)
{
- angleMotor.setPosition(getAbsolutePosition() - angleOffset);
+ synchronizeEncoderQueued = true;
}
}
/**
- * Set the desired state of the swerve module.
+ * Set the desired state of the swerve module. WARNING: If you are not using one of the functions from
+ * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics}
*
* @param desiredState Desired swerve module state.
* @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
@@ -186,10 +191,22 @@ public class SwerveModule
// Prevents module rotation if speed is less than 1%
angle = Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ? lastAngle : angle;
// Prevent module rotation if angle is the same as the previous angle.
- if (angle != lastAngle)
+ if (angle != lastAngle || synchronizeEncoderQueued)
{
- angleMotor.setReference(
- angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
+ // Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
+ if (absoluteEncoder != null && synchronizeEncoderQueued)
+ {
+ double absoluteEncoderPosition = getAbsolutePosition();
+ angleMotor.setPosition(absoluteEncoderPosition);
+ angleMotor.setReference(angle,
+ Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV,
+ absoluteEncoderPosition);
+ synchronizeEncoderQueued = false;
+ } else
+ {
+ angleMotor.setReference(
+ angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
+ }
}
}
lastAngle = angle;
@@ -260,22 +277,29 @@ public class SwerveModule
/**
* Get the absolute position. Falls back to relative position on reading failure.
*
- * @return Absolute encoder angle in degrees.
+ * @return Absolute encoder angle in degrees in the range [0, 360).
*/
public double getAbsolutePosition()
{
+ double angle;
if (absoluteEncoder != null)
{
- double angle = absoluteEncoder.getAbsolutePosition();
+ angle = absoluteEncoder.getAbsolutePosition() - angleOffset;
if (absoluteEncoder.readingError)
{
angle = getRelativePosition();
}
-
- return angle;
+ } else
+ {
+ angle = getRelativePosition();
+ }
+ angle %= 360;
+ if (angle < 0.0)
+ {
+ angle += 360;
}
- return getRelativePosition();
+ return angle;
}
/**
diff --git a/swervelib/math/SwerveKinematics2.java b/swervelib/math/SwerveKinematics2.java
index c0ea077..72798e9 100644
--- a/swervelib/math/SwerveKinematics2.java
+++ b/swervelib/math/SwerveKinematics2.java
@@ -9,6 +9,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Timer;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
@@ -48,10 +49,14 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
/**
* Previous CoR
*/
- private Translation2d m_prevCoR = new Translation2d();
+ private Translation2d m_prevCoR = new Translation2d();
+ private ChassisSpeeds m_prevChassisSpeeds = new ChassisSpeeds();
+ private final Timer m_moduleAccelTimer = new Timer();
+ private double m_prevModuleAccelTime = 0.0;
/**
- * Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as Translation2ds.
+ * 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.
@@ -82,6 +87,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
}
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+ m_moduleAccelTimer.start();
MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
}
@@ -159,11 +165,13 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
}
/**
- * Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used
+ * Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often
+ * used
* to convert joystick values into module speeds and angles.
*
*
This function also supports variable centers of rotation. During normal operations, the
- * center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to
+ * center of rotation is usually the same as the physical center of the robot; therefore, the argument is
+ * defaulted to
* that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or
* for any other use case, you can do so.
*
@@ -171,8 +179,10 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
* the previously calculated module angle will be maintained.
*
* @param chassisSpeeds The desired chassis speed.
- * @param centerOfRotationMeters The center of rotation. For example, if you set the center of rotation at one corner
- * of the robot and provide a chassis speed that only has a dtheta component, the robot
+ * @param centerOfRotationMeters The center of rotation. For example, if you set the center of rotation at one
+ * corner
+ * of the robot and provide a chassis speed that only has a dtheta component, the
+ * robot
* will rotate around that corner.
* @return An array containing the module states. Use caution because these module states are not normalized.
* Sometimes, a user input may cause one of the module speeds to go above the attainable max velocity. Use the
@@ -182,6 +192,17 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
public SwerveModuleState2[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters)
{
+ var time = m_moduleAccelTimer.get();
+ var dt = time - m_prevModuleAccelTime;
+ m_prevModuleAccelTime = time;
+
+ var accelChassisSpeeds = new ChassisSpeeds(
+ (chassisSpeeds.vxMetersPerSecond - m_prevChassisSpeeds.vxMetersPerSecond) / dt,
+ (chassisSpeeds.vyMetersPerSecond - m_prevChassisSpeeds.vyMetersPerSecond) / dt,
+ (chassisSpeeds.omegaRadiansPerSecond - m_prevChassisSpeeds.omegaRadiansPerSecond) / dt
+ );
+ m_prevChassisSpeeds = chassisSpeeds;
+
if (chassisSpeeds.vxMetersPerSecond == 0.0
&& chassisSpeeds.vyMetersPerSecond == 0.0
&& chassisSpeeds.omegaRadiansPerSecond == 0.0)
@@ -235,7 +256,11 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
var accelerationVector = new SimpleMatrix(4, 1);
- accelerationVector.setColumn(0, 0, 0, 0, Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), 0);
+ accelerationVector.setColumn(0, 0,
+ accelChassisSpeeds.vxMetersPerSecond,
+ accelChassisSpeeds.vyMetersPerSecond,
+ chassisSpeeds.omegaRadiansPerSecond * chassisSpeeds.omegaRadiansPerSecond,
+ accelChassisSpeeds.omegaRadiansPerSecond);
var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector);
diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
index a5d1a2a..fd03677 100644
--- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java
+++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -344,6 +344,19 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
feedforward);
}
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in meters per second or angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ * @param position Only used on the angle motor, the position of the motor in degrees.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward, double position)
+ {
+ setReference(setpoint, feedforward);
+ }
+
/**
* Get the velocity of the integrated encoder.
*
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index 8d5ed48..e07b506 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -302,6 +302,19 @@ public class SparkMaxSwerve extends SwerveMotor
feedforward);
}
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in meters per second or angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ * @param position Only used on the angle motor, the position of the motor in degrees.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward, double position)
+ {
+ setReference(setpoint, feedforward);
+ }
+
/**
* Get the velocity of the integrated encoder.
*
diff --git a/swervelib/motors/SwerveMotor.java b/swervelib/motors/SwerveMotor.java
index 2a90b59..415b79e 100644
--- a/swervelib/motors/SwerveMotor.java
+++ b/swervelib/motors/SwerveMotor.java
@@ -88,6 +88,15 @@ public abstract class SwerveMotor
*/
public abstract void setReference(double setpoint, double feedforward);
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in meters per second or angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ * @param position Only used on the angle motor, the position of the motor in degrees.
+ */
+ public abstract void setReference(double setpoint, double feedforward, double position);
+
/**
* Get the velocity of the integrated encoder.
*
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index 54e381e..3e5187d 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -318,12 +318,13 @@ public class TalonFXSwerve extends SwerveMotor
* Convert the setpoint into native sensor units.
*
* @param setpoint Setpoint to mutate. In meters per second or degrees.
+ * @param position Position in degrees, only used on angle motors.
* @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick.
*/
- public double convertToNativeSensorUnits(double setpoint)
+ public double convertToNativeSensorUnits(double setpoint, double position)
{
setpoint =
- isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getPosition(), setpoint);
+ isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(position, setpoint);
return setpoint / positionConversionFactor;
}
@@ -335,6 +336,19 @@ public class TalonFXSwerve extends SwerveMotor
*/
@Override
public void setReference(double setpoint, double feedforward)
+ {
+ setReference(setpoint, feedforward, getPosition());
+ }
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in meters per second or angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ * @param position Only used on the angle motor, the position of the motor in degrees.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward, double position)
{
if (SwerveDriveTelemetry.isSimulation)
{
@@ -345,7 +359,7 @@ public class TalonFXSwerve extends SwerveMotor
motor.set(
isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
- convertToNativeSensorUnits(setpoint),
+ convertToNativeSensorUnits(setpoint, position),
DemandType.ArbitraryFeedForward,
feedforward / nominalVoltage);
}
@@ -382,7 +396,7 @@ public class TalonFXSwerve extends SwerveMotor
{
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
- position = position < 0 ? (position % 360) + 360 : position; // Fixes initial 360 movement.
+ position = position < 0 ? (position % 360) + 360 : position;
motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 250);
}
}
diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java
index 3b29087..d95b983 100644
--- a/swervelib/motors/TalonSRXSwerve.java
+++ b/swervelib/motors/TalonSRXSwerve.java
@@ -308,12 +308,13 @@ public class TalonSRXSwerve extends SwerveMotor
* Convert the setpoint into native sensor units.
*
* @param setpoint Setpoint to mutate. In meters per second or degrees.
+ * @param position Position in degrees, only used on angle motors.
* @return Setpoint as native sensor units. Encoder ticks per 100ms, or Encoder tick.
*/
- public double convertToNativeSensorUnits(double setpoint)
+ public double convertToNativeSensorUnits(double setpoint, double position)
{
setpoint =
- isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getPosition(), setpoint);
+ isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(position, setpoint);
return setpoint / positionConversionFactor;
}
@@ -325,6 +326,19 @@ public class TalonSRXSwerve extends SwerveMotor
*/
@Override
public void setReference(double setpoint, double feedforward)
+ {
+ setReference(setpoint, feedforward, getPosition());
+ }
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in meters per second or angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ * @param position Only used on the angle motor, the position of the motor in degrees.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward, double position)
{
if (SwerveDriveTelemetry.isSimulation)
{
@@ -335,7 +349,7 @@ public class TalonSRXSwerve extends SwerveMotor
motor.set(
isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
- convertToNativeSensorUnits(setpoint),
+ convertToNativeSensorUnits(setpoint, position),
DemandType.ArbitraryFeedForward,
feedforward / nominalVoltage);
}
@@ -359,7 +373,19 @@ public class TalonSRXSwerve extends SwerveMotor
@Override
public double getPosition()
{
- return motor.getSelectedSensorPosition() * positionConversionFactor;
+ if (isDriveMotor)
+ {
+ return motor.getSelectedSensorPosition() * positionConversionFactor;
+ } else
+ {
+ var pos = motor.getSelectedSensorPosition() * positionConversionFactor;
+ pos %= 360;
+ if (pos < 360)
+ {
+ pos += 360;
+ }
+ return pos;
+ }
}
/**
@@ -372,7 +398,6 @@ public class TalonSRXSwerve extends SwerveMotor
{
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
- position = position < 0 ? (position % 360) + 360 : position; // Fixes initial 360 movement.
motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 250);
}
}