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.
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.
maximumSpeed - Maximum speed for the drive motors in meters / second.
+
updateModuleFeedforward - Update the swerve module feedforward to account for the new maximum speed. This
+ should be true unless you have replaced the drive motor feedforward with
+ replaceSwerveModuleFeedforward(SimpleMotorFeedforward)
public staticdoubleplaceInAppropriate0To360Scope(double scopeReference,
+ double newAngle)
+
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.
Put an angle within the 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.
Put an angle within the 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.
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.
Free speed rotations per minute of the motor, as described by the vendor.
+
+
+
angleMotorKV
+
public finaldoubleangleMotorKV
+
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.
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.
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.
+
+
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index 55bed02..aa8b1cf 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 7911dba..3054cde 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 4a09e98..d75e8fd 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 5be968f..0732505 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 abfc4f4..beccda0 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 7b807ea..6487229 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 26ea24a..d612df8 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 52d472b..c692144 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 0b3af95..b96c179 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 f182d11..7d32744 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 5ac36d6..f5e4af2 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 ca80af6..41f7dc8 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 8718ac5..2b9e102 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 65f37fa..82c31ee 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 bfef207..25b25ba 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 af36aef..6c93ad8 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 309618f..398a0bd 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 492d357..92c831f 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 dd6700a..6066e3d 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 ca4c9b5..273a06c 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 594fd84..c9a35cf 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 9a0be13..e4bae25 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 f51a122..ab5e80e 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 96cf3ae..442fa8a 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/SwerveController.java b/swervelib/SwerveController.java
index 9d38874..ad23fad 100644
--- a/swervelib/SwerveController.java
+++ b/swervelib/SwerveController.java
@@ -16,27 +16,27 @@ public class SwerveController
* {@link SwerveControllerConfiguration} object storing data to generate the {@link PIDController} for controlling the
* robot heading, and deadband for heading joystick.
*/
- public final SwerveControllerConfiguration config;
+ public final SwerveControllerConfiguration config;
/**
* PID Controller for the robot heading.
*/
- public final PIDController thetaController; // TODO: Switch to ProfilePIDController
+ public final PIDController thetaController; // TODO: Switch to ProfilePIDController
/**
* Last angle as a scalar [-1,1] the robot was set to.
*/
- public double lastAngleScalar;
+ public double lastAngleScalar;
/**
* {@link SlewRateLimiter} for movement in the X direction in meters/second.
*/
- public SlewRateLimiter xLimiter = null;
+ public SlewRateLimiter xLimiter = null;
/**
* {@link SlewRateLimiter} for movement in the Y direction in meters/second.
*/
- public SlewRateLimiter yLimiter = null;
+ public SlewRateLimiter yLimiter = null;
/**
* {@link SlewRateLimiter} for angular movement in radians/second.
*/
- public SlewRateLimiter angleLimiter = null;
+ public SlewRateLimiter angleLimiter = null;
/**
* Construct the SwerveController object which is used for determining the speeds of the robot based on controller
@@ -200,4 +200,16 @@ public class SwerveController
{
return thetaController.calculate(currentHeadingAngleRadians, targetHeadingAngleRadians) * config.maxAngularVelocity;
}
+
+ /**
+ * Set a new maximum angular velocity that is different from the auto-generated one. Modified the
+ * {@link SwerveControllerConfiguration#maxAngularVelocity} field which is used in the {@link SwerveController} class
+ * for {@link ChassisSpeeds} generation.
+ *
+ * @param angularVelocity Angular velocity in radians per second.
+ */
+ public void setMaximumAngularVelocity(double angularVelocity)
+ {
+ config.maxAngularVelocity = angularVelocity;
+ }
}
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index e2e6f21..eb1ecac 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -272,18 +272,18 @@ public class SwerveDrive
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.desiredStates[module.moduleNumber *
- 2] = desiredStates[module.moduleNumber].angle.getDegrees();
+ 2] = module.lastState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) +
- 1] = desiredStates[module.moduleNumber].speedMetersPerSecond;
+ 1] = module.lastState.speedMetersPerSecond;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
- "Module " + module.moduleNumber + " Speed Setpoint: ",
- desiredStates[module.moduleNumber].speedMetersPerSecond);
+ "Module[" + module.moduleNumber + "] Speed Setpoint: ",
+ module.lastState.speedMetersPerSecond);
SmartDashboard.putNumber(
- "Module " + module.moduleNumber + " Angle Setpoint: ",
- desiredStates[module.moduleNumber].angle.getDegrees());
+ "Module[" + module.moduleNumber + "] Angle Setpoint: ",
+ module.lastState.angle.getDegrees());
}
}
}
@@ -533,6 +533,47 @@ public class SwerveDrive
}
}
+ /**
+ * Set the maximum speed of the drive motors, modified {@link SwerveControllerConfiguration#maxSpeed} and
+ * {@link SwerveDriveConfiguration#maxSpeed} which is used for the
+ * {@link SwerveDrive#setRawModuleStates(SwerveModuleState2[], boolean)} function and
+ * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
+ * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates.
+ *
+ * @param maximumSpeed Maximum speed for the drive motors in meters / second.
+ * @param updateModuleFeedforward Update the swerve module feedforward to account for the new maximum speed. This
+ * should be true unless you have replaced the drive motor feedforward with
+ * {@link SwerveDrive#replaceSwerveModuleFeedforward(SimpleMotorFeedforward)}
+ */
+ public void setMaximumSpeed(double maximumSpeed, boolean updateModuleFeedforward)
+ {
+ swerveDriveConfiguration.maxSpeed = maximumSpeed;
+ swerveController.config.maxSpeed = maximumSpeed;
+ for (SwerveModule module : swerveModules)
+ {
+ module.configuration.maxSpeed = maximumSpeed;
+ if (updateModuleFeedforward)
+ {
+ module.feedforward = module.configuration.createDriveFeedforward();
+ }
+ }
+ }
+
+ /**
+ * Set the maximum speed of the drive motors, modified {@link SwerveControllerConfiguration#maxSpeed} and
+ * {@link SwerveDriveConfiguration#maxSpeed} which is used for the
+ * {@link SwerveDrive#setRawModuleStates(SwerveModuleState2[], boolean)} function and
+ * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
+ * what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the
+ * {@link SwerveModule#feedforward}.
+ *
+ * @param maximumSpeed Maximum speed for the drive motors in meters / second.
+ */
+ public void setMaximumSpeed(double maximumSpeed)
+ {
+ setMaximumSpeed(maximumSpeed, true);
+ }
+
/**
* Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep
* the current pose.
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 31ea182..5883768 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -3,9 +3,10 @@ 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.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.math.SwerveMath;
import swervelib.math.SwerveModuleState2;
import swervelib.motors.SwerveMotor;
import swervelib.parser.SwerveModuleConfiguration;
@@ -44,13 +45,9 @@ public class SwerveModule
*/
public SimpleMotorFeedforward feedforward;
/**
- * Last angle set for the swerve module.
+ * Last swerve module state applied.
*/
- public double lastAngle;
- /**
- * Last velocity set for the swerve module.
- */
- public double lastVelocity;
+ public SwerveModuleState2 lastState;
/**
* Simulated swerve module.
*/
@@ -124,8 +121,7 @@ public class SwerveModule
simModule = new SwerveModuleSimulation();
}
- lastAngle = getState().angle.getDegrees();
- lastVelocity = getState().speedMetersPerSecond;
+ lastState = getState();
}
/**
@@ -150,40 +146,34 @@ 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);
-
+// 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
if (isOpenLoop)
{
double percentOutput = desiredState.speedMetersPerSecond / configuration.maxSpeed;
driveMotor.set(percentOutput);
} else
{
- double velocity = desiredState.speedMetersPerSecond;
- if (velocity != lastVelocity)
+ if (desiredState.speedMetersPerSecond != lastState.speedMetersPerSecond)
{
+ double velocity = desiredState.speedMetersPerSecond;
driveMotor.setReference(velocity, feedforward.calculate(velocity));
}
- lastVelocity = velocity;
}
- double angle = desiredState.angle.getDegrees();
-
// If we are forcing the angle
if (!force)
{
// Prevents module rotation if speed is less than 1%
- angle = Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ? lastAngle : angle;
- }
-
- // Ensure the angle is above 0
- while (angle < 0)
- {
- angle += 360;
+ SwerveMath.antiJitter(desiredState, lastState, configuration.maxSpeed);
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
@@ -191,29 +181,30 @@ public class SwerveModule
SmartDashboard.putNumber(
"Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
SmartDashboard.putNumber(
- "Optimized " + moduleNumber + " Angle Setpoint: ", angle);
+ "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
SmartDashboard.putNumber(
"Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
}
// Prevent module rotation if angle is the same as the previous angle.
- if (angle != lastAngle || synchronizeEncoderQueued)
+ if (desiredState.angle != lastState.angle || synchronizeEncoderQueued)
{
// 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,
+ angleMotor.setReference(desiredState.angle.getDegrees(),
Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV,
absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else
{
- angleMotor.setReference(angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
+ angleMotor.setReference(desiredState.angle.getDegrees(),
+ Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
}
}
- lastAngle = angle;
+ lastState = desiredState;
if (SwerveDriveTelemetry.isSimulation)
{
@@ -229,7 +220,7 @@ public class SwerveModule
public void setAngle(double angle)
{
angleMotor.setReference(angle, configuration.angleKV);
- lastAngle = angle;
+ lastState.angle = Rotation2d.fromDegrees(angle);
}
/**
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index 4745579..9d7613d 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -309,4 +309,98 @@ 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.
+ *
+ * @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.
+ *
+ * @param scopeReference Current Angle (deg)
+ * @param newAngle Target Angle (deg)
+ * @return Closest angle within scope (deg)
+ */
+ public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
+ {
+ double lowerBound;
+ double upperBound;
+ double lowerOffset = scopeReference % 360;
+ if (lowerOffset >= 0)
+ {
+ lowerBound = scopeReference - lowerOffset;
+ upperBound = scopeReference + (360 - lowerOffset);
+ } else
+ {
+ upperBound = scopeReference - lowerOffset;
+ lowerBound = scopeReference - (360 + lowerOffset);
+ }
+ while (newAngle < lowerBound)
+ {
+ newAngle += 360;
+ }
+ while (newAngle > upperBound)
+ {
+ newAngle -= 360;
+ }
+ if (newAngle - scopeReference > 180)
+ {
+ newAngle -= 360;
+ } else if (newAngle - scopeReference < -180)
+ {
+ newAngle += 360;
+ }
+ return newAngle;
+ }
+
+ /**
+ * Perform anti-jitter within modules if the speed requested is too low.
+ *
+ * @param moduleState Current {@link SwerveModuleState2} requested.
+ * @param lastModuleState Previous {@link SwerveModuleState2} used.
+ * @param maxSpeed Maximum speed of the modules, should be in {@link SwerveDriveConfiguration#maxSpeed}.
+ */
+ public static void antiJitter(SwerveModuleState2 moduleState, SwerveModuleState2 lastModuleState, double maxSpeed)
+ {
+ if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01))
+ {
+ moduleState.angle = lastModuleState.angle;
+// moduleState.omegaRadPerSecond = lastModuleState.omegaRadPerSecond;
+ moduleState.omegaRadPerSecond = 0;
+ }
+ }
}
diff --git a/swervelib/math/package-info.java b/swervelib/math/package-info.java
index d95b8e0..4604fe6 100644
--- a/swervelib/math/package-info.java
+++ b/swervelib/math/package-info.java
@@ -1,4 +1,5 @@
/**
- * Mathematics for swerve drives.
+ * Mathematics for swerve drives. Original second order kinematics was developed by Team 3181 her.e
*/
package swervelib.math;
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index e71ffb9..3f3f96e 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -199,6 +199,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
int pidSlot =
isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ pidSlot = 0;
pid.setP(config.p, pidSlot);
pid.setI(config.i, pidSlot);
pid.setD(config.d, pidSlot);
@@ -293,8 +294,11 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
+ boolean possibleBurnOutIssue = true;
int pidSlot =
isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ pidSlot = 0;
+
if (isDriveMotor)
{
pid.setReference(
@@ -306,7 +310,8 @@ public class SparkMaxSwerve extends SwerveMotor
{
pid.setReference(
setpoint,
- ControlType.kPosition);
+ ControlType.kPosition,
+ pidSlot);
}
}
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index 3e5187d..1ea15a7 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.Timer;
import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
import swervelib.simulation.ctre.PhysicsSim;
import swervelib.telemetry.SwerveDriveTelemetry;
@@ -270,50 +271,6 @@ public class TalonFXSwerve extends SwerveMotor
motor.set(percentOutput);
}
- /**
- * Put an angle within the 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.
- *
- * @param scopeReference Current Angle (deg)
- * @param newAngle Target Angle (deg)
- * @return Closest angle within scope (deg)
- */
- private double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
- {
- double lowerBound;
- double upperBound;
- double lowerOffset = (scopeReference % 360);
-
- // Create the interval from the reference angle.
- if (lowerOffset >= 0)
- {
- lowerBound = scopeReference - lowerOffset;
- upperBound = scopeReference + (360 - lowerOffset);
- } else
- {
- upperBound = scopeReference - lowerOffset;
- lowerBound = scopeReference - (360 + lowerOffset);
- }
- // Put the angle in the interval.
- while (newAngle < lowerBound)
- {
- newAngle += 360;
- }
- while (newAngle > upperBound)
- {
- newAngle -= 360;
- }
- // Smooth the transition between interval boundaries.
- if (newAngle - scopeReference > 180)
- {
- newAngle -= 360;
- } else if (newAngle - scopeReference < -180)
- {
- newAngle += 360;
- }
- return newAngle;
- }
-
/**
* Convert the setpoint into native sensor units.
*
@@ -324,7 +281,7 @@ public class TalonFXSwerve extends SwerveMotor
public double convertToNativeSensorUnits(double setpoint, double position)
{
setpoint =
- isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(position, setpoint);
+ isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint);
return setpoint / positionConversionFactor;
}
diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java
index d95b983..60a5899 100644
--- a/swervelib/motors/TalonSRXSwerve.java
+++ b/swervelib/motors/TalonSRXSwerve.java
@@ -9,6 +9,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.Timer;
import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
import swervelib.simulation.ctre.PhysicsSim;
import swervelib.telemetry.SwerveDriveTelemetry;
@@ -260,49 +261,6 @@ public class TalonSRXSwerve extends SwerveMotor
motor.set(percentOutput);
}
- /**
- * Put an angle within the 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.
- *
- * @param scopeReference Current Angle (deg)
- * @param newAngle Target Angle (deg)
- * @return Closest angle within scope (deg)
- */
- private double placeInAppropriate0To360Scope(double scopeReference, double newAngle)
- {
- double lowerBound;
- double upperBound;
- double lowerOffset = (scopeReference % 360);
-
- // Create the interval from the reference angle.
- if (lowerOffset >= 0)
- {
- lowerBound = scopeReference - lowerOffset;
- upperBound = scopeReference + (360 - lowerOffset);
- } else
- {
- upperBound = scopeReference - lowerOffset;
- lowerBound = scopeReference - (360 + lowerOffset);
- }
- // Put the angle in the interval.
- while (newAngle < lowerBound)
- {
- newAngle += 360;
- }
- while (newAngle > upperBound)
- {
- newAngle -= 360;
- }
- // Smooth the transition between interval boundaries.
- if (newAngle - scopeReference > 180)
- {
- newAngle -= 360;
- } else if (newAngle - scopeReference < -180)
- {
- newAngle += 360;
- }
- return newAngle;
- }
/**
* Convert the setpoint into native sensor units.
@@ -314,7 +272,7 @@ public class TalonSRXSwerve extends SwerveMotor
public double convertToNativeSensorUnits(double setpoint, double position)
{
setpoint =
- isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(position, setpoint);
+ isDriveMotor ? setpoint * .1 : SwerveMath.placeInAppropriate0To360Scope(position, setpoint);
return setpoint / positionConversionFactor;
}
diff --git a/swervelib/parser/SwerveControllerConfiguration.java b/swervelib/parser/SwerveControllerConfiguration.java
index 2649544..20ae8e7 100644
--- a/swervelib/parser/SwerveControllerConfiguration.java
+++ b/swervelib/parser/SwerveControllerConfiguration.java
@@ -11,11 +11,11 @@ public class SwerveControllerConfiguration
/**
* Maximum robot speed in meters per second.
*/
- public final double maxSpeed;
+ public double maxSpeed;
/**
* Maximum angular velocity in rad/s
*/
- public final double maxAngularVelocity;
+ public double maxAngularVelocity;
/**
* PIDF for the heading of the robot.
*/
diff --git a/swervelib/parser/SwerveModuleConfiguration.java b/swervelib/parser/SwerveModuleConfiguration.java
index 39e6f8c..b64d8a3 100644
--- a/swervelib/parser/SwerveModuleConfiguration.java
+++ b/swervelib/parser/SwerveModuleConfiguration.java
@@ -4,7 +4,6 @@ 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;
@@ -35,7 +34,7 @@ public class SwerveModuleConfiguration
/**
* Maximum robot speed in meters per second.
*/
- public final double maxSpeed;
+ public double maxSpeed;
/**
* PIDF configuration options for the angle motor closed-loop PID controller.
*/
@@ -116,11 +115,11 @@ public class SwerveModuleConfiguration
this.anglePIDF = anglePIDF;
this.velocityPIDF = velocityPIDF;
this.maxSpeed = maxSpeed;
- this.angleKV =
- calculateAngleKV(
- physicalCharacteristics.optimalVoltage,
- angleMotorFreeSpeedRPM,
- physicalCharacteristics.angleGearRatio);
+ this.angleKV = physicalCharacteristics.angleMotorKV == 0 ?
+ calculateAngleKV(
+ physicalCharacteristics.optimalVoltage,
+ angleMotorFreeSpeedRPM,
+ physicalCharacteristics.angleGearRatio) : physicalCharacteristics.angleMotorKV;
this.physicalCharacteristics = physicalCharacteristics;
this.angleMotorEncoderPulsePerRevolution = angleMotorEncoderPulsePerRevolution;
}
diff --git a/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/swervelib/parser/SwerveModulePhysicalCharacteristics.java
index 8d07dc9..4a62b14 100644
--- a/swervelib/parser/SwerveModulePhysicalCharacteristics.java
+++ b/swervelib/parser/SwerveModulePhysicalCharacteristics.java
@@ -46,6 +46,12 @@ public class SwerveModulePhysicalCharacteristics
* 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;
/**
* Construct the swerve module physical characteristics.
@@ -78,7 +84,8 @@ public class SwerveModulePhysicalCharacteristics
double driveMotorRampRate,
double angleMotorRampRate,
int driveEncoderPulsePerRotation,
- int angleEncoderPulsePerRotation)
+ int angleEncoderPulsePerRotation,
+ double angleMotorKV)
{
this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
this.optimalVoltage = optimalVoltage;
@@ -94,6 +101,7 @@ public class SwerveModulePhysicalCharacteristics
this.angleMotorCurrentLimit = angleMotorCurrentLimit;
this.driveMotorRampRate = driveMotorRampRate;
this.angleMotorRampRate = angleMotorRampRate;
+ this.angleMotorKV = angleMotorKV;
}
/**
@@ -134,6 +142,6 @@ public class SwerveModulePhysicalCharacteristics
driveMotorRampRate,
angleMotorRampRate,
driveEncoderPulsePerRotation,
- angleEncoderPulsePerRotation);
+ angleEncoderPulsePerRotation, 0);
}
}
diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java
index b8a5929..3667c15 100644
--- a/swervelib/parser/json/PhysicalPropertiesJson.java
+++ b/swervelib/parser/json/PhysicalPropertiesJson.java
@@ -37,6 +37,12 @@ public class PhysicalPropertiesJson
* 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.
+ */
+ public double angleMotorsKV = 0;
/**
* Create the physical characteristics based off the parsed data.
@@ -58,7 +64,8 @@ public class PhysicalPropertiesJson
rampRate.drive,
rampRate.angle,
encoderPulsePerRotation.drive,
- encoderPulsePerRotation.angle);
+ encoderPulsePerRotation.angle,
+ angleMotorsKV);
}
}