diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index f858eea..637dcb9 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -18,6 +18,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -78,28 +79,28 @@ public class SwerveDrive
/**
* Invert odometry readings of drive motor positions, used as a patch for debugging currently.
*/
- public boolean invertOdometry = false;
+ public boolean invertOdometry = false;
/**
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
*/
- public boolean chassisVelocityCorrection = true;
+ public boolean chassisVelocityCorrection = true;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
- private SwerveIMU imu;
+ private SwerveIMU imu;
/**
* Simulation of the swerve drive.
*/
- private SwerveIMUSimulation simIMU;
+ private SwerveIMUSimulation simIMU;
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
- private int moduleSynchronizationCounter = 0;
+ private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
- private double lastHeadingRadians = 0;
+ private double lastHeadingRadians = 0;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -312,9 +313,6 @@ public class SwerveDrive
swerveDriveConfiguration.maxSpeed,
swerveDriveConfiguration.attainableMaxTranslationalSpeedMetersPerSecond,
swerveDriveConfiguration.attainableMaxRotationalVelocityRadiansPerSecond);
- } else
- {
- SwerveKinematics2.desaturateWheelSpeeds(desiredStates, swerveDriveConfiguration.maxSpeed);
}
// Sets states
@@ -411,7 +409,8 @@ public class SwerveDrive
*/
public void resetOdometry(Pose2d pose)
{
- swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
+ swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose);
+ kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
}
/**
@@ -766,9 +765,28 @@ public class SwerveDrive
}
}
+ /**
+ * Set the gyro scope offset to a desired known rotation. Unlike {@link SwerveDrive#setGyro(Rotation3d)} it DOES NOT
+ * take the current rotation into account.
+ *
+ * @param offset {@link Rotation3d} known offset of the robot for gyroscope to use.
+ */
+ public void setGyroOffset(Rotation3d offset)
+ {
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ simIMU.setAngle(offset.getZ());
+ } else
+ {
+ imu.setOffset(offset);
+ }
+ }
+
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
- * the given timestamp of the vision measurement.
+ * the given timestamp of the vision measurement.
IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
+ * AFTER USING THIS FUNCTION!
To update your gyroscope readings directly use
+ * {@link SwerveDrive#setGyroOffset(Rotation3d)}.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
@@ -779,7 +797,7 @@ public class SwerveDrive
* {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry#resetPosition(Rotation2d,
* SwerveModulePosition[], Pose2d)}.
* @param trustWorthiness Trust level of vision reading when using a soft measurement, used to multiply the standard
- * deviation. Set to 1 for full trust.
+ * deviation. Set to 1 for full trust. Higher = Less Weight.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp, boolean soft, double trustWorthiness)
{
@@ -792,6 +810,10 @@ public class SwerveDrive
swerveDrivePoseEstimator.resetPosition(
robotPose.getRotation(), getModulePositions(), robotPose);
}
+
+ setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
+ resetOdometry(
+ new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), robotPose.getRotation()));
}
/**
@@ -818,14 +840,20 @@ public class SwerveDrive
/**
- * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new
- * {@link Rotation3d}.
+ * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
+ * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
*
- * @param gyro expected gyroscope angle.
+ * @param gyro expected gyroscope angle as {@link Rotation3d}.
*/
public void setGyro(Rotation3d gyro)
{
- imu.setOffset(imu.getRawRotation3d().minus(gyro));
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
+ } else
+ {
+ setGyroOffset(imu.getRawRotation3d().minus(gyro));
+ }
}
/**
@@ -865,6 +893,19 @@ public class SwerveDrive
}
}
+ /**
+ * Configure whether the {@link SwerveModuleState2} will be optimized to prevent spinning more than 90deg.
+ *
+ * @param optimizationEnabled Whether the module optimization is enabled.
+ */
+ public void setModuleStateOptimization(boolean optimizationEnabled)
+ {
+ for (SwerveModule module : swerveModules)
+ {
+ module.moduleStateOptimization = optimizationEnabled;
+ }
+ }
+
/**
* Enable second order kinematics for simulation and modifying the feedforward. Second order kinematics could increase
* accuracy in odometry.
@@ -879,6 +920,25 @@ public class SwerveDrive
}
}
+ /**
+ * Enable second order kinematics with calculated values for the feedforward and return the value used.
+ *
+ * @param motorFreeSpeedRPM Steering motor free speed RPM.
+ * @return The feedforward value used for the last swerve module.
+ */
+ public double enableSecondOrderKinematics(int motorFreeSpeedRPM)
+ {
+ double feedforwardValue = 0;
+ for (SwerveModule module : swerveModules)
+ {
+ feedforwardValue = module.configuration.moduleSteerFFCL = RobotController.getBatteryVoltage() /
+ (2 * Math.PI * (((double) motorFreeSpeedRPM) /
+ module.configuration.physicalCharacteristics.angleGearRatio) /
+ 60);
+ }
+ return feedforwardValue;
+ }
+
/**
* Enable second order kinematics for tracking purposes but completely untuned.
*/
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 4013b94..942ea4b 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -38,23 +38,28 @@ public class SwerveModule
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
- public int moduleNumber;
+ public int moduleNumber;
/**
* Feedforward for drive motor during closed loop control.
*/
- public SimpleMotorFeedforward feedforward;
+ public SimpleMotorFeedforward feedforward;
/**
* Last swerve module state applied.
*/
- public SwerveModuleState2 lastState;
+ public SwerveModuleState2 lastState;
+ /**
+ * Enable {@link SwerveModuleState2} optimizations so the angle is reversed and speed is reversed to ensure the module
+ * never turns more than 90deg.
+ */
+ public boolean moduleStateOptimization = true;
/**
* Simulated swerve module.
*/
- private SwerveModuleSimulation simModule;
+ private SwerveModuleSimulation simModule;
/**
* Encoder synchronization queued.
*/
- private boolean synchronizeEncoderQueued = false;
+ private boolean synchronizeEncoderQueued = false;
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
@@ -144,10 +149,13 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop, boolean force)
{
- desiredState = SwerveModuleState2.optimize(desiredState,
- Rotation2d.fromDegrees(getAbsolutePosition()),
- lastState,
- configuration.moduleSteerFFCL);
+ if (moduleStateOptimization)
+ {
+ desiredState = SwerveModuleState2.optimize(desiredState,
+ Rotation2d.fromDegrees(getAbsolutePosition()),
+ lastState,
+ configuration.moduleSteerFFCL);
+ }
if (isOpenLoop)
{
diff --git a/swervelib/encoders/CanAndCoderSwerve.java b/swervelib/encoders/CanAndCoderSwerve.java
new file mode 100644
index 0000000..e1489be
--- /dev/null
+++ b/swervelib/encoders/CanAndCoderSwerve.java
@@ -0,0 +1,80 @@
+package swervelib.encoders;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import swervelib.motors.SwerveMotor;
+
+/**
+ * SparkMax absolute encoder, attached through the data port.
+ */
+public class CanAndCoderSwerve extends SwerveAbsoluteEncoder {
+
+ /**
+ * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to
+ * the SparkMax.
+ */
+ public AbsoluteEncoder encoder;
+
+ /**
+ * Create the {@link AbsoluteEncoder} object as a duty cycle. from the
+ * {@link CANSparkMax} motor.
+ *
+ * @param motor Motor to create the encoder from.
+ */
+ public CanAndCoderSwerve(SwerveMotor motor) {
+ if (motor.getMotor() instanceof CANSparkMax) {
+ encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle);
+ encoder.setPositionConversionFactor(360);
+ encoder.setVelocityConversionFactor(360);
+ } else {
+ throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
+ }
+ }
+
+ /**
+ * Reset the encoder to factory defaults.
+ */
+ @Override
+ public void factoryDefault() {
+ // Do nothing
+ }
+
+ /**
+ * Clear sticky faults on the encoder.
+ */
+ @Override
+ public void clearStickyFaults() {
+ // Do nothing
+ }
+
+ /**
+ * Configure the absolute encoder to read from [0, 360) per second.
+ *
+ * @param inverted Whether the encoder is inverted.
+ */
+ @Override
+ public void configure(boolean inverted) {
+ encoder.setInverted(inverted);
+ }
+
+ /**
+ * Get the absolute position of the encoder.
+ *
+ * @return Absolute position in degrees from [0, 360).
+ */
+ @Override
+ public double getAbsolutePosition() {
+ return encoder.getPosition();
+ }
+
+ /**
+ * Get the instantiated absolute encoder Object.
+ *
+ * @return Absolute encoder object.
+ */
+ @Override
+ public Object getAbsoluteEncoder() {
+ return encoder;
+ }
+}
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index 2637e63..4661348 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -89,7 +89,7 @@ public class SwerveMath
public static double calculateMaxAngularVelocity(
double maxSpeed, double furthestModuleX, double furthestModuleY)
{
- return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
+ return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians();
}
/**
diff --git a/swervelib/math/SwerveModuleState2.java b/swervelib/math/SwerveModuleState2.java
index 19e1d6f..b784355 100644
--- a/swervelib/math/SwerveModuleState2.java
+++ b/swervelib/math/SwerveModuleState2.java
@@ -2,7 +2,6 @@ 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.
@@ -63,36 +62,87 @@ public class SwerveModuleState2 extends SwerveModuleState
public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotation2d currentAngle,
SwerveModuleState2 lastState, double moduleSteerFeedForwardClosedLoop)
{
+ SwerveModuleState2 optimized = new SwerveModuleState2(SwerveModuleState.optimize(desiredState, currentAngle),
+ desiredState.omegaRadPerSecond);
+ if (desiredState.angle.equals(currentAngle) || desiredState.angle.equals(
+ optimized.angle.rotateBy(Rotation2d.fromDegrees(180))) || moduleSteerFeedForwardClosedLoop == 0)
+ {
+ optimized.omegaRadPerSecond = 0;
+ }
+ if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(180))))
+ {
+ desiredState.omegaRadPerSecond = 0;
+ return desiredState;
+ }
+ return optimized;
+ /*
+ // NEVER optimize if it's the same angle, it just doesn't make sense...
+ if (currentAngle.equals(desiredState.angle.rotateBy(Rotation2d.fromDegrees(180))))
+ {
+ desiredState.invert();
+ desiredState.omegaRadPerSecond = 0;
+ return desiredState;
+ } else if (currentAngle.equals(desiredState.angle))
+ {
+ desiredState.omegaRadPerSecond = 0;
+ return desiredState;
+ }
+
+ SwerveModuleState2 optimized;
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);
+ optimized = 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)
+ Rotation2d delta = desiredState.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
+ moduleSteerFeedForwardClosedLoop *
+ 0.065))
+ .minus(currentAngle);
+ if (Double.compare(Math.abs(delta.getDegrees()), 90.0) < 0)
{
- targetSpeed = -targetSpeed;
- if (delta > 90)
- {
- targetAngle -= 180;
- } else
- {
- targetAngle += 180;
- }
+ optimized = desiredState.invert();
+ optimized.angle = optimized.angle.plus(Rotation2d.fromRadians(lastState.omegaRadPerSecond *
+ moduleSteerFeedForwardClosedLoop *
+ 0.065));
+ } else
+ {
+ optimized = desiredState;
}
- return new SwerveModuleState2(targetSpeed, Rotation2d.fromDegrees(targetAngle), desiredState.omegaRadPerSecond);
}
+
+ if (Double.compare(Math.abs(currentAngle.minus(optimized.angle).getDegrees()),
+ Math.abs(currentAngle.minus(desiredState.angle)
+ .getDegrees())) > 0)
+ {
+ desiredState.omegaRadPerSecond = 0;
+ return desiredState;
+ }
+
+ // Maybe the omega rad per second will always be off when optimized?
+// optimized.omegaRadPerSecond = 0;
+ return optimized;
+ */
+ }
+
+ /**
+ * Invert the swerve module state.
+ *
+ * @return Current inverted {@link SwerveModuleState2}
+ */
+ public SwerveModuleState2 invert()
+ {
+// omegaRadPerSecond *= -1;
+// speedMetersPerSecond *= -1;
+// angle = angle.rotateBy(Rotation2d.fromDegrees(180));
+// return this;
+ return new SwerveModuleState2(-speedMetersPerSecond,
+ angle.rotateBy(Rotation2d.fromDegrees(180)),
+ -omegaRadPerSecond);
}
/**
diff --git a/swervelib/math/SwervePoseEstimator2.java b/swervelib/math/SwervePoseEstimator2.java
index 46282d7..b61e3e6 100644
--- a/swervelib/math/SwervePoseEstimator2.java
+++ b/swervelib/math/SwervePoseEstimator2.java
@@ -224,6 +224,13 @@ public class SwervePoseEstimator2 // extends SwerveDrivePoseEstimator
sample.get().modulePositions,
sample.get().poseMeters.exp(scaledTwist));
+ // Step 6: Record the current pose to allow multiple measurements from the same timestamp
+// m_poseBuffer.addSample(
+// timestampSeconds,
+// new InterpolationRecord(
+// getEstimatedPosition(), sample.get().gyroAngle, sample.get().gyroPitch, sample.get().gyroRoll,
+// sample.get().modulePositions));
+
// Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry entry :
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index b4f9db4..8daece1 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -269,8 +269,12 @@ public class SparkMaxSwerve extends SwerveMotor
* Save the configurations from flash to EEPROM.
*/
@Override
- public void burnFlash()
+ public void burnFlash()
{
+ try {
+ Thread.sleep(200);
+ } catch (Exception e) {
+ }
motor.burnFlash();
}
diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java
index 52d6ba0..2ff1b18 100644
--- a/swervelib/parser/json/DeviceJson.java
+++ b/swervelib/parser/json/DeviceJson.java
@@ -4,6 +4,7 @@ import com.revrobotics.SparkMaxRelativeEncoder.Type;
import edu.wpi.first.wpilibj.SerialPort.Port;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
import swervelib.encoders.CANCoderSwerve;
+import swervelib.encoders.CanAndCoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.imu.ADIS16448Swerve;
@@ -42,16 +43,33 @@ public class DeviceJson
/**
* Create a {@link SwerveAbsoluteEncoder} from the current configuration.
*
+ * @param motor {@link SwerveMotor} of which attached encoders will be created from, only used when the type is
+ * "attached" or "canandencoder".
* @return {@link SwerveAbsoluteEncoder} given.
*/
- public SwerveAbsoluteEncoder createEncoder()
+ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
{
+ SwerveAbsoluteEncoder attachedChoice = null;
switch (type)
{
case "none":
case "integrated":
case "attached":
- return null;
+ if (motor instanceof SparkMaxBrushedMotorSwerve || motor instanceof SparkMaxSwerve)
+ {
+ attachedChoice = new SparkMaxEncoderSwerve(motor);
+ motor.setAbsoluteEncoder(attachedChoice);
+ } else if (motor instanceof TalonFXSwerve || motor instanceof TalonSRXSwerve)
+ {
+ motor.setAbsoluteEncoder(null);
+ } else
+ {
+ throw new RuntimeException(
+ "Could not create absolute encoder from data port of " + type + " id " + id);
+ }
+ return attachedChoice;
+ case "canandcoder":
+ return new CanAndCoderSwerve(motor);
case "thrifty":
case "throughbore":
case "dutycycle":
@@ -142,24 +160,4 @@ public class DeviceJson
throw new RuntimeException(type + " is not a recognized absolute encoder type.");
}
}
-
- /**
- * Create a {@link SwerveAbsoluteEncoder} from the data port on the motor controller.
- *
- * @param motor The motor to create the absolute encoder from.
- * @return {@link SwerveAbsoluteEncoder} from the motor controller.
- */
- public SwerveAbsoluteEncoder createIntegratedEncoder(SwerveMotor motor)
- {
- switch (type)
- {
- case "sparkmax":
- return new SparkMaxEncoderSwerve(motor);
- case "falcon":
- case "talonfx":
- return null;
- }
- throw new RuntimeException(
- "Could not create absolute encoder from data port of " + type + " id " + id);
- }
}
diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java
index e3f31c4..34951d0 100644
--- a/swervelib/parser/json/ModuleJson.java
+++ b/swervelib/parser/json/ModuleJson.java
@@ -66,14 +66,7 @@ public class ModuleJson
String name)
{
SwerveMotor angleMotor = angle.createMotor(false);
- SwerveAbsoluteEncoder absEncoder = encoder.createEncoder();
-
- // If the absolute encoder is attached.
- if (absEncoder == null)
- {
- absEncoder = angle.createIntegratedEncoder(angleMotor);
- angleMotor.setAbsoluteEncoder(absEncoder);
- }
+ SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
return new SwerveModuleConfiguration(
drive.createMotor(true),
diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java
index 1a398bd..64fbc0e 100644
--- a/swervelib/parser/json/PhysicalPropertiesJson.java
+++ b/swervelib/parser/json/PhysicalPropertiesJson.java
@@ -39,7 +39,7 @@ public class PhysicalPropertiesJson
* 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 moduleFeedForwardClosedLoop = SwerveDriveTelemetry.isSimulation ? -0.33 : 0;
+ public double moduleFeedForwardClosedLoop = SwerveDriveTelemetry.isSimulation ? 0.33 : 0;
/**
* DEPRECATED: No longer needed, tune {@link PhysicalPropertiesJson#moduleFeedForwardClosedLoop} instead.
*/