Rotation2d.SlewRateLimiter for angular movement in radians/second.PhysicalPropertiesJson.moduleFeedForwardClosedLoop instead.SwerveModule JSON parsed class.SwerveModuleState2 to be the closest angle to the current angle.SwerveModule.SwerveModuleState2 based on the SwerveModuleState with the radians per second defined.SwerveModuleState.SwerveAbsoluteEncoder from the data port on the motor controller.voidsetChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds) voidsetGyro(edu.wpi.first.math.geometry.Rotation3d gyro) attainableMaxModuleSpeedMetersPerSecond - The absolute max speed that a module can reach in meters per second.attainableMaxModuleSpeedMetersPerSecond - The absolute max speed that a module can reach in meters per
+ second.attainableMaxTranslationalSpeedMetersPerSecond - The absolute max speed that your robot can reach while
translating in meters per second.attainableMaxRotationalVelocityRadiansPerSecond - The absolute max speed the robot can reach while rotating in radians per second.attainableMaxRotationalVelocityRadiansPerSecond - The absolute max speed the robot can reach while rotating in
+ radians per second.chassisSpeeds - Chassis speeds to set.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. @@ -391,10 +388,8 @@ loadScripts(document, 'script');
toSwerveModuleStates in class edu.wpi.first.math.kinematics.SwerveDriveKinematicschassisSpeeds - 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.static doublecalculateAngleKV(double optimalVoltage,
- double motorFreeSpeedRPM,
- double angleGearRatio) static doublecalculateDegreesPerSteeringRotation(double angleGearRatio,
+calculateDegreesPerSteeringRotation(double angleGearRatio,
double pulsePerRotation)
-
+
Calculate the degrees per steering rotation for the integrated encoder.
-static double
-calculateMaxAcceleration(double cof)
-
+static double
+calculateMaxAcceleration(double cof)
+
Calculate the practical maximum acceleration of the robot using the wheel coefficient of friction.
-static double
-calculateMaxAcceleration(double stallTorqueNm,
+static double
+calculateMaxAcceleration(double stallTorqueNm,
double gearRatio,
double moduleCount,
double wheelDiameter,
double robotMass)
-
+
Calculate the maximum theoretical acceleration without friction.
-static double
-calculateMaxAngularVelocity(double maxSpeed,
+static double
+calculateMaxAngularVelocity(double maxSpeed,
double furthestModuleX,
double furthestModuleY)
-
+
Calculate the maximum angular velocity.
-static double
-calculateMetersPerRotation(double wheelDiameter,
+static double
+calculateMetersPerRotation(double wheelDiameter,
double driveGearRatio,
double pulsePerRotation)
-
+
Calculate the meters per rotation for the integrated encoder.
-static SwerveModuleConfiguration
-getSwerveModule(SwerveModule[] modules,
+static SwerveModuleConfiguration
+getSwerveModule(SwerveModule[] modules,
boolean front,
boolean left)
-
+
Get the fruthest module from center based on the module locations.
-static edu.wpi.first.math.geometry.Translation2d
-limitVelocity(edu.wpi.first.math.geometry.Translation2d commandedVelocity,
+static edu.wpi.first.math.geometry.Translation2d
+limitVelocity(edu.wpi.first.math.geometry.Translation2d commandedVelocity,
edu.wpi.first.math.kinematics.ChassisSpeeds fieldVelocity,
edu.wpi.first.math.geometry.Pose2d robotPose,
double loopTime,
double robotMass,
List<Matter> matter,
SwerveDriveConfiguration config)
-
+
Limits a commanded velocity to prevent exceeding the maximum acceleration given by calcMaxAccel(edu.wpi.first.math.geometry.Rotation2d, java.util.List<swervelib.math.Matter>, double, swervelib.parser.SwerveDriveConfiguration).
-static double
-normalizeAngle(double angle)
-
-Normalize an angle to be within 0 to 360.
-
-static SwerveModuleState2
-optimize(SwerveModuleState2 desiredState,
- edu.wpi.first.math.geometry.Rotation2d currentAngle,
- double secondOrderOffsetDegrees)
+static double
+normalizeAngle(double angle)
-Optimize the angle of the SwerveModuleState2 to be the closest angle to the current angle.
+Normalize an angle to be within 0 to 360.
static double
placeInAppropriate0To360Scope(double scopeReference,
@@ -205,6 +191,11 @@ loadScripts(document, 'script');
Put an angle within the 360 deg scope of a reference.
+static edu.wpi.first.math.geometry.Twist2d
+PoseLog(edu.wpi.first.math.geometry.Pose2d transform)
+
+Logical inverse of the Pose exponential from 254.
+
@@ -237,24 +228,6 @@ loadScripts(document, 'script');
Method Details
-
-
-calculateAngleKV
-public static double calculateAngleKV(double optimalVoltage,
- double motorFreeSpeedRPM,
- double angleGearRatio)
-Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
- degree == (maxVolts) / (maxSpeed)
-
-- Parameters:
-optimalVoltage - Optimal voltage to use when calculating the angle kV.motorFreeSpeedRPM - Motor free speed in Rotations per Minute.angleGearRatio - Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.- Returns:
-- angle kV for feedforward.
-
-
-
--
calculateMetersPerRotation
public static double calculateMetersPerRotation(double wheelDiameter,
@@ -391,6 +364,17 @@ loadScripts(document, 'script');
-
+
+PoseLog
+public static edu.wpi.first.math.geometry.Twist2d PoseLog(edu.wpi.first.math.geometry.Pose2d transform)
+Logical inverse of the Pose exponential from 254. Taken from team 3181.
+
+- Parameters:
+transform - Pose to perform the log on.
+
+
+-
limitVelocity
public static edu.wpi.first.math.geometry.Translation2d limitVelocity(edu.wpi.first.math.geometry.Translation2d commandedVelocity,
@@ -436,25 +420,6 @@ loadScripts(document, 'script');
-
-
-optimize
-public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState,
- edu.wpi.first.math.geometry.Rotation2d currentAngle,
- double secondOrderOffsetDegrees)
-Optimize the angle of the SwerveModuleState2 to be the closest angle to the current angle. Taken from Team
- 3181 at
- https://github.com/pittsfordrobotics/REVSwerve2023/blob/master/src/main/java/com/team3181/lib/swerve/SwerveOptimizer.java
-
-- Parameters:
-desiredState - Desired SwerveModuleState2 to achieve.currentAngle - Current angle as a Rotation2d.secondOrderOffsetDegrees - Offset calculated using 2nd order kinematics.- Returns:
-- Optimized
SwerveModuleState2
-
-
-
--
placeInAppropriate0To360Scope
public static double placeInAppropriate0To360Scope(double scopeReference,
diff --git a/docs/swervelib/math/SwerveModuleState2.html b/docs/swervelib/math/SwerveModuleState2.html
index 267f3fd..834eca5 100644
--- a/docs/swervelib/math/SwerveModuleState2.html
+++ b/docs/swervelib/math/SwerveModuleState2.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleState2
-
+
@@ -16,7 +16,11 @@
-
+
+Fields inherited from class edu.wpi.first.math.kinematics.SwerveModuleState
+angle, speedMetersPerSecond
@@ -129,6 +126,11 @@ loadScripts(document, 'script');
Constructs a SwerveModuleState.
+SwerveModuleState2(edu.wpi.first.math.kinematics.SwerveModuleState state,
+ double omegaRadPerSecond)
+
+Create a SwerveModuleState2 based on the SwerveModuleState with the radians per second defined.
+
@@ -136,6 +138,30 @@ loadScripts(document, 'script');
-
Method Summary
+
+
+
+
+Modifier and Type
+Method
+Description
+static SwerveModuleState2
+optimize(SwerveModuleState2 desiredState,
+ edu.wpi.first.math.geometry.Rotation2d currentAngle,
+ SwerveModuleState2 lastState,
+ double moduleSteerFeedForwardClosedLoop)
+
+Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
+ the wheel spins.
+
+edu.wpi.first.math.kinematics.SwerveModuleState
+
+
+Convert to a SwerveModuleState.
+
+
+
+
Methods inherited from class edu.wpi.first.math.kinematics.SwerveModuleState
compareTo, equals, hashCode, optimize, toString
@@ -154,26 +180,12 @@ loadScripts(document, 'script');
Field Details
-
-
-speedMetersPerSecond
-public double speedMetersPerSecond
-Swerve module speed in meters per second.
-
-
--
omegaRadPerSecond
public double omegaRadPerSecond
Rad per sec
--
-
-angle
-public edu.wpi.first.math.geometry.Rotation2d angle
-Swerve module angle as a Rotation2d.
-
-
@@ -204,6 +216,59 @@ loadScripts(document, 'script');
+-
+
+SwerveModuleState2
+public SwerveModuleState2(edu.wpi.first.math.kinematics.SwerveModuleState state,
+ double omegaRadPerSecond)
+Create a SwerveModuleState2 based on the SwerveModuleState with the radians per second defined.
+
+- Parameters:
+state - First order kinematic module state.omegaRadPerSecond - Module wheel angular rotation in radians per second.
+
+
+
+
+
+
+-
+
+Method Details
+
+-
+
+optimize
+public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState,
+ edu.wpi.first.math.geometry.Rotation2d currentAngle,
+ SwerveModuleState2 lastState,
+ double moduleSteerFeedForwardClosedLoop)
+Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
+ the wheel spins. If this is used with the PIDController class's continuous input functionality, the furthest a
+ wheel will ever rotate is 90 degrees.
+
+- Parameters:
+desiredState - The desired state.currentAngle - The current module angle.lastState - The last state of the module.moduleSteerFeedForwardClosedLoop - The module feed forward closed loop for the angle motor.- Returns:
+- Optimized swerve module state.
+
+
+
+-
+
+toSwerveModuleState
+public edu.wpi.first.math.kinematics.SwerveModuleState toSwerveModuleState()
+Convert to a SwerveModuleState.
+
+- Returns:
+SwerveModuleState with the same angle and speed.
+
+
diff --git a/docs/swervelib/math/package-summary.html b/docs/swervelib/math/package-summary.html
index 8b755fd..638441b 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 b1d4142..f745b32 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 e61f0c5..4d6d2e9 100644
--- a/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
+++ b/docs/swervelib/motors/SparkMaxBrushedMotorSwerve.html
@@ -1,11 +1,11 @@
-
+
SparkMaxBrushedMotorSwerve
-
+
diff --git a/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html b/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
index 17c1cac..c91ea84 100644
--- a/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
+++ b/docs/swervelib/motors/SparkMaxSwerve.SparkMAX_slotIdx.html
@@ -1,11 +1,11 @@
-
+
SparkMaxSwerve.SparkMAX_slotIdx
-
+
diff --git a/docs/swervelib/motors/SparkMaxSwerve.html b/docs/swervelib/motors/SparkMaxSwerve.html
index 3d02405..55cc0d3 100644
--- a/docs/swervelib/motors/SparkMaxSwerve.html
+++ b/docs/swervelib/motors/SparkMaxSwerve.html
@@ -1,11 +1,11 @@
-
+
SparkMaxSwerve
-
+
diff --git a/docs/swervelib/motors/SwerveMotor.html b/docs/swervelib/motors/SwerveMotor.html
index 01d3169..757b059 100644
--- a/docs/swervelib/motors/SwerveMotor.html
+++ b/docs/swervelib/motors/SwerveMotor.html
@@ -1,11 +1,11 @@
-
+
SwerveMotor
-
+
diff --git a/docs/swervelib/motors/TalonFXSwerve.html b/docs/swervelib/motors/TalonFXSwerve.html
index 9ad6cca..b2b2861 100644
--- a/docs/swervelib/motors/TalonFXSwerve.html
+++ b/docs/swervelib/motors/TalonFXSwerve.html
@@ -1,11 +1,11 @@
-
+
TalonFXSwerve
-
+
diff --git a/docs/swervelib/motors/TalonSRXSwerve.html b/docs/swervelib/motors/TalonSRXSwerve.html
index ba43d6c..f18d31b 100644
--- a/docs/swervelib/motors/TalonSRXSwerve.html
+++ b/docs/swervelib/motors/TalonSRXSwerve.html
@@ -1,11 +1,11 @@
-
+
TalonSRXSwerve
-
+
diff --git a/docs/swervelib/motors/package-summary.html b/docs/swervelib/motors/package-summary.html
index 2560ad3..14b1f7c 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 cd22dae..207af10 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 11e89ba..376c6ad 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 c3ac62e..efdb3cd 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 ecda3f1..fdd116b 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 9d6eefc..fb24cee 100644
--- a/docs/swervelib/parser/SwerveControllerConfiguration.html
+++ b/docs/swervelib/parser/SwerveControllerConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveControllerConfiguration
-
+
@@ -151,20 +151,6 @@ loadScripts(document, 'script');
Field Details
-
-
-maxSpeed
-public double maxSpeed
-Maximum robot speed in meters per second.
-
-
--
-
-maxAngularVelocity
-public double maxAngularVelocity
-Maximum angular velocity in rad/s
-
-
--
headingPIDF
@@ -178,6 +164,20 @@ loadScripts(document, 'script');
hypotenuse deadband for the robot angle control joystick.
+-
+
+maxSpeed
+public double maxSpeed
+Maximum robot speed in meters per second.
+
+
+-
+
+maxAngularVelocity
+public double maxAngularVelocity
+Maximum angular velocity in rad/s
+
+
diff --git a/docs/swervelib/parser/SwerveDriveConfiguration.html b/docs/swervelib/parser/SwerveDriveConfiguration.html
index af66a0d..0e4d302 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 547a18f..e7dbf45 100644
--- a/docs/swervelib/parser/SwerveModuleConfiguration.html
+++ b/docs/swervelib/parser/SwerveModuleConfiguration.html
@@ -1,11 +1,11 @@
-
+
SwerveModuleConfiguration
-
+
@@ -101,64 +101,69 @@ loadScripts(document, 'script');
Whether the absolute encoder is inverted.
-double
-
+
+
-Angle volt-meter-per-second.
-
-
-
-
The drive motor and angle motor of this swerve module.
-double
-
-
+double
+
+
The integrated encoder pulse per revolution.
-final boolean
-
-
+final boolean
+
+
State of inversion of the angle motor.
-final double
-
-
+final double
+
+
Angle offset in degrees for the Swerve Module.
-
-
-
+
+
+
PIDF configuration options for the angle motor closed-loop PID controller.
-
-
-
+
+
+
The drive motor and angle motor of this swerve module.
-final boolean
-
-
+final boolean
+
+
State of inversion of the drive motor.
-double
-
-
+double
+
+
Maximum robot speed in meters per second.
-edu.wpi.first.math.geometry.Translation2d
-
-
+edu.wpi.first.math.geometry.Translation2d
+
+
Swerve module location relative to the robot.
-
-
+double
+
+
+Angle volt-meter-per-second.
+
+
+
+Name for the swerve module for telemetry.
+
+
+
+
Physical characteristics of the swerve module.
-
-
-
+
+
+
PIDF configuration options for the drive motor closed-loop PID controller.
@@ -172,20 +177,7 @@ loadScripts(document, 'script');
Constructor
Description
-SwerveModuleConfiguration(SwerveMotor driveMotor,
- SwerveMotor angleMotor,
- SwerveAbsoluteEncoder absoluteEncoder,
- double angleOffset,
- double xMeters,
- double yMeters,
- PIDFConfig anglePIDF,
- PIDFConfig velocityPIDF,
- double maxSpeed,
- SwerveModulePhysicalCharacteristics physicalCharacteristics)
-
-Construct a configuration object for swerve modules.
-
-SwerveModuleConfiguration(SwerveMotor driveMotor,
+SwerveModuleConfiguration(SwerveMotor driveMotor,
SwerveMotor angleMotor,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
@@ -199,7 +191,21 @@ loadScripts(document, 'script');
boolean driveMotorInverted,
boolean angleMotorInverted,
double angleMotorEncoderPulsePerRevolution,
- double angleMotorFreeSpeedRPM)
+ String name)
+
+Construct a configuration object for swerve modules.
+
+SwerveModuleConfiguration(SwerveMotor driveMotor,
+ SwerveMotor angleMotor,
+ SwerveAbsoluteEncoder absoluteEncoder,
+ double angleOffset,
+ double xMeters,
+ double yMeters,
+ PIDFConfig anglePIDF,
+ PIDFConfig velocityPIDF,
+ double maxSpeed,
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ String name)
Construct a configuration object for swerve modules.
@@ -294,9 +300,9 @@ loadScripts(document, 'script');
-
-
-angleKV
-public double angleKV
+
+moduleSteerFFCL
+public double moduleSteerFFCL
Angle volt-meter-per-second.
@@ -342,6 +348,13 @@ loadScripts(document, 'script');
The Absolute Encoder for the swerve module.
+-
+
+name
+
+Name for the swerve module for telemetry.
+
+
@@ -351,7 +364,7 @@ loadScripts(document, 'script');
Constructor Details
-
-
+
SwerveModuleConfiguration
public SwerveModuleConfiguration(SwerveMotor driveMotor,
SwerveMotor angleMotor,
@@ -367,7 +380,7 @@ loadScripts(document, 'script');
boolean driveMotorInverted,
boolean angleMotorInverted,
double angleMotorEncoderPulsePerRevolution,
- double angleMotorFreeSpeedRPM)
+ String name)
Construct a configuration object for swerve modules.
- Parameters:
@@ -385,12 +398,11 @@ loadScripts(document, 'script');
maxSpeed - Maximum speed in meters per second.physicalCharacteristics - Physical characteristics of the swerve module.angleMotorEncoderPulsePerRevolution - The encoder pulse per revolution for the angle motor encoder.angleMotorFreeSpeedRPM - The free speed RPM of the angle motor.
-
-
+
SwerveModuleConfiguration
public SwerveModuleConfiguration(SwerveMotor driveMotor,
SwerveMotor angleMotor,
@@ -401,7 +413,8 @@ loadScripts(document, 'script');
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
- SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ String name)
Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not
inverted.
@@ -416,6 +429,7 @@ loadScripts(document, 'script');
velocityPIDF - Velocity PIDF configuration.maxSpeed - Maximum robot speed in meters per second.physicalCharacteristics - Physical characteristics of the swerve module.name - Name for the module.
diff --git a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
index 82fb68c..7e6d68c 100644
--- a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
+++ b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
@@ -1,11 +1,11 @@
-
+
SwerveModulePhysicalCharacteristics
-
+
@@ -103,17 +103,6 @@ loadScripts(document, 'script');
Current limits for the Swerve Module.
final double
-
-
-Free speed rotations per minute of the motor, as described by the vendor.
-
-final double
-
-
-Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
- your drive train does not drift towards the direction you are rotating while you translate.
-
-final double
The time it takes for the motor to go from 0 to full throttle in seconds.
@@ -139,18 +128,24 @@ loadScripts(document, 'script');
The time it takes for the motor to go from 0 to full throttle in seconds.
final double
-
+
-Optimal voltage of the robot.
+Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
+ your drive train does not drift towards the direction you are rotating while you translate.
final double
-
+
-Wheel diameter in meters.
+Optimal voltage of the robot.
final double
-
+
+Wheel diameter in meters.
+
+final double
+
+
Wheel grip tape coefficient of friction on carpet, as described by the vendor.
@@ -164,9 +159,8 @@ loadScripts(document, 'script');
Constructor
Description
-SwerveModulePhysicalCharacteristics(double driveGearRatio,
+SwerveModulePhysicalCharacteristics(double driveGearRatio,
double angleGearRatio,
- double angleMotorFreeSpeedRPM,
double wheelDiameter,
double driveMotorRampRate,
double angleMotorRampRate,
@@ -175,9 +169,8 @@ loadScripts(document, 'script');
Construct the swerve module physical characteristics.
-SwerveModulePhysicalCharacteristics(double driveGearRatio,
+SwerveModulePhysicalCharacteristics(double driveGearRatio,
double angleGearRatio,
- double angleMotorFreeSpeedRPM,
double wheelDiameter,
double wheelGripCoefficientOfFriction,
double optimalVoltage,
@@ -187,7 +180,7 @@ loadScripts(document, 'script');
double angleMotorRampRate,
int driveEncoderPulsePerRotation,
int angleEncoderPulsePerRotation,
- double angleMotorKV)
+ double moduleSteerFFCL)
Construct the swerve module physical characteristics.
@@ -290,16 +283,9 @@ loadScripts(document, 'script');
-
-
-angleMotorFreeSpeedRPM
-public final double angleMotorFreeSpeedRPM
-Free speed rotations per minute of the motor, as described by the vendor.
-
-
--
-
-angleMotorKV
-public final double angleMotorKV
+
+moduleSteerFFCL
+public final double moduleSteerFFCL
Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
your drive train does not drift towards the direction you are rotating while you translate. When set to 0 the
calculated kV will be used.
@@ -314,11 +300,10 @@ loadScripts(document, 'script');
Constructor Details
-
-
+
SwerveModulePhysicalCharacteristics
public SwerveModulePhysicalCharacteristics(double driveGearRatio,
double angleGearRatio,
- double angleMotorFreeSpeedRPM,
double wheelDiameter,
double wheelGripCoefficientOfFriction,
double optimalVoltage,
@@ -328,14 +313,13 @@ loadScripts(document, 'script');
double angleMotorRampRate,
int driveEncoderPulsePerRotation,
int angleEncoderPulsePerRotation,
- double angleMotorKV)
+ double moduleSteerFFCL)
Construct the swerve module physical characteristics.
- Parameters:
driveGearRatio - Gear ratio of the drive motor. Number of motor rotations to rotate the
wheel.angleGearRatio - Gear ratio of the angle motor. Number of motor rotations to spin the wheel.angleMotorFreeSpeedRPM - Motor free speed rotation per minute.wheelDiameter - Wheel diameter in meters.wheelGripCoefficientOfFriction - Wheel grip coefficient of friction on carpet given by manufacturer.optimalVoltage - Optimal robot voltage.driveEncoderPulsePerRotation - The number of encoder pulses per motor rotation, 1 for integrated encoders.angleEncoderPulsePerRotation - The number of encoder pulses per motor rotation, 1 for integrated encoders.angleMotorKV - The kV applied to the steering motor to ensure your drivetrain does not drift
+moduleSteerFFCL - The kV applied to the steering motor to ensure your drivetrain does not drift
towards a direction when rotating while translating.
-
-
+
SwerveModulePhysicalCharacteristics
public SwerveModulePhysicalCharacteristics(double driveGearRatio,
double angleGearRatio,
- double angleMotorFreeSpeedRPM,
double wheelDiameter,
double driveMotorRampRate,
double angleMotorRampRate,
@@ -370,7 +353,6 @@ loadScripts(document, 'script');
- Parameters:
driveGearRatio - Gear ratio of the drive motor. Number of motor rotations to rotate the wheel.angleGearRatio - Gear ratio of the angle motor. Number of motor rotations to spin the wheel.angleMotorFreeSpeedRPM - Motor free speed rotation per minute.wheelDiameter - Wheel diameter in meters.driveMotorRampRate - The time in seconds to go from 0 to full throttle on the motor. (Prevents over
drawing power from battery)SwerveParser
-
+
diff --git a/docs/swervelib/parser/deserializer/PIDFRange.html b/docs/swervelib/parser/deserializer/PIDFRange.html
index 1b41a70..025b276 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 f45bc05..9a834d3 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 ca1dd74..ab52627 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 81b44b6..4264d01 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 fb6f43f..63e8c20 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 d3bc148..301dcae 100644
--- a/docs/swervelib/parser/json/ModuleJson.html
+++ b/docs/swervelib/parser/json/ModuleJson.html
@@ -1,11 +1,11 @@
-
+
ModuleJson
-
+
@@ -111,29 +111,24 @@ loadScripts(document, 'script');
The angle encoder pulse per revolution override.
-double
-
+
+
-Angle motor free speed RPM.
-
-
-
-
Drive motor device configuration.
-
-
-
+
+
+
Absolute encoder device configuration.
-
-
-
+
+
+
Defines which motors are inverted.
-
-
-
@@ -164,10 +159,11 @@ loadScripts(document, 'script');
Method
Description
-createModuleConfiguration(PIDFConfig anglePIDF,
+createModuleConfiguration(PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
- SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ String name)
Create the swerve module configuration based off of parsed data.
@@ -238,13 +234,6 @@ loadScripts(document, 'script');
-
-
-angleMotorFreeSpeedRPM
-public double angleMotorFreeSpeedRPM
-Angle motor free speed RPM.
-
-
--
location
@@ -274,12 +263,13 @@ loadScripts(document, 'script');
Method Details
-
-
+
createModuleConfiguration
public SwerveModuleConfiguration createModuleConfiguration(PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
- SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ String name)
Create the swerve module configuration based off of parsed data.
- Parameters:
@@ -287,6 +277,7 @@ loadScripts(document, 'script');
velocityPIDF - The velocity PIDF values for the drive motor.maxSpeed - The maximum speed of the robot in meters per second.physicalCharacteristics - Physical characteristics of the swerve module.name - Module json filename.- Returns:
SwerveModuleConfiguration based on the provided data and parsed data.
diff --git a/docs/swervelib/parser/json/MotorConfigDouble.html b/docs/swervelib/parser/json/MotorConfigDouble.html
index 936ad50..db9d818 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 28ca83d..798b52e 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 d6f9f58..a71a782 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 5806db5..375e419 100644
--- a/docs/swervelib/parser/json/PhysicalPropertiesJson.html
+++ b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
@@ -1,11 +1,11 @@
-
+
PhysicalPropertiesJson
-
+
@@ -94,29 +94,29 @@ loadScripts(document, 'script');
double
-Angle motor free speed rotations per minute.
-
-double
-
-
-Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
- your drive train does not drift towards the direction you are rotating while you translate.
-
-
-
-
-The current limit in AMPs to apply to the motors.
+DEPRECATED: No longer needed, tune moduleFeedForwardClosedLoop instead.
-
+
+The current limit in AMPs to apply to the motors.
+
+
+
+
Encoder pulse per rotation for non-integrated encoders.
-
-
-
+
+
+
Gear ratio for the motors, number of times the motor has to spin before the wheel rotates a single time.
+double
+
+
+Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
+ your drive train does not drift towards the direction you are rotating while you translate.
+
@@ -224,19 +224,19 @@ loadScripts(document, 'script');
-
-
-angleMotorFreeSpeedRPM
-public double angleMotorFreeSpeedRPM
-Angle motor free speed rotations per minute.
+
+moduleFeedForwardClosedLoop
+public double moduleFeedForwardClosedLoop
+Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
+ your drive train does not drift towards the direction you are rotating while you translate. Default value is 0. If
+ robot arcs while translating and rotating negate this.
-
-
-angleMotorsKV
-public double angleMotorsKV
-Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
- your drive train does not drift towards the direction you are rotating while you translate. When set to 0 the
- calculated kV will be used.
+
+angleMotorFreeSpeedRPM
+public double angleMotorFreeSpeedRPM
+DEPRECATED: No longer needed, tune moduleFeedForwardClosedLoop instead.
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index d470998..c502379 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 44653e3..17c1432 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 3dc8d1d..f98bd3c 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 b125e47..da4a656 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 bdc5791..47a8d13 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 7380795..c7fc94f 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 d75fe86..1bf9224 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 d66b4b5..ff7d729 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 bc03acf..4a33daf 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 74668e6..e373b40 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 b854a67..f7e368c 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 5031b7c..ba9597d 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 84b51c1..f636626 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 425b48c..29cb582 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 acddad8..0144707 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 4e63fa3..86a59bb 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 71c94d5..fd8dc07 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 1794908..839df8a 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 0ef4ed7..9b0982a 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 0e18cbb..bec01a4 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 44dcaf3..4c405da 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 d738d75..95309ae 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 d0eb83d..0019313 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 842078e..eda414b 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 343e357..a5896a5 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -5,7 +5,13 @@ 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.*;
+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.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
@@ -15,6 +21,9 @@ 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;
@@ -25,10 +34,6 @@ 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.
*/
@@ -217,6 +222,17 @@ public class SwerveDrive
translation.getX(), translation.getY(), rotation, getYaw())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
+ // Thank you to Jared Russell FRC254 for Open Loop Compensation Code
+ // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
+ double dtConstant = 0.009;
+ Pose2d robotPoseVel = new Pose2d(velocity.vxMetersPerSecond * dtConstant,
+ velocity.vyMetersPerSecond * dtConstant,
+ Rotation2d.fromRadians(velocity.omegaRadiansPerSecond * dtConstant));
+ Twist2d twistVel = SwerveMath.PoseLog(robotPoseVel);
+
+ velocity = new ChassisSpeeds(twistVel.dx / dtConstant, twistVel.dy / dtConstant,
+ twistVel.dtheta / dtConstant);
+
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
if (headingCorrection)
@@ -252,18 +268,23 @@ public class SwerveDrive
/**
* Set the maximum speeds for desaturation.
*
- * @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach in meters per second.
+ * @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.
+ * @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) {
+ double attainableMaxModuleSpeedMetersPerSecond,
+ double attainableMaxTranslationalSpeedMetersPerSecond,
+ double attainableMaxRotationalVelocityRadiansPerSecond)
+ {
setMaximumSpeed(attainableMaxModuleSpeedMetersPerSecond);
- swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
- swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
+ swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond =
+ attainableMaxTranslationalSpeedMetersPerSecond;
+ swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond =
+ attainableMaxRotationalVelocityRadiansPerSecond;
}
/**
@@ -272,33 +293,40 @@ 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.
*/
- private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop) {
+ private void setRawModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
+ {
// Desaturates wheel speeds
if (swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond != 0 ||
- swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond != 0)
+ swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond != 0)
+ {
SwerveKinematics2.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
- swerveDriveConfiguration.maxSpeed,
- swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
- swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
- else
+ 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.configuration.name + "] Speed Setpoint: ",
+ module.lastState.speedMetersPerSecond);
SmartDashboard.putNumber(
- "Module[" + module.moduleNumber + "] Angle Setpoint: ",
+ "Module[" + module.configuration.name + "] Angle Setpoint: ",
module.lastState.angle.getDegrees());
}
}
@@ -312,11 +340,12 @@ public class SwerveDrive
*/
public void setModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
{
- setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)), isOpenLoop);
+ setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)),
+ isOpenLoop);
}
/**
- * Set chassis speeds with closed-loop velocity control.
+ * Set chassis speeds with closed-loop velocity control and second order kinematics.
*
* @param chassisSpeeds Chassis speeds to set.
*/
@@ -691,9 +720,9 @@ public class SwerveDrive
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
- "Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
+ "Module[" + module.configuration.name + "] Relative Encoder", module.getRelativePosition());
SmartDashboard.putNumber(
- "Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
+ "Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 0caa5c8..e77a8e0 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -3,7 +3,6 @@ package swervelib;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
@@ -100,8 +99,7 @@ public class SwerveModule
}
// Config angle motor/controller
- angleMotor.configureIntegratedEncoder(
- moduleConfiguration.getPositionEncoderConversion(false));
+ angleMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(false));
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
angleMotor.configurePIDWrapping(-180, 180);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
@@ -146,16 +144,11 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop, boolean force)
{
-// SwerveModuleState simpleState =
-// new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
-// simpleState = SwerveModuleState.optimize(simpleState, getState().angle);
-// desiredState =
-// new SwerveModuleState2(
-// simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond);
- // Taken from https://github.com/pittsfordrobotics/REVSwerve2023/blob/a13156d573b6390c2130edb741cc7381c4d31583/src/main/java/com/team3181/frc2023/subsystems/swerve/Swerve.java#L101
- desiredState = SwerveMath.optimize(desiredState, getState().angle,
- Units.radiansToDegrees(lastState.omegaRadPerSecond * configuration.angleKV) *
- 0.065); // I am unsure of what the 0.065 represents
+ desiredState = SwerveModuleState2.optimize(desiredState,
+ Rotation2d.fromDegrees(getAbsolutePosition()),
+ lastState,
+ configuration.moduleSteerFFCL);
+
if (isOpenLoop)
{
double percentOutput = desiredState.speedMetersPerSecond / configuration.maxSpeed;
@@ -173,41 +166,34 @@ public class SwerveModule
if (!force)
{
// Prevents module rotation if speed is less than 1%
- SwerveMath.antiJitter(desiredState, lastState, configuration.maxSpeed);
- } else
- {
- desiredState.omegaRadPerSecond = 0;
+ SwerveMath.antiJitter(desiredState, lastState, Math.min(configuration.maxSpeed, 4));
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
- 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));
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint:", desiredState.speedMetersPerSecond);
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint:", desiredState.angle.getDegrees());
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Omega:",
+ Math.toDegrees(desiredState.omegaRadPerSecond));
}
// Prevent module rotation if angle is the same as the previous angle.
if (desiredState.angle != lastState.angle || synchronizeEncoderQueued)
{
+ double moduleFF = desiredState.omegaRadPerSecond * configuration.moduleSteerFFCL;
// 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;
if (absoluteEncoder != null && synchronizeEncoderQueued)
{
double absoluteEncoderPosition = getAbsolutePosition();
angleMotor.setPosition(absoluteEncoderPosition);
- angleMotor.setReference(desiredState.angle.getDegrees(),
- feedforward,
- absoluteEncoderPosition);
+ angleMotor.setReference(desiredState.angle.getDegrees(), moduleFF, absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
{
- angleMotor.setReference(desiredState.angle.getDegrees(),
- feedforward);
+ angleMotor.setReference(desiredState.angle.getDegrees(), moduleFF);
}
}
+
lastState = desiredState;
if (SwerveDriveTelemetry.isSimulation)
@@ -223,7 +209,7 @@ public class SwerveModule
*/
public void setAngle(double angle)
{
- angleMotor.setReference(angle, configuration.angleKV);
+ angleMotor.setReference(angle, configuration.moduleSteerFFCL);
lastState.angle = Rotation2d.fromDegrees(angle);
}
@@ -268,7 +254,7 @@ public class SwerveModule
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
- SmartDashboard.putNumber("Module " + moduleNumber + "Angle", azimuth.getDegrees());
+ SmartDashboard.putNumber("Module[" + configuration.name + "] Angle", azimuth.getDegrees());
}
return new SwerveModulePosition(position, azimuth);
}
diff --git a/swervelib/math/SwerveKinematics2.java b/swervelib/math/SwerveKinematics2.java
index 72798e9..0a20372 100644
--- a/swervelib/math/SwerveKinematics2.java
+++ b/swervelib/math/SwerveKinematics2.java
@@ -46,17 +46,16 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
* Swerve module states.
*/
private final SwerveModuleState2[] m_moduleStates;
+ private final Timer m_moduleAccelTimer = new Timer();
/**
* Previous CoR
*/
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;
+ private ChassisSpeeds m_prevChassisSpeeds = new ChassisSpeeds();
+ 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.
@@ -165,13 +164,11 @@ 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.
*
@@ -179,10 +176,8 @@ 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
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index e3022b1..a098ce4 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -4,6 +4,7 @@ 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.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.List;
@@ -20,22 +21,6 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
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.
@@ -214,6 +199,32 @@ public class SwerveMath
return maxAccel;
}
+ /**
+ * Logical inverse of the Pose exponential from 254. Taken from team 3181.
+ *
+ * @param transform Pose to perform the log on.
+ */
+ public static Twist2d PoseLog(final Pose2d transform)
+ {
+
+ final double kEps = 1E-9;
+ final double dtheta = transform.getRotation().getRadians();
+ final double half_dtheta = 0.5 * dtheta;
+ final double cos_minus_one = transform.getRotation().getCos() - 1.0;
+ double halftheta_by_tan_of_halfdtheta;
+ if (Math.abs(cos_minus_one) < kEps)
+ {
+ halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+ } else
+ {
+ halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one;
+ }
+ final Translation2d translation_part = transform.getTranslation()
+ .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta,
+ -half_dtheta));
+ return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta);
+ }
+
/**
* Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link SwerveMath#calcMaxAccel}.
* Note that this takes and returns field-relative velocities.
@@ -310,43 +321,6 @@ public class SwerveMath
return configuration;
}
- /**
- * Optimize the angle of the {@link SwerveModuleState2} to be the closest angle to the current angle. Taken from Team
- * 3181 at
- * https://github.com/pittsfordrobotics/REVSwerve2023/blob/master/src/main/java/com/team3181/lib/swerve/SwerveOptimizer.java
- *
- * @param desiredState Desired {@link SwerveModuleState2} to achieve.
- * @param currentAngle Current angle as a {@link Rotation2d}.
- * @param secondOrderOffsetDegrees Offset calculated using 2nd order kinematics.
- * @return Optimized {@link SwerveModuleState2}
- */
- public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
- double secondOrderOffsetDegrees)
- {
- double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(),
- desiredState.angle.getDegrees() + secondOrderOffsetDegrees);
- double targetSpeed = desiredState.speedMetersPerSecond;
- double delta = targetAngle - currentAngle.getDegrees();
- if (Math.abs(delta) > 90)
- {
- targetSpeed = -targetSpeed;
- if (delta > 90)
- {
- targetAngle -= 180;
- } else
- {
- targetAngle += 180;
- }
- }
- // Ensure outputted angle is positive.
- while (targetAngle < 0)
- {
- targetAngle += 360;
- }
- return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle),
- desiredState.omegaRadPerSecond);
- }
-
/**
* Put an angle within the 360 deg scope of a reference. For example, given a scope reference of 756 degrees, assumes
* the full scope is (720-1080), and places an angle of 22 degrees into it, returning 742 deg.
@@ -399,8 +373,7 @@ public class SwerveMath
if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01))
{
moduleState.angle = lastModuleState.angle;
-// moduleState.omegaRadPerSecond = lastModuleState.omegaRadPerSecond;
- moduleState.omegaRadPerSecond = 0;
+ moduleState.omegaRadPerSecond = lastModuleState.omegaRadPerSecond;
}
}
}
diff --git a/swervelib/math/SwerveModuleState2.java b/swervelib/math/SwerveModuleState2.java
index 2f88e9d..19e1d6f 100644
--- a/swervelib/math/SwerveModuleState2.java
+++ b/swervelib/math/SwerveModuleState2.java
@@ -2,6 +2,7 @@ package swervelib.math;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
/**
* Second order kinematics swerve module state.
@@ -9,24 +10,17 @@ import edu.wpi.first.math.kinematics.SwerveModuleState;
public class SwerveModuleState2 extends SwerveModuleState
{
- /**
- * Swerve module speed in meters per second.
- */
- public double speedMetersPerSecond;
/**
* Rad per sec
*/
- public double omegaRadPerSecond = 0;
- /**
- * Swerve module angle as a {@link Rotation2d}.
- */
- public Rotation2d angle = Rotation2d.fromDegrees(0);
+ public double omegaRadPerSecond = 0;
/**
* Constructs a SwerveModuleState with zeros for speed and angle.
*/
public SwerveModuleState2()
{
+ super();
}
/**
@@ -39,8 +33,75 @@ public class SwerveModuleState2 extends SwerveModuleState
public SwerveModuleState2(
double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
{
- this.speedMetersPerSecond = speedMetersPerSecond;
- this.angle = angle;
+ super(speedMetersPerSecond, angle);
this.omegaRadPerSecond = omegaRadPerSecond;
}
+
+ /**
+ * Create a {@link SwerveModuleState2} based on the {@link SwerveModuleState} with the radians per second defined.
+ *
+ * @param state First order kinematic module state.
+ * @param omegaRadPerSecond Module wheel angular rotation in radians per second.
+ */
+ public SwerveModuleState2(SwerveModuleState state, double omegaRadPerSecond)
+ {
+ super(state.speedMetersPerSecond, state.angle);
+ this.omegaRadPerSecond = omegaRadPerSecond;
+ }
+
+ /**
+ * Minimize the change in heading the desired swerve module state would require by potentially reversing the direction
+ * the wheel spins. If this is used with the PIDController class's continuous input functionality, the furthest a
+ * wheel will ever rotate is 90 degrees.
+ *
+ * @param desiredState The desired state.
+ * @param currentAngle The current module angle.
+ * @param lastState The last state of the module.
+ * @param moduleSteerFeedForwardClosedLoop The module feed forward closed loop for the angle motor.
+ * @return Optimized swerve module state.
+ */
+ public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
+ SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop)
+ {
+ if (moduleSteerFeedForwardClosedLoop == 0)
+ {
+// desiredState.angle = desiredState.angle.plus(Rotation2d.fromRadians(
+// lastState.omegaRadPerSecond * moduleSteerFeedForwardClosedLoop * 0.065));
+// return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
+// desiredState.omegaRadPerSecond);
+ return new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle), 0);
+// return desiredState;
+ } else
+ {
+ double targetAngle = SwerveMath.placeInAppropriate0To360Scope(currentAngle.getDegrees(),
+ desiredState.angle.getDegrees() +
+ Units.radiansToDegrees(lastState.omegaRadPerSecond *
+ moduleSteerFeedForwardClosedLoop *
+ 0.065));
+ double targetSpeed = desiredState.speedMetersPerSecond;
+ double delta = targetAngle - currentAngle.getDegrees();
+ if (Math.abs(delta) > 90)
+ {
+ targetSpeed = -targetSpeed;
+ if (delta > 90)
+ {
+ targetAngle -= 180;
+ } else
+ {
+ targetAngle += 180;
+ }
+ }
+ return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle), desiredState.omegaRadPerSecond);
+ }
+ }
+
+ /**
+ * Convert to a {@link SwerveModuleState}.
+ *
+ * @return {@link SwerveModuleState} with the same angle and speed.
+ */
+ public SwerveModuleState toSwerveModuleState()
+ {
+ return new SwerveModuleState(this.speedMetersPerSecond, this.angle);
+ }
}
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index 3f3f96e..b4f9db4 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -311,7 +311,8 @@ public class SparkMaxSwerve extends SwerveMotor
pid.setReference(
setpoint,
ControlType.kPosition,
- pidSlot);
+ pidSlot,
+ feedforward);
}
}
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index 1ea15a7..53da69b 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -1,9 +1,10 @@
package swervelib.motors;
-import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.Timer;
@@ -101,6 +102,8 @@ public class TalonFXSwerve extends SwerveMotor
{
motor.configFactoryDefault();
motor.setSensorPhase(true);
+ motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
+ motor.configNeutralDeadband(0.001);
}
}
@@ -314,11 +317,21 @@ public class TalonFXSwerve extends SwerveMotor
burnFlash();
- motor.set(
- isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
- convertToNativeSensorUnits(setpoint, position),
- DemandType.ArbitraryFeedForward,
- feedforward / nominalVoltage);
+ if (isDriveMotor)
+ {
+ motor.set(
+ TalonFXControlMode.Velocity,
+ convertToNativeSensorUnits(setpoint, position),
+ DemandType.ArbitraryFeedForward,
+ feedforward / nominalVoltage);
+ } else
+ {
+ motor.set(
+ TalonFXControlMode.Position,
+ convertToNativeSensorUnits(setpoint, position),
+ DemandType.ArbitraryFeedForward,
+ feedforward);
+ }
}
/**
diff --git a/swervelib/parser/SwerveControllerConfiguration.java b/swervelib/parser/SwerveControllerConfiguration.java
index 20ae8e7..e1533ef 100644
--- a/swervelib/parser/SwerveControllerConfiguration.java
+++ b/swervelib/parser/SwerveControllerConfiguration.java
@@ -8,14 +8,6 @@ import static swervelib.math.SwerveMath.calculateMaxAngularVelocity;
public class SwerveControllerConfiguration
{
- /**
- * Maximum robot speed in meters per second.
- */
- public double maxSpeed;
- /**
- * Maximum angular velocity in rad/s
- */
- public double maxAngularVelocity;
/**
* PIDF for the heading of the robot.
*/
@@ -25,6 +17,14 @@ public class SwerveControllerConfiguration
*/
public final double
angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
+ /**
+ * Maximum robot speed in meters per second.
+ */
+ public double maxSpeed;
+ /**
+ * Maximum angular velocity in rad/s
+ */
+ public double maxAngularVelocity;
/**
* Construct the swerve controller configuration.
diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java
index d1673ce..27c568e 100644
--- a/swervelib/parser/SwerveDriveConfiguration.java
+++ b/swervelib/parser/SwerveDriveConfiguration.java
@@ -7,70 +7,76 @@ 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 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;
+ /**
+ * 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.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 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]);
- }
- return modArr;
+ /**
+ * 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
index b64d8a3..0f2440a 100644
--- a/swervelib/parser/SwerveModuleConfiguration.java
+++ b/swervelib/parser/SwerveModuleConfiguration.java
@@ -1,9 +1,9 @@
package swervelib.parser;
-import static swervelib.math.SwerveMath.calculateAngleKV;
import static swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation;
import static swervelib.math.SwerveMath.calculateMaxAcceleration;
import static swervelib.math.SwerveMath.calculateMetersPerRotation;
+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -46,7 +46,7 @@ public class SwerveModuleConfiguration
/**
* Angle volt-meter-per-second.
*/
- public double angleKV;
+ public double moduleSteerFFCL;
/**
* The integrated encoder pulse per revolution.
*/
@@ -67,6 +67,10 @@ public class SwerveModuleConfiguration
* The Absolute Encoder for the swerve module.
*/
public SwerveAbsoluteEncoder absoluteEncoder;
+ /**
+ * Name for the swerve module for telemetry.
+ */
+ public String name;
/**
* Construct a configuration object for swerve modules.
@@ -85,7 +89,6 @@ public class SwerveModuleConfiguration
* @param maxSpeed Maximum speed in meters per second.
* @param physicalCharacteristics Physical characteristics of the swerve module.
* @param angleMotorEncoderPulsePerRevolution The encoder pulse per revolution for the angle motor encoder.
- * @param angleMotorFreeSpeedRPM The free speed RPM of the angle motor.
*/
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
@@ -102,7 +105,7 @@ public class SwerveModuleConfiguration
boolean driveMotorInverted,
boolean angleMotorInverted,
double angleMotorEncoderPulsePerRevolution,
- double angleMotorFreeSpeedRPM)
+ String name)
{
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
@@ -115,13 +118,10 @@ public class SwerveModuleConfiguration
this.anglePIDF = anglePIDF;
this.velocityPIDF = velocityPIDF;
this.maxSpeed = maxSpeed;
- this.angleKV = physicalCharacteristics.angleMotorKV == 0 ?
- calculateAngleKV(
- physicalCharacteristics.optimalVoltage,
- angleMotorFreeSpeedRPM,
- physicalCharacteristics.angleGearRatio) : physicalCharacteristics.angleMotorKV;
+ this.moduleSteerFFCL = physicalCharacteristics.moduleSteerFFCL;
this.physicalCharacteristics = physicalCharacteristics;
this.angleMotorEncoderPulsePerRevolution = angleMotorEncoderPulsePerRevolution;
+ this.name = name;
}
/**
@@ -138,6 +138,7 @@ public class SwerveModuleConfiguration
* @param velocityPIDF Velocity PIDF configuration.
* @param maxSpeed Maximum robot speed in meters per second.
* @param physicalCharacteristics Physical characteristics of the swerve module.
+ * @param name Name for the module.
*/
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
@@ -149,7 +150,8 @@ public class SwerveModuleConfiguration
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
- SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ String name)
{
this(
driveMotor,
@@ -166,7 +168,7 @@ public class SwerveModuleConfiguration
false,
false,
physicalCharacteristics.angleEncoderPulsePerRotation,
- physicalCharacteristics.angleMotorFreeSpeedRPM);
+ name);
}
/**
diff --git a/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/swervelib/parser/SwerveModulePhysicalCharacteristics.java
index dba6ebe..67eace3 100644
--- a/swervelib/parser/SwerveModulePhysicalCharacteristics.java
+++ b/swervelib/parser/SwerveModulePhysicalCharacteristics.java
@@ -42,16 +42,12 @@ public class SwerveModulePhysicalCharacteristics
* 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;
/**
* Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
* your drive train does not drift towards the direction you are rotating while you translate. When set to 0 the
* calculated kV will be used.
*/
- public final double angleMotorKV;
+ public final double moduleSteerFFCL;
/**
* Construct the swerve module physical characteristics.
@@ -59,7 +55,6 @@ public class SwerveModulePhysicalCharacteristics
* @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.
@@ -71,13 +66,12 @@ public class SwerveModulePhysicalCharacteristics
* 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.
- * @param angleMotorKV The kV applied to the steering motor to ensure your drivetrain does not drift
+ * @param moduleSteerFFCL The kV applied to the steering motor to ensure your drivetrain does not drift
* towards a direction when rotating while translating.
*/
public SwerveModulePhysicalCharacteristics(
double driveGearRatio,
double angleGearRatio,
- double angleMotorFreeSpeedRPM,
double wheelDiameter,
double wheelGripCoefficientOfFriction,
double optimalVoltage,
@@ -87,12 +81,11 @@ public class SwerveModulePhysicalCharacteristics
double angleMotorRampRate,
int driveEncoderPulsePerRotation,
int angleEncoderPulsePerRotation,
- double angleMotorKV)
+ double moduleSteerFFCL)
{
this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
this.optimalVoltage = optimalVoltage;
- this.angleMotorFreeSpeedRPM = angleMotorFreeSpeedRPM;
this.angleGearRatio = angleGearRatio;
this.driveGearRatio = driveGearRatio;
this.angleEncoderPulsePerRotation = angleEncoderPulsePerRotation;
@@ -103,7 +96,7 @@ public class SwerveModulePhysicalCharacteristics
this.angleMotorCurrentLimit = angleMotorCurrentLimit;
this.driveMotorRampRate = driveMotorRampRate;
this.angleMotorRampRate = angleMotorRampRate;
- this.angleMotorKV = angleMotorKV;
+ this.moduleSteerFFCL = moduleSteerFFCL;
}
/**
@@ -113,7 +106,6 @@ public class SwerveModulePhysicalCharacteristics
*
* @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)
@@ -125,7 +117,6 @@ public class SwerveModulePhysicalCharacteristics
public SwerveModulePhysicalCharacteristics(
double driveGearRatio,
double angleGearRatio,
- double angleMotorFreeSpeedRPM,
double wheelDiameter,
double driveMotorRampRate,
double angleMotorRampRate,
@@ -135,7 +126,6 @@ public class SwerveModulePhysicalCharacteristics
this(
driveGearRatio,
angleGearRatio,
- angleMotorFreeSpeedRPM,
wheelDiameter,
1.19,
12,
@@ -144,6 +134,6 @@ public class SwerveModulePhysicalCharacteristics
driveMotorRampRate,
angleMotorRampRate,
driveEncoderPulsePerRotation,
- angleEncoderPulsePerRotation, 0);
+ angleEncoderPulsePerRotation, -0.30);
}
}
diff --git a/swervelib/parser/SwerveParser.java b/swervelib/parser/SwerveParser.java
index 1e29b78..0ec95c3 100644
--- a/swervelib/parser/SwerveParser.java
+++ b/swervelib/parser/SwerveParser.java
@@ -142,7 +142,8 @@ public class SwerveParser
pidfPropertiesJson.angle,
pidfPropertiesJson.drive,
maxSpeedMPS,
- physicalPropertiesJson.createPhysicalProperties(swerveDriveJson.optimalVoltage));
+ physicalPropertiesJson.createPhysicalProperties(swerveDriveJson.optimalVoltage),
+ swerveDriveJson.modules[i]);
}
SwerveDriveConfiguration swerveDriveConfiguration =
new SwerveDriveConfiguration(
diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java
index 588429c..e3f31c4 100644
--- a/swervelib/parser/json/ModuleJson.java
+++ b/swervelib/parser/json/ModuleJson.java
@@ -43,10 +43,6 @@ public class ModuleJson
* The angle encoder pulse per revolution override. 1 for Neo encoder. 2048 for Falcons.
*/
public double angleEncoderPulsePerRevolution = 0;
- /**
- * Angle motor free speed RPM.
- */
- public double angleMotorFreeSpeedRPM = 0;
/**
* The location of the swerve module from the center of the robot in inches.
*/
@@ -59,13 +55,15 @@ public class ModuleJson
* @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.
+ * @param name Module json filename.
* @return {@link SwerveModuleConfiguration} based on the provided data and parsed data.
*/
public SwerveModuleConfiguration createModuleConfiguration(
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
- SwerveModulePhysicalCharacteristics physicalCharacteristics)
+ SwerveModulePhysicalCharacteristics physicalCharacteristics,
+ String name)
{
SwerveMotor angleMotor = angle.createMotor(false);
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder();
@@ -93,6 +91,6 @@ public class ModuleJson
inverted.angle,
angleEncoderPulsePerRevolution == 0 ? physicalCharacteristics.angleEncoderPulsePerRotation
: angleEncoderPulsePerRevolution,
- angleMotorFreeSpeedRPM == 0 ? physicalCharacteristics.angleMotorFreeSpeedRPM : angleMotorFreeSpeedRPM);
+ name.replaceAll("\\.json", ""));
}
}
diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java
index 3667c15..1a398bd 100644
--- a/swervelib/parser/json/PhysicalPropertiesJson.java
+++ b/swervelib/parser/json/PhysicalPropertiesJson.java
@@ -2,6 +2,7 @@ package swervelib.parser.json;
import edu.wpi.first.math.util.Units;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
+import swervelib.telemetry.SwerveDriveTelemetry;
/**
* {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule.
@@ -33,16 +34,16 @@ public class PhysicalPropertiesJson
* 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;
/**
* Angle motor kV used for second order kinematics to tune the feedforward, this variable should be adjusted so that
- * your drive train does not drift towards the direction you are rotating while you translate. When set to 0 the
- * calculated kV will be used.
+ * your drive train does not drift towards the direction you are rotating while you translate. Default value is 0. If
+ * robot arcs while translating and rotating negate this.
*/
- public double angleMotorsKV = 0;
+ public double moduleFeedForwardClosedLoop = SwerveDriveTelemetry.isSimulation ? -0.33 : 0;
+ /**
+ * DEPRECATED: No longer needed, tune {@link PhysicalPropertiesJson#moduleFeedForwardClosedLoop} instead.
+ */
+ public double angleMotorFreeSpeedRPM = 0;
/**
* Create the physical characteristics based off the parsed data.
@@ -55,7 +56,6 @@ public class PhysicalPropertiesJson
return new SwerveModulePhysicalCharacteristics(
gearRatio.drive,
gearRatio.angle,
- angleMotorFreeSpeedRPM,
Units.inchesToMeters(wheelDiameter),
wheelGripCoefficientOfFriction,
optimalVoltage,
@@ -65,7 +65,7 @@ public class PhysicalPropertiesJson
rampRate.angle,
encoderPulsePerRotation.drive,
encoderPulsePerRotation.angle,
- angleMotorsKV);
+ moduleFeedForwardClosedLoop);
}
}