getEstimatedGlobalPose()
+ {
+ updateUnreadResults();
+ return estimatedRobotPose;
+ }
+
+ /**
+ * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
+ */
+ private void updateUnreadResults()
+ {
+ double mostRecentTimestamp = resultsList.get(0).getTimestampSeconds();
+ double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
+ double debounceTime = Milliseconds.of(15).in(Seconds);
+ for (PhotonPipelineResult result : resultsList)
+ {
+ mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
+ }
+ if ((resultsList.isEmpty() || (currentTimestamp - mostRecentTimestamp >= debounceTime)) &&
+ (currentTimestamp - lastReadTimestamp) >= debounceTime)
+ {
+ resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
+ lastReadTimestamp = currentTimestamp;
+ resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
+ return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
+ });
+ if (!resultsList.isEmpty())
+ {
+ updateEstimatedGlobalPose();
+ }
+ }
+ }
+
+ /**
+ * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
+ * per loop.
+ *
+ * Also includes updates for the standard deviations, which can (optionally) be retrieved with
+ * {@link Cameras#updateEstimationStdDevs}
+ *
+ * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
+ * estimation.
+ */
+ private void updateEstimatedGlobalPose()
+ {
+ Optional visionEst = Optional.empty();
+ for (var change : camera.getAllUnreadResults())
+ {
+ visionEst = poseEstimator.update(change);
+ updateEstimationStdDevs(visionEst, change.getTargets());
+ }
+ estimatedRobotPose = visionEst;
+ }
+
+ /**
+ * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
+ * on number of tags, estimation strategy, and distance from the tags.
+ *
+ * @param estimatedPose The estimated pose to guess standard deviations for.
+ * @param targets All targets in this camera frame
+ */
+ private void updateEstimationStdDevs(
+ Optional estimatedPose, List targets)
+ {
+ if (estimatedPose.isEmpty())
+ {
+ // No pose input. Default to single-tag std devs
+ curStdDevs = singleTagStdDevs;
+
+ } else
+ {
+ // Pose present. Start running Heuristic
+ var estStdDevs = singleTagStdDevs;
+ int numTags = 0;
+ double avgDist = 0;
+
+ // Precalculation - see how many tags we found, and calculate an average-distance metric
+ for (var tgt : targets)
+ {
+ var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+ if (tagPose.isEmpty())
+ {
+ continue;
+ }
+ numTags++;
+ avgDist +=
+ tagPose
+ .get()
+ .toPose2d()
+ .getTranslation()
+ .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+ }
+
+ if (numTags == 0)
+ {
+ // No tags visible. Default to single-tag std devs
+ curStdDevs = singleTagStdDevs;
+ } else
+ {
+ // One or more tags visible, run the full heuristic.
+ avgDist /= numTags;
+ // Decrease std devs if multiple targets are visible
+ if (numTags > 1)
+ {
+ estStdDevs = multiTagStdDevs;
+ }
+ // Increase std devs based on (average) distance
+ if (numTags == 1 && avgDist > 4)
+ {
+ estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+ } else
+ {
+ estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+ }
+ curStdDevs = estStdDevs;
+ }
+ }
+ }
+
+
}
}
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
index 7d83e5d0..74ed27f6 100644
--- a/src/main/java/swervelib/SwerveDrive.java
+++ b/src/main/java/swervelib/SwerveDrive.java
@@ -1,5 +1,16 @@
package swervelib;
+import static edu.wpi.first.hal.FRCNetComm.tInstances.kRobotDriveSwerve_YAGSL;
+import static edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_RobotDrive;
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.Kilograms;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -18,7 +29,11 @@
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.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -29,6 +44,10 @@
import java.util.Optional;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
+import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
+import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.IMUVelocity;
import swervelib.imu.Pigeon2Swerve;
@@ -39,8 +58,6 @@
import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.simulation.SwerveIMUSimulation;
-import swervelib.telemetry.Alert;
-import swervelib.telemetry.Alert.AlertType;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -84,7 +101,7 @@ public class SwerveDrive
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
- AlertType.WARNING);
+ AlertType.kWarning);
/**
* Field object.
*/
@@ -107,20 +124,24 @@ public class SwerveDrive
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
*/
- public boolean angularVelocityCorrection = false;
+ public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto.
*/
- public boolean autonomousAngularVelocityCorrection = false;
+ public boolean autonomousAngularVelocityCorrection = false;
/**
* Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
*/
- public double angularVelocityCoefficient = 0;
+ public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
+ /**
+ * MapleSim SwerveDrive.
+ */
+ private SwerveDriveSimulation mapleSimDrive;
/**
* Amount of seconds the duration of the timestep the speeds should be applied for.
*/
@@ -137,7 +158,7 @@ public class SwerveDrive
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/
- private IMUVelocity imuVelocity;
+ private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
@@ -175,9 +196,11 @@ public class SwerveDrive
* {@link SwerveController}.
* @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if
* you have feet per second!
+ * @param startingPose Starting {@link Pose2d} on the field.
*/
public SwerveDrive(
- SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS)
+ SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS,
+ Pose2d startingPose)
{
this.maxSpeedMPS = maxSpeedMPS;
swerveDriveConfiguration = config;
@@ -186,11 +209,48 @@ public SwerveDrive(
kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters);
odometryThread = new Notifier(this::updateOdometry);
+ this.swerveModules = config.modules;
+
// Create an integrator for angle if the robot is being simulated to emulate an IMU
// If the robot is real, instantiate the IMU instead.
if (SwerveDriveTelemetry.isSimulation)
{
- simIMU = new SwerveIMUSimulation();
+ DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default()
+ .withBumperSize(
+ Meters.of(config.getTracklength())
+ .plus(Inches.of(5)),
+ Meters.of(config.getTrackwidth())
+ .plus(Inches.of(5)))
+ .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg))
+ .withCustomModuleTranslations(config.moduleLocationsMeters)
+ .withGyro(config.getGyroSim())
+ .withSwerveModule(() -> new SwerveModuleSimulation(
+ config.getDriveMotorSim(),
+ config.getAngleMotorSim(),
+ config.physicalCharacteristics.conversionFactor.drive.gearRatio,
+ config.physicalCharacteristics.conversionFactor.angle.gearRatio,
+ Amps.of(config.physicalCharacteristics.driveMotorCurrentLimit),
+ Amps.of(20),
+ Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
+ Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
+ Inches.of(
+ config.physicalCharacteristics.conversionFactor.drive.diameter /
+ 2),
+ KilogramSquareMeters.of(0.02),
+ config.physicalCharacteristics.wheelGripCoefficientOfFriction));
+
+ mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
+
+ // feed module simulation instances to modules
+ for (int i = 0; i < swerveModules.length; i++)
+ {
+ this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i]);
+ }
+
+ // register the drivetrain simulation
+ SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
+
+ simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
} else
{
@@ -199,16 +259,13 @@ public SwerveDrive(
imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
}
- this.swerveModules = config.modules;
-
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator =
new SwerveDrivePoseEstimator(
kinematics,
getYaw(),
getModulePositions(),
- new Pose2d(new Translation2d(0, 0),
- Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
+ startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
setMaximumSpeed(maxSpeedMPS);
@@ -244,11 +301,15 @@ public SwerveDrive(
}
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
+ SwerveDriveTelemetry.desiredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount];
+ SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount];
}
- odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02);
+ setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.004 : 0.02);
checkIfTunerXCompatible();
+
+ HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL);
}
/**
@@ -300,6 +361,7 @@ private void checkIfTunerXCompatible()
public void setOdometryPeriod(double period)
{
odometryThread.stop();
+ SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
odometryThread.startPeriodic(period);
}
@@ -309,6 +371,7 @@ public void setOdometryPeriod(double period)
public void stopOdometryThread()
{
odometryThread.stop();
+ SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5);
}
/**
@@ -381,9 +444,8 @@ public void setHeadingCorrection(boolean state, double deadband)
public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity,
ChassisSpeeds robotOrientedVelocity)
{
- ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading())
- .plus(robotOrientedVelocity);
- drive(TotalVelocties);
+ fieldOrientedVelocity.toRobotRelativeSpeeds(getOdometryHeading());
+ drive(fieldOrientedVelocity.plus(robotOrientedVelocity));
}
/**
@@ -393,8 +455,8 @@ public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVeloci
*/
public void driveFieldOriented(ChassisSpeeds velocity)
{
- ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
- drive(fieldOrientedVelocity);
+ velocity.toRobotRelativeSpeeds(getOdometryHeading());
+ drive(velocity);
}
/**
@@ -405,8 +467,8 @@ public void driveFieldOriented(ChassisSpeeds velocity)
*/
public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
{
- ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
- drive(fieldOrientedVelocity, centerOfRotationMeters);
+ velocity.toRobotRelativeSpeeds(getOdometryHeading());
+ drive(velocity, centerOfRotationMeters);
}
/**
@@ -454,12 +516,11 @@ public void drive(
{
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
// necessary.
- ChassisSpeeds velocity =
- fieldRelative
- ? ChassisSpeeds.fromFieldRelativeSpeeds(
- translation.getX(), translation.getY(), rotation, getOdometryHeading())
- : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
-
+ ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
+ if (fieldRelative)
+ {
+ velocity.toRobotRelativeSpeeds(getOdometryHeading());
+ }
drive(velocity, isOpenLoop, centerOfRotationMeters);
}
@@ -483,12 +544,12 @@ public void drive(
{
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
// necessary.
- ChassisSpeeds velocity =
- fieldRelative
- ? ChassisSpeeds.fromFieldRelativeSpeeds(
- translation.getX(), translation.getY(), rotation, getOdometryHeading())
- : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
+ ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
+ if (fieldRelative)
+ {
+ velocity.toRobotRelativeSpeeds(getOdometryHeading());
+ }
drive(velocity, isOpenLoop, new Translation2d());
}
@@ -524,15 +585,9 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
}
// Display commanded speed for testing
- if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO)
- {
- SmartDashboard.putString("RobotVelocity", velocity.toString());
- }
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
- SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;
- SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond;
- SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond);
+ SwerveDriveTelemetry.desiredChassisSpeedsObj = velocity;
}
// Calculate required module states via kinematics
@@ -634,6 +689,27 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo
}
}
+ /**
+ * Drive the robot using the {@link SwerveModuleState[]}, it is recommended to have
+ * {@link SwerveDrive#setCosineCompensator(boolean)} set to false for this.
+ *
+ * @param robotRelativeVelocity Robot relative {@link ChassisSpeeds}
+ * @param states Corresponding {@link SwerveModuleState[]} to use (not checked against the
+ * {@param robotRelativeVelocity}).
+ * @param feedforwardAmp Feedforward in amperage. (Ignored for now)
+ */
+ public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, double[] feedforwardAmp)
+ {
+ if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
+ {
+ SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity;
+ }
+ for (SwerveModule module : swerveModules)
+ {
+ module.setDesiredState(states[module.moduleNumber], false, 0); //feedforwardAmp[module.moduleNumber]);
+ }
+ }
+
/**
* Set chassis speeds with closed-loop velocity control.
*
@@ -646,15 +722,13 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);
- SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
- SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
- SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
+ SwerveDriveTelemetry.desiredChassisSpeedsObj = chassisSpeeds;
setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false);
}
/**
- * Gets the current pose (position and rotation) of the robot, as reported by odometry.
+ * Gets the measured pose (position and rotation) of the robot, as reported by odometry.
*
* @return The robot's pose
*/
@@ -668,7 +742,26 @@ public Pose2d getPose()
}
/**
- * Gets the current field-relative velocity (x, y and omega) of the robot
+ * Gets the actual pose of the drivetrain during simulation
+ *
+ * @return an optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running
+ * on real robot
+ */
+ public Optional getSimulationDriveTrainPose()
+ {
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ odometryLock.lock();
+ Pose2d simulationPose = mapleSimDrive.getSimulatedDriveTrainPose();
+ odometryLock.unlock();
+ return Optional.of(simulationPose);
+ }
+
+ return Optional.empty();
+ }
+
+ /**
+ * Gets the measured field-relative robot velocity (x, y and omega)
*
* @return A ChassisSpeeds object of the current field-relative velocity
*/
@@ -678,8 +771,30 @@ public ChassisSpeeds getFieldVelocity()
// but not the reverse. However, because this transform is a simple rotation, negating the
// angle
// given as the robot angle reverses the direction of rotation, and the conversion is reversed.
- return ChassisSpeeds.fromFieldRelativeSpeeds(
- kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
+ ChassisSpeeds robotRelativeSpeeds = kinematics.toChassisSpeeds(getStates());
+ robotRelativeSpeeds.toFieldRelativeSpeeds(getOdometryHeading().unaryMinus());
+ return robotRelativeSpeeds;
+ }
+
+ /**
+ * Gets the actual field-relative robot velocity (x, y and omega) during simulation
+ *
+ * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty
+ * optional when running on real robot
+ * @deprecated for testing version of maple-sim only
+ */
+ @Deprecated
+ public Optional getSimulationFieldVelocity()
+ {
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ odometryLock.lock();
+ ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsFieldRelative();
+ odometryLock.unlock();
+ return Optional.of(simulationFieldRelativeVelocity);
+ }
+
+ return Optional.empty();
}
/**
@@ -692,6 +807,27 @@ public ChassisSpeeds getRobotVelocity()
return kinematics.toChassisSpeeds(getStates());
}
+ /**
+ * Gets the actual robot-relative robot velocity (x, y and omega) during simulation
+ *
+ * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty
+ * optional when running on real robot
+ * @deprecated for testing version of maple-sim only
+ */
+ @Deprecated
+ public Optional getSimulationRobotVelocity()
+ {
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ odometryLock.lock();
+ ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative();
+ odometryLock.unlock();
+ return Optional.of(simulationFieldRelativeVelocity);
+ }
+
+ return Optional.empty();
+ }
+
/**
* Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
* method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
@@ -703,8 +839,15 @@ public void resetOdometry(Pose2d pose)
{
odometryLock.lock();
swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ mapleSimDrive.setSimulationWorldPose(pose);
+ }
odometryLock.unlock();
- kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw()));
+ ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds();
+ robotRelativeSpeeds.toFieldRelativeSpeeds(getYaw());
+ kinematics.toSwerveModuleStates(robotRelativeSpeeds);
+
}
/**
@@ -943,10 +1086,7 @@ public void lockPose()
new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle());
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
- SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] =
- desiredState.angle.getDegrees();
- SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] =
- desiredState.speedMetersPerSecond;
+ SwerveDriveTelemetry.desiredStatesObj[swerveModule.moduleNumber] = desiredState;
}
swerveModule.setDesiredState(desiredState, false, true);
@@ -996,34 +1136,42 @@ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforwa
public void updateOdometry()
{
odometryLock.lock();
+ invalidateCache();
try
{
// Update odometry
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
- // Update angle accumulator if the robot is simulated
- if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
+ if (SwerveDriveTelemetry.isSimulation)
{
- Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
- if (SwerveDriveTelemetry.isSimulation)
+ try
+ {
+ SimulatedArena.getInstance().simulationPeriodic();
+ } catch (Exception e)
{
- simIMU.updateOdometry(
- kinematics,
- getStates(),
- modulePoses,
- field);
+ DriverStation.reportError("MapleSim error", false);
}
+ }
- ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
- SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
- SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
- SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
- SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
+ // Update angle accumulator if the robot is simulated
+ if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
+ {
+ SwerveDriveTelemetry.measuredChassisSpeedsObj = getRobotVelocity();
+ SwerveDriveTelemetry.robotRotationObj = getOdometryHeading();
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
{
- field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose());
+ field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition());
+ field.getObject("XModules").setPoses(getSwerveModulePoses(mapleSimDrive.getSimulatedDriveTrainPose()));
+
+ } else
+ {
+ field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+ }
}
double sumVelocity = 0;
@@ -1039,8 +1187,7 @@ public void updateOdometry()
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
- SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
- SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
+ SwerveDriveTelemetry.measuredStatesObj[module.moduleNumber] = moduleState;
}
}
@@ -1065,6 +1212,18 @@ public void updateOdometry()
odometryLock.unlock();
}
+ /**
+ * Invalidate all {@link Cache} object used by the {@link SwerveDrive}
+ */
+ public void invalidateCache()
+ {
+ imuReadingCache.update();
+ for (SwerveModule module : swerveModules)
+ {
+ module.invalidateCache();
+ }
+ }
+
/**
* Synchronize angle motor integrated encoders with data from absolute encoders.
*/
@@ -1259,8 +1418,8 @@ public void setCosineCompensator(boolean enabled)
/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
*
- * @param enable Enable chassis velocity correction, which will use
- * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following.
+ * @param enable Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
+ * the following.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
@@ -1276,10 +1435,10 @@ public void setChassisDiscretization(boolean enable, double dtSeconds)
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
* and/or auto
*
- * @param useInTeleop Enable chassis velocity correction, which will use
- * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
- * @param useInAuto Enable chassis velocity correction, which will use
- * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
+ * @param useInTeleop Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
+ * the following in teleop.
+ * @param useInAuto Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
+ * the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
@@ -1328,12 +1487,8 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
if (angularVelocity.getRadians() != 0.0)
{
- velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
- velocity.vxMetersPerSecond,
- velocity.vyMetersPerSecond,
- velocity.omegaRadiansPerSecond,
- getOdometryHeading());
- velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
+ velocity.toFieldRelativeSpeeds(getOdometryHeading());
+ velocity.toRobotRelativeSpeeds(getOdometryHeading().plus(angularVelocity));
}
return velocity;
}
@@ -1359,10 +1514,25 @@ private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesC
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (uesChassisDiscretize)
{
- velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
+ velocity.discretize(discretizationdtSeconds);
}
return velocity;
}
+ /**
+ * Convert a {@link ChassisSpeeds} to {@link SwerveModuleState[]} for use elsewhere.
+ *
+ * @param velocity {@link ChassisSpeeds} velocity to use.
+ * @param optimize Perform chassis velocity correction or angular velocity correction.
+ * @return {@link SwerveModuleState[]} for use elsewhere.
+ */
+ public SwerveModuleState[] toServeModuleStates(ChassisSpeeds velocity, boolean optimize)
+ {
+ if (optimize)
+ {
+ velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection);
+ }
+ return kinematics.toSwerveModuleStates(velocity);
+ }
}
diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java
index a3d71dfe..70405a3e 100644
--- a/src/main/java/swervelib/SwerveDriveTest.java
+++ b/src/main/java/swervelib/SwerveDriveTest.java
@@ -1,20 +1,20 @@
package swervelib;
-import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Meter;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.Angle;
-import edu.wpi.first.units.Distance;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.MutableMeasure;
-import edu.wpi.first.units.Velocity;
-import edu.wpi.first.units.Voltage;
+import edu.wpi.first.units.measure.MutAngle;
+import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutDistance;
+import edu.wpi.first.units.measure.MutLinearVelocity;
+import edu.wpi.first.units.measure.MutVoltage;
+import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
@@ -37,23 +37,23 @@ public class SwerveDriveTest
/**
* Tracks the voltage being applied to a motor
*/
- private static final MutableMeasure m_appliedVoltage = mutable(Volts.of(0));
+ private static final MutVoltage m_appliedVoltage = new MutVoltage(0, 0, Volts);
/**
* Tracks the distance travelled of a position motor
*/
- private static final MutableMeasure m_distance = mutable(Meters.of(0));
+ private static final MutDistance m_distance = new MutDistance(0, 0, Meter);
/**
* Tracks the velocity of a positional motor
*/
- private static final MutableMeasure> m_velocity = mutable(MetersPerSecond.of(0));
+ private static final MutLinearVelocity m_velocity = new MutLinearVelocity(0, 9, MetersPerSecond);
/**
* Tracks the rotations of an angular motor
*/
- private static final MutableMeasure m_anglePosition = mutable(Degrees.of(0));
+ private static final MutAngle m_anglePosition = new MutAngle(0, 0, Degrees);
/**
* Tracks the velocity of an angular motor
*/
- private static final MutableMeasure> m_angVelocity = mutable(DegreesPerSecond.of(0));
+ private static final MutAngularVelocity m_angVelocity = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Set the angle of the modules to a given {@link Rotation2d}
@@ -214,7 +214,7 @@ public static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo
Timer.delay(1);
Rotation2d startingAbsoluteEncoderPosition = Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition());
double driveEncoderPositionRotations = module.getDriveMotor().getPosition() /
- module.configuration.conversionFactors.drive;
+ module.configuration.conversionFactors.drive.factor;
if (automatic)
{
module.getAngleMotor().setVoltage(volts);
@@ -230,8 +230,9 @@ public static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo
false);
Timer.delay(60);
}
- double couplingRatio = (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive) -
- driveEncoderPositionRotations;
+ double couplingRatio =
+ (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive.factor) -
+ driveEncoderPositionRotations;
DriverStation.reportWarning(module.configuration.name + " Coupling Ratio: " + couplingRatio, false);
couplingRatioSum += couplingRatio;
}
@@ -308,7 +309,7 @@ public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swe
SwerveDrive swerveDrive, double maxVolts)
{
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
- (Measure voltage) -> {
+ (Voltage voltage) -> {
SwerveDriveTest.centerModules(swerveDrive);
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
},
@@ -380,7 +381,7 @@ public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swe
SwerveDrive swerveDrive)
{
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
- (Measure voltage) -> {
+ (Voltage voltage) -> {
SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
},
diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java
index 8b5dec10..e989cdab 100644
--- a/src/main/java/swervelib/SwerveModule.java
+++ b/src/main/java/swervelib/SwerveModule.java
@@ -1,20 +1,25 @@
package swervelib;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.MotorFeedbackSensor;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
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.units.measure.LinearVelocity;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
+import swervelib.motors.SparkMaxBrushedMotorSwerve;
+import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.parser.Cache;
import swervelib.parser.PIDFConfig;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.simulation.SwerveModuleSimulation;
-import swervelib.telemetry.Alert;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -43,7 +48,7 @@ public class SwerveModule
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
- public final int moduleNumber;
+ public final int moduleNumber;
/**
* Swerve Motors.
*/
@@ -95,7 +100,7 @@ public class SwerveModule
/**
* Anti-Jitter AKA auto-centering disabled.
*/
- private boolean antiJitterEnabled = true;
+ private boolean antiJitterEnabled = true;
/**
* Last swerve module state applied.
*/
@@ -111,15 +116,15 @@ public class SwerveModule
/**
* Encoder synchronization queued.
*/
- private boolean synchronizeEncoderQueued = false;
+ private boolean synchronizeEncoderQueued = false;
/**
* Encoder, Absolute encoder synchronization enabled.
*/
- private boolean synchronizeEncoderEnabled = false;
+ private boolean synchronizeEncoderEnabled = false;
/**
* Encoder synchronization deadband in degrees.
*/
- private double synchronizeEncoderDeadband = 3;
+ private double synchronizeEncoderDeadband = 3;
/**
@@ -166,13 +171,18 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
}
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ simModule = new SwerveModuleSimulation();
+ }
+
// Setup the cache for the absolute encoder position.
- absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15);
+ absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
// Config angle motor/controller
- angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
+ angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
- angleMotor.configurePIDWrapping(0, 180);
+ angleMotor.configurePIDWrapping(0, 360);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
angleMotor.setMotorBrake(false);
@@ -183,7 +193,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat
}
// Config drive motor/controller
- driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive);
+ driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive.factor);
driveMotor.configurePIDF(moduleConfiguration.velocityPIDF);
driveMotor.setInverted(moduleConfiguration.driveMotorInverted);
driveMotor.setMotorBrake(true);
@@ -191,8 +201,8 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat
driveMotor.burnFlash();
angleMotor.burnFlash();
- drivePositionCache = new Cache<>(driveMotor::getPosition, 15);
- driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15);
+ drivePositionCache = new Cache<>(driveMotor::getPosition, 20);
+ driveVelocityCache = new Cache<>(driveMotor::getVelocity, 20);
if (SwerveDriveTelemetry.isSimulation)
{
@@ -210,11 +220,11 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat
noEncoderWarning = new Alert("Motors",
"There is no Absolute Encoder on module #" +
moduleNumber,
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
encoderOffsetWarning = new Alert("Motors",
"Pushing the Absolute Encoder offset to the encoder failed on module #" +
moduleNumber,
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
rawAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Raw Absolute Encoder";
adjAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder";
@@ -361,7 +371,8 @@ public void setAnglePIDF(PIDFConfig config)
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
- desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
+
+ desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
// If we are forcing the angle
if (!force && antiJitterEnabled)
@@ -371,9 +382,26 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
}
// Cosine compensation.
- double velocity = configuration.useCosineCompensator
- ? getCosineCompensatedVelocity(desiredState)
- : desiredState.speedMetersPerSecond;
+ LinearVelocity nextVelocity = configuration.useCosineCompensator
+ ? getCosineCompensatedVelocity(desiredState)
+ : MetersPerSecond.of(desiredState.speedMetersPerSecond);
+ LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
+ desiredState.speedMetersPerSecond = nextVelocity.magnitude();
+
+ setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(curVelocity, nextVelocity).magnitude());
+ }
+
+ /**
+ * Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from
+ * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics}
+ *
+ * @param desiredState Desired swerve module state.
+ * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
+ * @param driveFeedforwardVoltage Drive motor controller feedforward as a voltage.
+ */
+ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
+ double driveFeedforwardVoltage)
+ {
if (isOpenLoop)
{
@@ -381,8 +409,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
driveMotor.set(percentOutput);
} else
{
- driveMotor.setReference(velocity, driveMotorFeedforward.calculate(velocity));
- desiredState.speedMetersPerSecond = velocity;
+ driveMotor.setReference(desiredState.speedMetersPerSecond, driveFeedforwardVoltage);
}
// Prevent module rotation if angle is the same as the previous angle.
@@ -408,10 +435,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
simModule.updateStateAndPosition(desiredState);
}
+ // TODO: Change and move to SwerveDriveTelemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
- SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees();
- SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity;
+ SwerveDriveTelemetry.desiredStatesObj[moduleNumber] = desiredState;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
@@ -429,7 +456,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
* @param desiredState Desired {@link SwerveModuleState} to use.
* @return Cosine compensated velocity in meters/second.
*/
- private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
+ private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState)
{
double cosineScalar = 1.0;
// Taken from the CTRE SwerveModule class.
@@ -447,7 +474,7 @@ private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
cosineScalar = 1;
}
- return desiredState.speedMetersPerSecond * (cosineScalar);
+ return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar);
}
/**
@@ -518,6 +545,13 @@ public double getAbsolutePosition()
*/
public double getRawAbsolutePosition()
{
+ /* During simulation, when no absolute encoders are available, we return the state from the simulation module instead. */
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ Rotation2d absolutePosition = simModule.getState().angle;
+ return absolutePosition.getDegrees();
+ }
+
double angle;
if (absoluteEncoder != null)
{
@@ -629,9 +663,9 @@ public void pushOffsetsToEncoders()
if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
{
// If the absolute encoder is attached.
- if (angleMotor.getMotor() instanceof CANSparkMax)
+ if (angleMotor instanceof SparkMaxSwerve || angleMotor instanceof SparkMaxBrushedMotorSwerve)
{
- if (absoluteEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
+ if (absoluteEncoder instanceof SparkMaxEncoderSwerve)
{
angleMotor.setAbsoluteEncoder(absoluteEncoder);
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
@@ -687,9 +721,42 @@ public void updateTelemetry()
SmartDashboard.putNumber(rawAbsoluteAngleName, absoluteEncoder.getAbsolutePosition());
}
SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition());
- SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition());
- SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity());
+ SmartDashboard.putNumber(rawDriveName, drivePositionCache.getValue());
+ SmartDashboard.putNumber(rawDriveVelName, driveVelocityCache.getValue());
SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0);
}
+
+ /**
+ * Invalidate the {@link Cache} objects used by {@link SwerveModule}.
+ */
+ public void invalidateCache()
+ {
+ absolutePositionCache.update();
+ drivePositionCache.update();
+ driveVelocityCache.update();
+ }
+
+ /**
+ * Obtains the {@link SwerveModuleSimulation} used in simulation.
+ *
+ * @return the module simulation, null if this method is called on a real robot
+ */
+ public SwerveModuleSimulation getSimModule()
+ {
+ return simModule;
+ }
+
+ /**
+ * Configure the {@link SwerveModule#simModule} with the MapleSim
+ * {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
+ *
+ * @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to
+ * configure with.
+ */
+ public void configureModuleSimulation(
+ org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation)
+ {
+ this.simModule.configureSimModule(swerveModuleSimulation);
+ }
}
diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
index fbd8214f..11016f65 100644
--- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
+++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
@@ -1,8 +1,9 @@
package swervelib.encoders;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.RobotController;
-import swervelib.telemetry.Alert;
/**
* Swerve Absolute Encoder for Thrifty Encoders and other analog encoders.
@@ -39,11 +40,11 @@ public AnalogAbsoluteEncoderSwerve(AnalogInput encoder)
cannotSetOffset = new Alert(
"Encoders",
"Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(),
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
inaccurateVelocities = new Alert(
"Encoders",
"The Analog Absolute encoder may not report accurate velocities!",
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
}
/**
diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java
index 24ca2013..0477b240 100644
--- a/src/main/java/swervelib/encoders/CANCoderSwerve.java
+++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java
@@ -1,15 +1,20 @@
package swervelib.encoders;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Rotations;
+
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.CANcoderConfigurator;
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
import com.ctre.phoenix6.signals.MagnetHealthValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
-import swervelib.telemetry.Alert;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
/**
* Swerve Absolute Encoder for CTRE CANCoders.
@@ -65,21 +70,21 @@ public CANCoderSwerve(int id, String canbus)
magnetFieldLessThanIdeal = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.",
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
readingFaulty = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " reading was faulty.",
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
readingIgnored = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.",
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
cannotSetOffset = new Alert(
"Encoders",
"Failure to set CANCoder "
+ encoder.getDeviceID()
+ " Absolute Encoder Offset",
- Alert.AlertType.WARNING);
+ AlertType.kWarning);
}
/**
@@ -112,7 +117,7 @@ public void configure(boolean inverted)
MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs();
cfg.refresh(magnetSensorConfiguration);
cfg.apply(magnetSensorConfiguration
- .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+ .withAbsoluteSensorDiscontinuityPoint(Rotations.of(1))
.withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive));
}
@@ -139,7 +144,7 @@ public double getAbsolutePosition()
readingFaulty.set(false);
}
- StatusSignal angle = encoder.getAbsolutePosition();
+ StatusSignal angle = encoder.getAbsolutePosition();
// Taken from democat's library.
// Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
@@ -160,7 +165,7 @@ public double getAbsolutePosition()
readingIgnored.set(false);
}
- return angle.getValue() * 360;
+ return angle.getValue().in(Degrees);
}
/**
@@ -213,6 +218,6 @@ public boolean setAbsoluteEncoderOffset(double offset)
@Override
public double getVelocity()
{
- return encoder.getVelocity().getValue() * 360;
+ return encoder.getVelocity().getValue().in(DegreesPerSecond);
}
}
diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java
index 1b8bb6b8..52270951 100644
--- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java
+++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java
@@ -1,7 +1,8 @@
package swervelib.encoders;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import swervelib.telemetry.Alert;
/**
* DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
@@ -42,7 +43,7 @@ public PWMDutyCycleEncoderSwerve(int pin)
inaccurateVelocities = new Alert(
"Encoders",
"The PWM Duty Cycle encoder may not report accurate velocities!",
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
}
diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
index bbf9fb6c..aa88efcf 100644
--- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
+++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
@@ -1,12 +1,15 @@
package swervelib.encoders;
-import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
-import com.revrobotics.SparkAnalogSensor;
-import com.revrobotics.SparkAnalogSensor.Mode;
+import com.revrobotics.spark.SparkAnalogSensor;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.function.Supplier;
+import swervelib.motors.SparkMaxBrushedMotorSwerve;
+import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
-import swervelib.telemetry.Alert;
/**
* SparkMax absolute encoder, attached through the data port analog pin.
@@ -14,33 +17,38 @@
public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
{
+ /**
+ * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object.
+ */
+ private final SwerveMotor sparkMax;
/**
* The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
*/
- public SparkAnalogSensor encoder;
+ public SparkAnalogSensor encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
- private Alert failureConfiguring;
+ private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder does not support integrated offsets.
*/
- private Alert doesNotSupportIntegratedOffsets;
-
+ private Alert doesNotSupportIntegratedOffsets;
/**
- * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data
- * port analog pin.
+ * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data port
+ * analog pin.
*
* @param motor Motor to create the encoder from.
* @param maxVoltage Maximum voltage for analog input reading.
*/
public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage)
{
- if (motor.getMotor() instanceof CANSparkMax)
+ if (motor.getMotor() instanceof SparkMax)
{
- encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute);
- encoder.setPositionConversionFactor(360 / maxVoltage);
+ sparkMax = motor;
+ encoder = ((SparkMax) motor.getMotor()).getAnalog();
+ motor.setAbsoluteEncoder(this);
+ sparkMax.configureIntegratedEncoder(360 / maxVoltage);
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
@@ -48,11 +56,11 @@ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage)
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
doesNotSupportIntegratedOffsets = new Alert(
"Encoders",
"SparkMax Analog Sensors do not support integrated offsets",
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
}
@@ -99,7 +107,17 @@ public void clearStickyFaults()
@Override
public void configure(boolean inverted)
{
- encoder.setInverted(inverted);
+ if (sparkMax instanceof SparkMaxSwerve)
+ {
+ SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
+ cfg.analogSensor.inverted(true);
+ ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
+ } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+ {
+ SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
+ cfg.analogSensor.inverted(true);
+ ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
+ }
}
/**
diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java
index 6bd68dad..b01a39f1 100644
--- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java
+++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java
@@ -1,12 +1,16 @@
package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
-import com.revrobotics.SparkAbsoluteEncoder.Type;
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.function.Supplier;
+import swervelib.motors.SparkMaxBrushedMotorSwerve;
+import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
-import swervelib.telemetry.Alert;
/**
* SparkMax absolute encoder, attached through the data port.
@@ -17,18 +21,23 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
*/
- public AbsoluteEncoder encoder;
+ public SparkAbsoluteEncoder encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
- private Alert failureConfiguring;
+ private Alert failureConfiguring;
/**
* An {@link Alert} for if there is a failure configuring the encoder offset.
*/
- private Alert offsetFailure;
+ private Alert offsetFailure;
+ /**
+ * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance.
+ */
+ private SwerveMotor sparkMax;
/**
- * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor.
+ * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax}
+ * motor.
*
* @param motor Motor to create the encoder from.
* @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
@@ -38,16 +47,17 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor)
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
offsetFailure = new Alert(
"Encoders",
"Failure to set Absolute Encoder Offset",
- Alert.AlertType.WARNING_TRACE);
- if (motor.getMotor() instanceof CANSparkMax)
+ AlertType.kWarning);
+ if (motor.getMotor() instanceof SparkMax)
{
- encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle);
- configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor));
- configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor));
+ sparkMax = motor;
+ encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder();
+ motor.setAbsoluteEncoder(this);
+ motor.configureIntegratedEncoder(conversionFactor);
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
@@ -97,7 +107,17 @@ public void clearStickyFaults()
@Override
public void configure(boolean inverted)
{
- encoder.setInverted(inverted);
+ if (sparkMax instanceof SparkMaxSwerve)
+ {
+ SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
+ cfg.analogSensor.inverted(true);
+ ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
+ } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+ {
+ SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
+ cfg.analogSensor.inverted(true);
+ ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
+ }
}
/**
@@ -131,17 +151,19 @@ public Object getAbsoluteEncoder()
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
- REVLibError error = null;
- for (int i = 0; i < maximumRetries; i++)
+ if (sparkMax instanceof SparkMaxSwerve)
{
- error = encoder.setZeroOffset(offset);
- if (error == REVLibError.kOk)
- {
- return true;
- }
+ SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
+ cfg.absoluteEncoder.zeroOffset(offset);
+ ((SparkMaxSwerve) sparkMax).updateConfig(cfg);
+ return true;
+ } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
+ {
+ SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
+ cfg.absoluteEncoder.zeroOffset(offset);
+ ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
+ return true;
}
- offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error);
- offsetFailure.set(true);
return false;
}
diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java
index 2d6454ef..dd2797d6 100644
--- a/src/main/java/swervelib/imu/NavXSwerve.java
+++ b/src/main/java/swervelib/imu/NavXSwerve.java
@@ -1,14 +1,12 @@
package swervelib.imu;
-import com.kauailabs.navx.frc.AHRS;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.SPI;
-import edu.wpi.first.wpilibj.SerialPort;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.Optional;
-import swervelib.telemetry.Alert;
/**
* Communicates with the NavX({@link AHRS}) as the IMU.
@@ -38,9 +36,9 @@ public class NavXSwerve extends SwerveIMU
*
* @param port Serial Port to connect to.
*/
- public NavXSwerve(SerialPort.Port port)
+ public NavXSwerve(NavXComType port)
{
- navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE);
+ navXError = new Alert("IMU", "Error instantiating NavX.", AlertType.kError);
try
{
/* Communicate w/navX-MXP via the MXP SPI Bus. */
@@ -48,7 +46,6 @@ public NavXSwerve(SerialPort.Port port)
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
imu = new AHRS(port);
factoryDefault();
- SmartDashboard.putData(imu);
} catch (RuntimeException ex)
{
navXError.setText("Error instantiating NavX: " + ex.getMessage());
@@ -56,49 +53,6 @@ public NavXSwerve(SerialPort.Port port)
}
}
- /**
- * Constructor for the NavX({@link AHRS}) swerve.
- *
- * @param port SPI Port to connect to.
- */
- public NavXSwerve(SPI.Port port)
- {
- try
- {
- /* Communicate w/navX-MXP via the MXP SPI Bus. */
- /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
- /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
- imu = new AHRS(port);
- factoryDefault();
- SmartDashboard.putData(imu);
- } catch (RuntimeException ex)
- {
- navXError.setText("Error instantiating NavX: " + ex.getMessage());
- navXError.set(true);
- }
- }
-
- /**
- * Constructor for the NavX({@link AHRS}) swerve.
- *
- * @param port I2C Port to connect to.
- */
- public NavXSwerve(I2C.Port port)
- {
- try
- {
- /* Communicate w/navX-MXP via the MXP SPI Bus. */
- /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
- /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
- imu = new AHRS(port);
- factoryDefault();
- SmartDashboard.putData(imu);
- } catch (RuntimeException ex)
- {
- navXError.setText("Error instantiating NavX: " + ex.getMessage());
- navXError.set(true);
- }
- }
/**
* Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be
@@ -137,6 +91,7 @@ public void setOffset(Rotation3d offset)
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
+ setOffset(getRawRotation3d());
}
/**
diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java
index 574da147..fc0a113c 100644
--- a/src/main/java/swervelib/imu/Pigeon2Swerve.java
+++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java
@@ -1,13 +1,17 @@
package swervelib.imu;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.measure.LinearAcceleration;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
+import java.util.function.Supplier;
/**
* SwerveIMU interface for the {@link Pigeon2}
@@ -36,6 +40,19 @@ public class Pigeon2Swerve extends SwerveIMU
*/
private Pigeon2Configurator cfg;
+ /**
+ * X Acceleration supplier
+ */
+ private Supplier> xAcc;
+ /**
+ * Y Accelleration supplier.
+ */
+ private Supplier> yAcc;
+ /**
+ * Z Acceleration supplier.
+ */
+ private Supplier> zAcc;
+
/**
* Generate the SwerveIMU for {@link Pigeon2}.
*
@@ -46,6 +63,9 @@ public Pigeon2Swerve(int canid, String canbus)
{
imu = new Pigeon2(canid, canbus);
this.cfg = imu.getConfigurator();
+ xAcc = imu::getAccelerationX;
+ yAcc = imu::getAccelerationY;
+ zAcc = imu::getAccelerationZ;
SmartDashboard.putData(imu);
}
@@ -123,6 +143,7 @@ public Rotation3d getRotation3d()
return getRawRotation3d().minus(offset);
}
+
/**
* Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
* empty.
@@ -132,24 +153,19 @@ public Rotation3d getRotation3d()
@Override
public Optional getAccel()
{
- // TODO: Switch to suppliers.
- StatusSignal xAcc = imu.getAccelerationX();
- StatusSignal yAcc = imu.getAccelerationY();
- StatusSignal zAcc = imu.getAccelerationZ();
-
- return Optional.of(new Translation3d(xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
- yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
- zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0));
+ // TODO: Implement later.
+
+ return Optional.empty();
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
- * @return {@link Double} of the rotation rate as an {@link Optional}.
+ * @return Rotation rate in DegreesPerSecond.
*/
public double getRate()
{
- return imu.getRate();
+ return imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(DegreesPerSecond);
}
/**
diff --git a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java
index 09509b75..9ac08efb 100644
--- a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java
+++ b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java
@@ -15,7 +15,7 @@ public class IMULinearMovingAverageFilter
/**
* Gain on each reading.
*/
- private final double m_inputGain;
+ private final double m_inputGain;
/**
* Construct a linear moving average fitler
diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java
index 400de964..7b58f04a 100644
--- a/src/main/java/swervelib/motors/SparkFlexSwerve.java
+++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java
@@ -1,58 +1,81 @@
package swervelib.motors;
+import static edu.wpi.first.units.Units.Seconds;
+
import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.CANSparkBase.ControlType;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkFlex;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkAnalogSensor;
-import com.revrobotics.SparkPIDController;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
-import swervelib.telemetry.Alert;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
- * An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}.
+ * An implementation of {@link SparkFlex} as a {@link SwerveMotor}.
*/
public class SparkFlexSwerve extends SwerveMotor
{
/**
- * {@link CANSparkFlex} Instance.
+ * {@link SparkFlex} Instance.
*/
- private final CANSparkFlex motor;
+ private final SparkFlex motor;
/**
* Integrated encoder.
*/
- public RelativeEncoder encoder;
+ public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
- public SwerveAbsoluteEncoder absoluteEncoder;
+ public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Closed-loop PID controller.
*/
- public SparkPIDController pid;
+ public SparkClosedLoopController pid;
+ /**
+ * Supplier for the velocity of the motor controller.
+ */
+ private Supplier velocity;
+ /**
+ * Supplier for the position of the motor controller.
+ */
+ private Supplier position;
/**
* Factory default already occurred.
*/
- private boolean factoryDefaultOccurred = false;
+ private boolean factoryDefaultOccurred = false;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
- private Alert failureConfiguring;
+ private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client.
*/
- private Alert absoluteEncoderOffsetWarning;
+ private Alert absoluteEncoderOffsetWarning;
+ /**
+ * Configuration object for {@link SparkFlex} motor.
+ */
+ private SparkFlexConfig cfg = new SparkFlexConfig();
+ /**
+ * Tracker for changes that need to be pushed.
+ */
+ private boolean cfgUpdated = false;
+
/**
* Initialize the swerve motor.
@@ -60,7 +83,7 @@ public class SparkFlexSwerve extends SwerveMotor
* @param motor The SwerveMotor as a SparkFlex object.
* @param isDriveMotor Is the motor being initialized a drive motor?
*/
- public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor)
+ public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
@@ -68,32 +91,33 @@ public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor)
clearStickyFaults();
encoder = motor.getEncoder();
- pid = motor.getPIDController();
- pid.setFeedbackDevice(
- encoder); // Configure feedback of the PID controller as the integrated encoder.
+ pid = motor.getClosedLoopController();
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
+ cfgUpdated = true;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
failureConfiguring = new Alert("Motors",
"Failure configuring motor " +
motor.getDeviceId(),
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
absoluteEncoderOffsetWarning = new Alert("Motors",
"IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " +
"absoluteEncoderOffset in the Swerve Module JSON!",
- Alert.AlertType.WARNING);
-
+ AlertType.kWarning);
+ velocity = encoder::getVelocity;
+ position = encoder::getPosition;
}
/**
- * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
+ * Initialize the {@link SwerveMotor} as a {@link SparkFlex} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
*/
public SparkFlexSwerve(int id, boolean isDriveMotor)
{
- this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor);
+ this(new SparkFlex(id, MotorType.kBrushless), isDriveMotor);
}
/**
@@ -109,11 +133,33 @@ private void configureSparkFlex(Supplier config)
{
return;
}
- Timer.delay(0.01);
+ Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
failureConfiguring.set(true);
}
+ /**
+ * Get the current configuration of the {@link SparkFlex}
+ *
+ * @return {@link SparkMaxConfig}
+ */
+ public SparkFlexConfig getConfig()
+ {
+ return cfg;
+ }
+
+ /**
+ * Update the config for the {@link SparkFlex}
+ *
+ * @param cfgGiven Given {@link SparkFlexConfig} which should have minimal modifications.
+ */
+ public void updateConfig(SparkFlexConfig cfgGiven)
+ {
+ cfg.apply(cfgGiven);
+ configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
+ cfgUpdated = false;
+ }
+
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -122,7 +168,8 @@ private void configureSparkFlex(Supplier config)
@Override
public void setVoltageCompensation(double nominalVoltage)
{
- configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage));
+ cfg.voltageCompensation(nominalVoltage);
+ cfgUpdated = true;
}
/**
@@ -134,7 +181,9 @@ public void setVoltageCompensation(double nominalVoltage)
@Override
public void setCurrentLimit(int currentLimit)
{
- configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit));
+
+ cfg.smartCurrentLimit(currentLimit);
+ cfgUpdated = true;
}
/**
@@ -145,8 +194,9 @@ public void setCurrentLimit(int currentLimit)
@Override
public void setLoopRampRate(double rampRate)
{
- configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate));
- configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate));
+ cfg.closedLoopRampRate(rampRate)
+ .openLoopRampRate(rampRate);
+ cfgUpdated = true;
}
/**
@@ -160,6 +210,17 @@ public Object getMotor()
return motor;
}
+ /**
+ * Get the {@link DCMotor} of the motor class.
+ *
+ * @return {@link DCMotor} of this type.
+ */
+ @Override
+ public DCMotor getSimMotor()
+ {
+ return DCMotor.getNeoVortex(1);
+ }
+
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -177,11 +238,7 @@ public boolean isAttachedAbsoluteEncoder()
@Override
public void factoryDefaults()
{
- if (!factoryDefaultOccurred)
- {
- configureSparkFlex(motor::restoreFactoryDefaults);
- factoryDefaultOccurred = true;
- }
+ // Do nothing
}
/**
@@ -205,12 +262,20 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
if (encoder == null)
{
absoluteEncoder = null;
- configureSparkFlex(() -> pid.setFeedbackDevice(this.encoder));
- } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+ cfgUpdated = true;
+
+ velocity = this.encoder::getVelocity;
+ position = this.encoder::getPosition;
+ } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
+ cfgUpdated = true;
absoluteEncoderOffsetWarning.set(true);
absoluteEncoder = encoder;
- configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
+
+ velocity = absoluteEncoder::getVelocity;
+ position = absoluteEncoder::getAbsolutePosition;
}
return this;
}
@@ -223,39 +288,70 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
+ cfg.signals
+ .absoluteEncoderPositionAlwaysOn(false)
+ .absoluteEncoderVelocityAlwaysOn(false)
+ .analogPositionAlwaysOn(false)
+ .analogVelocityAlwaysOn(false)
+ .analogVoltageAlwaysOn(false)
+ .externalOrAltEncoderPositionAlwaysOn(false)
+ .externalOrAltEncoderVelocityAlwaysOn(false)
+ .primaryEncoderPositionAlwaysOn(false)
+ .primaryEncoderVelocityAlwaysOn(false)
+ .iAccumulationAlwaysOn(false)
+ .appliedOutputPeriodMs(10)
+ .faultsPeriodMs(20)
+ ;
if (absoluteEncoder == null)
{
- configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor));
- configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+
+ cfg.encoder
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
+ // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+ // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
+ // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
+ // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+ // This value was taken from:
+ // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
+ // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
+ cfg.encoder
+ .quadratureMeasurementPeriod(10)
+ .quadratureAverageDepth(2);
// Taken from
- // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
- configureCANStatusFrames(10, 20, 20, 500, 500);
+ // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
+ // Unused frames can be set to 65535 to decrease CAN ultilization.
+ cfg.signals
+ .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
+ .primaryEncoderPositionAlwaysOn(true)
+ .primaryEncoderPositionPeriodMs(20);
+
} else
{
- configureSparkFlex(() -> {
- if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
- {
- return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
- positionConversionFactor);
- } else
- {
- return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
- positionConversionFactor);
- }
- });
- configureSparkFlex(() -> {
- if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
- {
- return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
- positionConversionFactor / 60);
- } else
- {
- return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
- positionConversionFactor / 60);
- }
- });
+ // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
+ // This needs to be set to 20ms or under to properly update the swerve module position for odometry
+ // Configuration taken from 3005, the team who helped develop the Max Swerve:
+ // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
+ // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
+ // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
+ // with limited testing 19ms did not return the same value while the module was constatntly rotating.
+ if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
+
+ cfg.signals
+ .absoluteEncoderPositionAlwaysOn(true)
+ .absoluteEncoderPositionPeriodMs(20);
+
+ cfg.absoluteEncoder
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
+ }
}
+ cfgUpdated = true;
+
}
/**
@@ -266,15 +362,10 @@ public void configureIntegratedEncoder(double positionConversionFactor)
@Override
public void configurePIDF(PIDFConfig config)
{
-// int pidSlot =
-// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- int pidSlot = 0;
- configureSparkFlex(() -> pid.setP(config.p, pidSlot));
- configureSparkFlex(() -> pid.setI(config.i, pidSlot));
- configureSparkFlex(() -> pid.setD(config.d, pidSlot));
- configureSparkFlex(() -> pid.setFF(config.f, pidSlot));
- configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot));
- configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
+ cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
+ .iZone(config.iz)
+ .outputRange(config.output.min, config.output.max);
+ cfgUpdated = true;
}
/**
@@ -286,30 +377,11 @@ public void configurePIDF(PIDFConfig config)
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true));
- configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput));
- configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
- }
+ cfg.closedLoop
+ .positionWrappingEnabled(true)
+ .positionWrappingInputRange(minInput, maxInput);
+ cfgUpdated = true;
- /**
- * Set the CAN status frames.
- *
- * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
- * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
- * @param CANStatus2 Motor Position
- * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
- * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
- */
- public void configureCANStatusFrames(
- int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
- {
- configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
- configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
- configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
- configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
- configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
- // TODO: Configure Status Frame 5 and 6 if necessary
- // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -320,7 +392,8 @@ public void configureCANStatusFrames(
@Override
public void setMotorBrake(boolean isBrakeMode)
{
- configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
+ cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ cfgUpdated = true;
}
/**
@@ -331,10 +404,8 @@ public void setMotorBrake(boolean isBrakeMode)
@Override
public void setInverted(boolean inverted)
{
- configureSparkFlex(() -> {
- motor.setInverted(inverted);
- return motor.getLastError();
- });
+ cfg.inverted(inverted);
+ cfgUpdated = true;
}
/**
@@ -349,7 +420,8 @@ public void burnFlash()
} catch (Exception e)
{
}
- configureSparkFlex(() -> motor.burnFlash());
+ motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+ cfgUpdated = false;
}
/**
@@ -372,11 +444,14 @@ public void set(double percentOutput)
@Override
public void setReference(double setpoint, double feedforward)
{
- boolean possibleBurnOutIssue = true;
-// int pidSlot =
-// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
+ if (cfgUpdated)
+ {
+ burnFlash();
+ Timer.delay(0.1); // Give 100ms to apply changes
+ }
+
if (isDriveMotor)
{
configureSparkFlex(() ->
@@ -454,7 +529,7 @@ public double getAppliedOutput()
@Override
public double getVelocity()
{
- return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+ return velocity.get();
}
/**
@@ -465,7 +540,7 @@ public double getVelocity()
@Override
public double getPosition()
{
- return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition();
+ return position.get();
}
/**
@@ -482,22 +557,4 @@ public void setPosition(double position)
}
}
- /**
- * REV Slots for PID configuration.
- */
- enum SparkMAX_slotIdx
- {
- /**
- * Slot 1, used for position PID's.
- */
- Position,
- /**
- * Slot 2, used for velocity PID's.
- */
- Velocity,
- /**
- * Slot 3, used arbitrarily. (Documentation show simulations).
- */
- Simulation
- }
}
diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java
index f384352f..1ce69a82 100644
--- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -1,24 +1,34 @@
package swervelib.motors;
+import static edu.wpi.first.units.Units.Seconds;
+
import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.CANSparkBase.ControlType;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
-import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkMaxAlternateEncoder;
-import com.revrobotics.SparkPIDController;
-import com.revrobotics.SparkRelativeEncoder.Type;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
+import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
+import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
-import swervelib.telemetry.Alert;
+import swervelib.telemetry.SwerveDriveTelemetry;
/**
- * Brushed motor control with {@link CANSparkMax}.
+ * Brushed motor control with {@link SparkMax}.
*/
public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
@@ -26,59 +36,73 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
/**
* SparkMAX Instance.
*/
- private final CANSparkMax motor;
-
+ private final SparkMax motor;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
- public AbsoluteEncoder absoluteEncoder;
+ public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Integrated encoder.
*/
- public RelativeEncoder encoder;
+ public RelativeEncoder encoder;
/**
* Closed-loop PID controller.
*/
- public SparkPIDController pid;
+ public SparkClosedLoopController pid;
+ /**
+ * Supplier for the velocity of the motor controller.
+ */
+ private Supplier velocity;
+ /**
+ * Supplier for the position of the motor controller.
+ */
+ private Supplier position;
/**
* Factory default already occurred.
*/
- private boolean factoryDefaultOccurred = false;
+ private boolean factoryDefaultOccurred = false;
/**
* An {@link Alert} for if the motor has no encoder.
*/
- private Alert noEncoderAlert;
+ private Alert noEncoderAlert;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
- private Alert failureConfiguringAlert;
+ private Alert failureConfiguringAlert;
/**
* An {@link Alert} for if the motor has no encoder defined.
*/
- private Alert noEncoderDefinedAlert;
+ private Alert noEncoderDefinedAlert;
+ /**
+ * Configuration object for {@link SparkMax} motor.
+ */
+ private SparkMaxConfig cfg = new SparkMaxConfig();
+ /**
+ * Tracker for changes that need to be pushed.
+ */
+ private boolean cfgUpdated = false;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a SparkMax object.
* @param isDriveMotor Is the motor being initialized a drive motor?
- * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device.
+ * @param encoderType {@link Type} of encoder to use for the {@link SparkMax} device.
* @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution.
* @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder.
*/
- public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
+ public SparkMaxBrushedMotorSwerve(SparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
boolean useDataPortQuadEncoder)
{
-
noEncoderAlert = new Alert("Motors",
"Cannot use motor without encoder.",
- Alert.AlertType.ERROR_TRACE);
+ AlertType.kError);
failureConfiguringAlert = new Alert("Motors",
"Failure configuring motor " + motor.getDeviceId(),
- Alert.AlertType.WARNING_TRACE);
+ AlertType.kWarning);
noEncoderDefinedAlert = new Alert("Motors",
"An encoder MUST be defined to work with a SparkMAX",
- Alert.AlertType.ERROR_TRACE);
+ AlertType.kError);
// Drive motors **MUST** have an encoder attached.
if (isDriveMotor && encoderType == Type.kNoSensor)
@@ -100,36 +124,49 @@ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type
clearStickyFaults();
// Get the onboard PID controller.
- pid = motor.getPIDController();
+ pid = motor.getClosedLoopController();
// If there is a sensor attached to the data port or encoder port set the relative encoder.
if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder))
{
- this.encoder = useDataPortQuadEncoder ?
- motor.getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) :
- motor.getEncoder(encoderType, countsPerRevolution);
- // Configure feedback of the PID controller as the integrated encoder.
- configureSparkMax(() -> pid.setFeedbackDevice(encoder));
+ if (useDataPortQuadEncoder)
+ {
+ this.encoder = motor.getAlternateEncoder();
+ cfg.alternateEncoder.countsPerRevolution(countsPerRevolution);
+
+ // Configure feedback of the PID controller as the integrated encoder.
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder);
+ } else
+ {
+ this.encoder = motor.getEncoder();
+ cfg.encoder.countsPerRevolution(countsPerRevolution);
+
+ // Configure feedback of the PID controller as the integrated encoder.
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+ }
+ cfgUpdated = true;
}
+ velocity = encoder::getVelocity;
+ position = encoder::getPosition;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback.
}
/**
- * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
+ * Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
- * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device.
+ * @param encoderType {@link Type} of encoder to use for the {@link SparkMax} device.
* @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution.
* @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder.
*/
public SparkMaxBrushedMotorSwerve(int id, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
boolean useDataPortQuadEncoder)
{
- this(new CANSparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution,
+ this(new SparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution,
useDataPortQuadEncoder);
}
@@ -146,11 +183,33 @@ private void configureSparkMax(Supplier config)
{
return;
}
- Timer.delay(0.01);
+ Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
failureConfiguringAlert.set(true);
}
+ /**
+ * Get the current configuration of the {@link SparkMax}
+ *
+ * @return {@link SparkMaxConfig}
+ */
+ public SparkMaxConfig getConfig()
+ {
+ return cfg;
+ }
+
+ /**
+ * Update the config for the {@link SparkMax}
+ *
+ * @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications.
+ */
+ public void updateConfig(SparkMaxConfig cfgGiven)
+ {
+ cfg.apply(cfgGiven);
+ configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
+ cfgUpdated = false;
+ }
+
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -159,7 +218,8 @@ private void configureSparkMax(Supplier config)
@Override
public void setVoltageCompensation(double nominalVoltage)
{
- configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
+ cfg.voltageCompensation(nominalVoltage);
+ cfgUpdated = true;
}
/**
@@ -171,7 +231,8 @@ public void setVoltageCompensation(double nominalVoltage)
@Override
public void setCurrentLimit(int currentLimit)
{
- configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
+ cfg.smartCurrentLimit(currentLimit);
+ cfgUpdated = true;
}
/**
@@ -182,8 +243,9 @@ public void setCurrentLimit(int currentLimit)
@Override
public void setLoopRampRate(double rampRate)
{
- configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
- configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
+ cfg.closedLoopRampRate(rampRate)
+ .openLoopRampRate(rampRate);
+ cfgUpdated = true;
}
/**
@@ -197,6 +259,17 @@ public Object getMotor()
return motor;
}
+ /**
+ * Get the {@link DCMotor} of the motor class.
+ *
+ * @return {@link DCMotor} of this type.
+ */
+ @Override
+ public DCMotor getSimMotor()
+ {
+ return DCMotor.getCIM(1);
+ }
+
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -214,11 +287,7 @@ public boolean isAttachedAbsoluteEncoder()
@Override
public void factoryDefaults()
{
- if (!factoryDefaultOccurred)
- {
- configureSparkMax(motor::restoreFactoryDefaults);
- factoryDefaultOccurred = true;
- }
+ // Do nothing
}
/**
@@ -242,11 +311,25 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
if (encoder == null)
{
absoluteEncoder = null;
- configureSparkMax(() -> pid.setFeedbackDevice(this.encoder));
- } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+ cfgUpdated = true;
+
+ velocity = this.encoder::getVelocity;
+ position = this.encoder::getPosition;
+ burnFlash();
+ } else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve)
{
- absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
- configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder));
+ cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
+ ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
+ cfgUpdated = true;
+
+ DriverStation.reportWarning(
+ "IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
+ " absoluteEncoderOffset in the Swerve Module JSON!",
+ false);
+ absoluteEncoder = encoder;
+ velocity = absoluteEncoder::getVelocity;
+ position = absoluteEncoder::getAbsolutePosition;
}
if (absoluteEncoder == null && this.encoder == null)
{
@@ -264,19 +347,79 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
+ cfg.signals
+ .absoluteEncoderPositionAlwaysOn(false)
+ .absoluteEncoderVelocityAlwaysOn(false)
+ .analogPositionAlwaysOn(false)
+ .analogVelocityAlwaysOn(false)
+ .analogVoltageAlwaysOn(false)
+ .externalOrAltEncoderPositionAlwaysOn(false)
+ .externalOrAltEncoderVelocityAlwaysOn(false)
+ .primaryEncoderPositionAlwaysOn(false)
+ .primaryEncoderVelocityAlwaysOn(false)
+ .iAccumulationAlwaysOn(false)
+ .appliedOutputPeriodMs(10)
+ .faultsPeriodMs(20);
if (absoluteEncoder == null)
{
- configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
- configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+ cfg.encoder
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
+ // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
+ // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
+ // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
+ // https://www.chiefdelphi.com/t/shooter-encoder/400211/11
+ // This value was taken from:
+ // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
+ // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
+ cfg.encoder
+ .quadratureMeasurementPeriod(10)
+ .quadratureAverageDepth(2);
// Taken from
- // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
- configureCANStatusFrames(10, 20, 20, 500, 500);
+ // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
+ // Unused frames can be set to 65535 to decrease CAN ultilization.
+ cfg.signals
+ .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
+ .primaryEncoderPositionAlwaysOn(true)
+ .primaryEncoderPositionPeriodMs(20);
} else
{
- configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor));
- configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60));
+ // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
+ // This needs to be set to 20ms or under to properly update the swerve module position for odometry
+ // Configuration taken from 3005, the team who helped develop the Max Swerve:
+ // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
+ // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
+ // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
+ // with limited testing 19ms did not return the same value while the module was constatntly rotating.
+ if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
+
+ cfg.signals
+ .absoluteEncoderPositionAlwaysOn(true)
+ .absoluteEncoderPositionPeriodMs(20);
+
+ cfg.absoluteEncoder
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
+ } else
+ {
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
+
+ cfg.signals
+ .analogVoltageAlwaysOn(true)
+ .analogPositionAlwaysOn(true)
+ .analogVoltagePeriodMs(20)
+ .analogPositionPeriodMs(20);
+
+ cfg.analogSensor
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
+ }
}
+ cfgUpdated = true;
}
/**
@@ -287,16 +430,10 @@ public void configureIntegratedEncoder(double positionConversionFactor)
@Override
public void configurePIDF(PIDFConfig config)
{
-// int pidSlot =
-// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- int pidSlot = 0;
-
- configureSparkMax(() -> pid.setP(config.p, pidSlot));
- configureSparkMax(() -> pid.setI(config.i, pidSlot));
- configureSparkMax(() -> pid.setD(config.d, pidSlot));
- configureSparkMax(() -> pid.setFF(config.f, pidSlot));
- configureSparkMax(() -> pid.setIZone(config.iz, pidSlot));
- configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
+ cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
+ .iZone(config.iz)
+ .outputRange(config.output.min, config.output.max);
+ cfgUpdated = true;
}
/**
@@ -308,30 +445,11 @@ public void configurePIDF(PIDFConfig config)
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
- configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
- configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
- }
+ cfg.closedLoop
+ .positionWrappingEnabled(true)
+ .positionWrappingInputRange(minInput, maxInput);
+ cfgUpdated = true;
- /**
- * Set the CAN status frames.
- *
- * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
- * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
- * @param CANStatus2 Motor Position
- * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
- * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
- */
- public void configureCANStatusFrames(
- int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
- {
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
- // TODO: Configure Status Frame 5 and 6 if necessary
- // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -342,7 +460,8 @@ public void configureCANStatusFrames(
@Override
public void setMotorBrake(boolean isBrakeMode)
{
- configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
+ cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ cfgUpdated = true;
}
/**
@@ -353,10 +472,8 @@ public void setMotorBrake(boolean isBrakeMode)
@Override
public void setInverted(boolean inverted)
{
- configureSparkMax(() -> {
- motor.setInverted(inverted);
- return motor.getLastError();
- });
+ cfg.inverted(inverted);
+ cfgUpdated = true;
}
/**
@@ -365,7 +482,14 @@ public void setInverted(boolean inverted)
@Override
public void burnFlash()
{
- configureSparkMax(() -> motor.burnFlash());
+ try
+ {
+ Thread.sleep(200);
+ } catch (Exception e)
+ {
+ }
+ motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+ cfgUpdated = false;
}
/**
@@ -388,16 +512,35 @@ public void set(double percentOutput)
@Override
public void setReference(double setpoint, double feedforward)
{
-// int pidSlot =
-// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
- configureSparkMax(() ->
- pid.setReference(
- setpoint,
- isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
- pidSlot,
- feedforward)
- );
+
+ if (cfgUpdated)
+ {
+ burnFlash();
+ Timer.delay(0.1); // Give 100ms to apply changes
+ }
+
+ if (isDriveMotor)
+ {
+ configureSparkMax(() ->
+ pid.setReference(
+ setpoint,
+ ControlType.kVelocity,
+ pidSlot,
+ feedforward));
+ } else
+ {
+ configureSparkMax(() ->
+ pid.setReference(
+ setpoint,
+ ControlType.kPosition,
+ pidSlot,
+ feedforward));
+ if (SwerveDriveTelemetry.isSimulation)
+ {
+ encoder.setPosition(setpoint);
+ }
+ }
}
/**
@@ -454,7 +597,7 @@ public double getAppliedOutput()
@Override
public double getVelocity()
{
- return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+ return velocity.get();
}
/**
@@ -465,7 +608,7 @@ public double getVelocity()
@Override
public double getPosition()
{
- return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition();
+ return position.get();
}
/**
@@ -481,4 +624,20 @@ public void setPosition(double position)
configureSparkMax(() -> encoder.setPosition(position));
}
}
+
+ /**
+ * Type for encoder for {@link SparkMax}
+ */
+ public enum Type
+ {
+ kNoSensor,
+ /**
+ * NO sensor
+ */
+ kHallSensor,
+ /**
+ * Hall sensor attached to dataport
+ */
+ kQuadrature, /** Quad encoder attached to alt */
+ }
}
diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java
index 14795716..68067a6c 100644
--- a/src/main/java/swervelib/motors/SparkMaxSwerve.java
+++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java
@@ -1,57 +1,73 @@
package swervelib.motors;
+import static edu.wpi.first.units.Units.Seconds;
+
import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.CANSparkBase.ControlType;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkAnalogSensor;
-import com.revrobotics.SparkPIDController;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
+import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
+import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
- * An implementation of {@link CANSparkMax} as a {@link SwerveMotor}.
+ * An implementation of {@link com.revrobotics.spark.SparkMax} as a {@link SwerveMotor}.
*/
public class SparkMaxSwerve extends SwerveMotor
{
/**
- * {@link CANSparkMax} Instance.
+ * {@link SparkMax} Instance.
*/
- private final CANSparkMax motor;
+ private final SparkMax motor;
/**
* Integrated encoder.
*/
- public RelativeEncoder encoder;
+ public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
- public SwerveAbsoluteEncoder absoluteEncoder;
+ public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Closed-loop PID controller.
*/
- public SparkPIDController pid;
+ public SparkClosedLoopController pid;
/**
* Factory default already occurred.
*/
- private boolean factoryDefaultOccurred = false;
+ private boolean factoryDefaultOccurred = false;
/**
* Supplier for the velocity of the motor controller.
*/
- private Supplier velocity;
+ private Supplier velocity;
/**
* Supplier for the position of the motor controller.
*/
- private Supplier position;
+ private Supplier position;
+ /**
+ * Configuration object for {@link SparkMax} motor.
+ */
+ private SparkMaxConfig cfg = new SparkMaxConfig();
+ /**
+ * Tracker for changes that need to be pushed.
+ */
+ private boolean cfgUpdated = false;
+
/**
* Initialize the swerve motor.
@@ -59,7 +75,7 @@ public class SparkMaxSwerve extends SwerveMotor
* @param motor The SwerveMotor as a SparkMax object.
* @param isDriveMotor Is the motor being initialized a drive motor?
*/
- public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor)
+ public SparkMaxSwerve(SparkMax motor, boolean isDriveMotor)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
@@ -67,25 +83,26 @@ public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor)
clearStickyFaults();
encoder = motor.getEncoder();
- pid = motor.getPIDController();
- pid.setFeedbackDevice(
- encoder); // Configure feedback of the PID controller as the integrated encoder.
+ pid = motor.getClosedLoopController();
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
+ cfgUpdated = true;
velocity = encoder::getVelocity;
position = encoder::getPosition;
+
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
}
/**
- * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
+ * Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param isDriveMotor Is the motor being initialized a drive motor?
*/
public SparkMaxSwerve(int id, boolean isDriveMotor)
{
- this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor);
+ this(new SparkMax(id, MotorType.kBrushless), isDriveMotor);
}
/**
@@ -101,11 +118,33 @@ private void configureSparkMax(Supplier config)
{
return;
}
- Timer.delay(0.01);
+ Timer.delay(Units.Milliseconds.of(10).in(Seconds));
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
+ /**
+ * Get the current configuration of the {@link SparkMax}
+ *
+ * @return {@link SparkMaxConfig}
+ */
+ public SparkMaxConfig getConfig()
+ {
+ return cfg;
+ }
+
+ /**
+ * Update the config for the {@link SparkMax}
+ *
+ * @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications.
+ */
+ public void updateConfig(SparkMaxConfig cfgGiven)
+ {
+ cfg.apply(cfgGiven);
+ configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
+ cfgUpdated = false;
+ }
+
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -114,7 +153,8 @@ private void configureSparkMax(Supplier config)
@Override
public void setVoltageCompensation(double nominalVoltage)
{
- configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
+ cfg.voltageCompensation(nominalVoltage);
+ cfgUpdated = true;
}
/**
@@ -126,7 +166,9 @@ public void setVoltageCompensation(double nominalVoltage)
@Override
public void setCurrentLimit(int currentLimit)
{
- configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
+ cfg.smartCurrentLimit(currentLimit);
+ cfgUpdated = true;
+
}
/**
@@ -137,8 +179,10 @@ public void setCurrentLimit(int currentLimit)
@Override
public void setLoopRampRate(double rampRate)
{
- configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
- configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
+ cfg.closedLoopRampRate(rampRate)
+ .openLoopRampRate(rampRate);
+ cfgUpdated = true;
+
}
/**
@@ -152,6 +196,17 @@ public Object getMotor()
return motor;
}
+ /**
+ * Get the {@link DCMotor} of the motor class.
+ *
+ * @return {@link DCMotor} of this type.
+ */
+ @Override
+ public DCMotor getSimMotor()
+ {
+ return DCMotor.getNEO(1);
+ }
+
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
@@ -169,11 +224,7 @@ public boolean isAttachedAbsoluteEncoder()
@Override
public void factoryDefaults()
{
- if (!factoryDefaultOccurred)
- {
- configureSparkMax(motor::restoreFactoryDefaults);
- factoryDefaultOccurred = true;
- }
+ // Do nothing
}
/**
@@ -197,17 +248,23 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
if (encoder == null)
{
absoluteEncoder = null;
- configureSparkMax(() -> pid.setFeedbackDevice(this.encoder));
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+ cfgUpdated = true;
+
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
- } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor)
+ burnFlash();
+ } else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve)
{
+ cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
+ ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
+ cfgUpdated = true;
+
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = encoder;
- configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
}
@@ -222,10 +279,26 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
+ cfg.signals
+ .absoluteEncoderPositionAlwaysOn(false)
+ .absoluteEncoderVelocityAlwaysOn(false)
+ .analogPositionAlwaysOn(false)
+ .analogVelocityAlwaysOn(false)
+ .analogVoltageAlwaysOn(false)
+ .externalOrAltEncoderPositionAlwaysOn(false)
+ .externalOrAltEncoderVelocityAlwaysOn(false)
+ .primaryEncoderPositionAlwaysOn(false)
+ .primaryEncoderVelocityAlwaysOn(false)
+ .iAccumulationAlwaysOn(false)
+ .appliedOutputPeriodMs(10)
+ .faultsPeriodMs(20);
+
if (absoluteEncoder == null)
{
- configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
- configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+ cfg.encoder
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
@@ -233,54 +306,55 @@ public void configureIntegratedEncoder(double positionConversionFactor)
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
- configureSparkMax(() -> encoder.setMeasurementPeriod(10));
- configureSparkMax(() -> encoder.setAverageDepth(2));
+ cfg.encoder
+ .quadratureMeasurementPeriod(10)
+ .quadratureAverageDepth(2);
// Taken from
- // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+ // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
- configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
+ cfg.signals
+ .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
+ .primaryEncoderPositionAlwaysOn(true)
+ .primaryEncoderPositionPeriodMs(20);
+
} else
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
- // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
+ // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
- configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50);
- }
- // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use
- else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
+
+ cfg.signals
+ .absoluteEncoderPositionAlwaysOn(true)
+ .absoluteEncoderPositionPeriodMs(20);
+
+ cfg.absoluteEncoder
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
+ } else
{
- configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
+ cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
+
+ cfg.signals
+ .analogVoltageAlwaysOn(true)
+ .analogPositionAlwaysOn(true)
+ .analogVoltagePeriodMs(20)
+ .analogPositionPeriodMs(20);
+
+ cfg.analogSensor
+ .positionConversionFactor(positionConversionFactor)
+ .velocityConversionFactor(positionConversionFactor / 60);
}
- configureSparkMax(() -> {
- if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
- {
- return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
- positionConversionFactor);
- } else
- {
- return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
- positionConversionFactor);
- }
- });
- configureSparkMax(() -> {
- if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
- {
- return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
- positionConversionFactor / 60);
- } else
- {
- return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
- positionConversionFactor / 60);
- }
- });
}
+ cfgUpdated = true;
+
}
/**
@@ -291,15 +365,11 @@ else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor)
@Override
public void configurePIDF(PIDFConfig config)
{
-// int pidSlot =
-// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- int pidSlot = 0;
- configureSparkMax(() -> pid.setP(config.p));
- configureSparkMax(() -> pid.setI(config.i));
- configureSparkMax(() -> pid.setD(config.d));
- configureSparkMax(() -> pid.setFF(config.f));
- configureSparkMax(() -> pid.setIZone(config.iz));
- configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max));
+ cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
+ .iZone(config.iz)
+ .outputRange(config.output.min, config.output.max);
+ cfgUpdated = true;
+
}
/**
@@ -311,33 +381,11 @@ public void configurePIDF(PIDFConfig config)
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
- configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
- configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
- }
+ cfg.closedLoop
+ .positionWrappingEnabled(true)
+ .positionWrappingInputRange(minInput, maxInput);
+ cfgUpdated = true;
- /**
- * Set the CAN status frames.
- *
- * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
- * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
- * @param CANStatus2 Motor Position
- * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
- * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
- * @param CANStatus5 Duty Cycle Absolute Encoder Position, Duty Cycle Absolute Encoder Absolute Angle
- * @param CANStatus6 Duty Cycle Absolute Encoder Velocity, Duty Cycle Absolute Encoder Frequency
- */
- public void configureCANStatusFrames(
- int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus5, int CANStatus6)
- {
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, CANStatus5));
- configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, CANStatus6));
- // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
/**
@@ -348,7 +396,9 @@ public void configureCANStatusFrames(
@Override
public void setMotorBrake(boolean isBrakeMode)
{
- configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
+ cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ cfgUpdated = true;
+
}
/**
@@ -359,10 +409,8 @@ public void setMotorBrake(boolean isBrakeMode)
@Override
public void setInverted(boolean inverted)
{
- configureSparkMax(() -> {
- motor.setInverted(inverted);
- return motor.getLastError();
- });
+ cfg.inverted(inverted);
+ cfgUpdated = true;
}
/**
@@ -377,7 +425,8 @@ public void burnFlash()
} catch (Exception e)
{
}
- configureSparkMax(() -> motor.burnFlash());
+ motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
+ cfgUpdated = false;
}
/**
@@ -400,11 +449,14 @@ public void set(double percentOutput)
@Override
public void setReference(double setpoint, double feedforward)
{
- boolean possibleBurnOutIssue = true;
-// int pidSlot =
-// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot = 0;
+ if (cfgUpdated)
+ {
+ burnFlash();
+ Timer.delay(0.1); // Give 100ms to apply changes
+ }
+
if (isDriveMotor)
{
configureSparkMax(() ->
@@ -509,23 +561,4 @@ public void setPosition(double position)
configureSparkMax(() -> encoder.setPosition(position));
}
}
-
- /**
- * REV Slots for PID configuration.
- */
- enum SparkMAX_slotIdx
- {
- /**
- * Slot 1, used for position PID's.
- */
- Position,
- /**
- * Slot 2, used for velocity PID's.
- */
- Velocity,
- /**
- * Slot 3, used arbitrarily. (Documentation show simulations).
- */
- Simulation
- }
}
diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java
index 22b104d4..bae6d2f4 100644
--- a/src/main/java/swervelib/motors/SwerveMotor.java
+++ b/src/main/java/swervelib/motors/SwerveMotor.java
@@ -1,5 +1,6 @@
package swervelib.motors;
+import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
@@ -172,6 +173,13 @@ public abstract class SwerveMotor
*/
public abstract Object getMotor();
+ /**
+ * Get the {@link DCMotor} of the motor class.
+ *
+ * @return {@link DCMotor} of this type.
+ */
+ public abstract DCMotor getSimMotor();
+
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java
index 233a868f..635df8ad 100644
--- a/src/main/java/swervelib/motors/TalonFXSwerve.java
+++ b/src/main/java/swervelib/motors/TalonFXSwerve.java
@@ -1,12 +1,18 @@
package swervelib.motors;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.Volts;
+
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.SwerveDriveTelemetry;
@@ -20,39 +26,39 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Wait time for status frames to show up.
*/
- public static double STATUS_TIMEOUT_SECONDS = 0.02;
+ public static double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* Factory default already occurred.
*/
- private final boolean factoryDefaultOccurred = false;
+ private final boolean factoryDefaultOccurred = false;
/**
* Whether the absolute encoder is integrated.
*/
- private final boolean absoluteEncoder = false;
+ private final boolean absoluteEncoder = false;
/**
* Motion magic angle voltage setter.
*/
- private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
+ private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
/**
* Velocity voltage setter for controlling drive motor.
*/
- private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
+ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
/**
* TalonFX motor controller.
*/
- private final TalonFX motor;
+ private final TalonFX motor;
/**
* Conversion factor for the motor.
*/
- private double conversionFactor;
+ private double conversionFactor;
/**
* Current TalonFX configuration.
*/
- private TalonFXConfiguration configuration = new TalonFXConfiguration();
+ private TalonFXConfiguration configuration = new TalonFXConfiguration();
/**
* Current TalonFX Configurator.
*/
- private TalonFXConfigurator cfg;
+ private TalonFXConfigurator cfg;
/**
@@ -181,63 +187,7 @@ public void configureIntegratedEncoder(double positionConversionFactor)
cfg.apply(configuration);
// Taken from democat's library.
// https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16
- configureCANStatusFrames(250);
- }
-
- /**
- * Set the CAN status frames.
- *
- * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information
- */
- public void configureCANStatusFrames(int CANStatus1)
- {
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
- }
-
- /**
- * Set the CAN status frames.
- *
- * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information
- * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current
- * Measurement, Sticky Fault Information
- * @param CANStatus3 Quadrature Information
- * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature
- * @param CANStatus8 Pulse Width Information
- * @param CANStatus10 Motion Profiling/Motion Magic Information
- * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1)
- * @param CANStatus13 PID0 (Primary PID) Information
- * @param CANStatus14 PID1 (Auxiliary PID) Information
- * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX)
- * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement
- */
- public void configureCANStatusFrames(
- int CANStatus1,
- int CANStatus2,
- int CANStatus3,
- int CANStatus4,
- int CANStatus8,
- int CANStatus10,
- int CANStatus12,
- int CANStatus13,
- int CANStatus14,
- int CANStatus21,
- int CANStatusCurrent)
- {
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21);
- // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current,
- // CANStatusCurrent);
-
- // TODO: Configure Status Frame 2 thru 21 if necessary
- // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods
+ // configureCANStatusFrames(250);
}
/**
@@ -290,7 +240,10 @@ public void setMotorBrake(boolean isBrakeMode)
public void setInverted(boolean inverted)
{
// Timer.delay(1);
- motor.setInverted(inverted);
+ cfg.refresh(configuration.MotorOutput);
+ configuration.MotorOutput.withInverted(
+ inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive);
+ cfg.apply(configuration.MotorOutput);
}
/**
@@ -357,7 +310,7 @@ public void setReference(double setpoint, double feedforward, double position)
@Override
public double getVoltage()
{
- return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
+ return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts);
}
/**
@@ -390,7 +343,7 @@ public double getAppliedOutput()
@Override
public double getVelocity()
{
- return motor.getVelocity().getValue();
+ return motor.getVelocity().getValue().magnitude();
}
/**
@@ -401,7 +354,7 @@ public double getVelocity()
@Override
public double getPosition()
{
- return motor.getPosition().getValue();
+ return motor.getPosition().getValue().magnitude();
}
/**
@@ -414,8 +367,7 @@ public void setPosition(double position)
{
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
- position = position < 0 ? (position % 360) + 360 : position;
- cfg.setPosition(position / 360);
+ cfg.setPosition(Degrees.of(position).in(Rotations));
}
}
@@ -441,8 +393,8 @@ public void setCurrentLimit(int currentLimit)
{
cfg.refresh(configuration.CurrentLimits);
cfg.apply(
- configuration.CurrentLimits.withStatorCurrentLimit(currentLimit)
- .withStatorCurrentLimitEnable(true));
+ configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit)
+ .withSupplyCurrentLimitEnable(true));
}
/**
@@ -468,6 +420,17 @@ public Object getMotor()
return motor;
}
+ /**
+ * Get the {@link DCMotor} of the motor class.
+ *
+ * @return {@link DCMotor} of this type.
+ */
+ @Override
+ public DCMotor getSimMotor()
+ {
+ return DCMotor.getKrakenX60(1);
+ }
+
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java
index 98cf6e7c..28d2e356 100644
--- a/src/main/java/swervelib/motors/TalonSRXSwerve.java
+++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java
@@ -7,6 +7,7 @@
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
@@ -21,31 +22,31 @@ public class TalonSRXSwerve extends SwerveMotor
/**
* Factory default already occurred.
*/
- private final boolean factoryDefaultOccurred = false;
+ private final boolean factoryDefaultOccurred = false;
/**
* Current TalonFX configuration.
*/
- private final TalonSRXConfiguration configuration = new TalonSRXConfiguration();
+ private final TalonSRXConfiguration configuration = new TalonSRXConfiguration();
/**
* Whether the absolute encoder is integrated.
*/
- private final boolean absoluteEncoder = false;
+ private final boolean absoluteEncoder = false;
/**
* TalonSRX motor controller.
*/
- private final WPI_TalonSRX motor;
+ private final WPI_TalonSRX motor;
/**
* The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees.
*/
- private double positionConversionFactor = 1;
+ private double positionConversionFactor = 1;
/**
* If the TalonFX configuration has changed.
*/
- private boolean configChanged = true;
+ private boolean configChanged = true;
/**
* Nominal voltage default to use with feedforward.
*/
- private double nominalVoltage = 12.0;
+ private double nominalVoltage = 12.0;
/**
* Constructor for TalonSRX swerve motor.
@@ -432,6 +433,17 @@ public Object getMotor()
return motor;
}
+ /**
+ * Get the {@link DCMotor} of the motor class.
+ *
+ * @return {@link DCMotor} of this type.
+ */
+ @Override
+ public DCMotor getSimMotor()
+ {
+ return DCMotor.getCIM(1);
+ }
+
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
diff --git a/src/main/java/swervelib/parser/Cache.java b/src/main/java/swervelib/parser/Cache.java
index 830a08c0..830a2731 100644
--- a/src/main/java/swervelib/parser/Cache.java
+++ b/src/main/java/swervelib/parser/Cache.java
@@ -1,5 +1,6 @@
package swervelib.parser;
+import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
import java.util.function.Supplier;
@@ -95,7 +96,7 @@ public Cache updateValidityPeriod(long validityPeriod)
*/
public T getValue()
{
- if (isStale())
+ if (isStale() || RobotBase.isSimulation())
{
update();
}
diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java
index 347b187d..9a6eebb4 100644
--- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java
+++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java
@@ -2,8 +2,14 @@
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import java.util.function.Supplier;
+import org.ironmaple.simulation.drivesims.GyroSimulation;
import swervelib.SwerveModule;
+import swervelib.imu.NavXSwerve;
+import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
+import swervelib.math.SwerveMath;
/**
* Swerve drive configurations used during SwerveDrive construction.
@@ -96,4 +102,68 @@ public double getDriveBaseRadiusMeters()
//Return Largest Radius
return centerOfModules.getDistance(moduleLocationsMeters[0]);
}
+
+ /**
+ * Get the trackwidth of the swerve modules.
+ *
+ * @return Effective trackwdtih in Meters
+ */
+ public double getTrackwidth()
+ {
+ SwerveModuleConfiguration fr = SwerveMath.getSwerveModule(modules, true, false);
+ SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
+ return fr.moduleLocation.getDistance(fl.moduleLocation);
+ }
+
+ /**
+ * Get the tracklength of the swerve modules.
+ *
+ * @return Effective tracklength in Meters
+ */
+ public double getTracklength()
+ {
+ SwerveModuleConfiguration br = SwerveMath.getSwerveModule(modules, false, false);
+ SwerveModuleConfiguration bl = SwerveMath.getSwerveModule(modules, false, true);
+ return br.moduleLocation.getDistance(bl.moduleLocation);
+ }
+
+ /**
+ * Get the {@link DCMotor} corresponding to the first module's configuration.
+ *
+ * @return {@link DCMotor} of the drive motor.
+ */
+ public DCMotor getDriveMotorSim()
+ {
+ SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
+ return fl.driveMotor.getSimMotor();
+ }
+
+ /**
+ * Get the {@link DCMotor} corresponding to the first module configuration.
+ *
+ * @return {@link DCMotor} of the angle motor.
+ */
+ public DCMotor getAngleMotorSim()
+ {
+ SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true);
+ return fl.angleMotor.getSimMotor();
+ }
+
+ /**
+ * Get the gyro simulation for the robot.
+ *
+ * @return {@link GyroSimulation} gyro simulation.
+ */
+ public Supplier getGyroSim()
+ {
+ if (imu instanceof Pigeon2Swerve)
+ {
+ return GyroSimulation.getPigeon2();
+ } else if (imu instanceof NavXSwerve)
+ {
+ return GyroSimulation.getNav2X();
+ }
+ return GyroSimulation.getGeneric();
+ }
+
}
diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java
index 24d06257..8996f448 100644
--- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java
+++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java
@@ -3,7 +3,7 @@
import edu.wpi.first.math.geometry.Translation2d;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.motors.SwerveMotor;
-import swervelib.parser.json.MotorConfigDouble;
+import swervelib.parser.json.modules.ConversionFactorsJson;
/**
* Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}.
@@ -17,7 +17,7 @@ public class SwerveModuleConfiguration
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the
* conversion factors.
*/
- public final MotorConfigDouble conversionFactors;
+ public final ConversionFactorsJson conversionFactors;
/**
* Angle offset in degrees for the Swerve Module.
*/
@@ -89,7 +89,7 @@ public class SwerveModuleConfiguration
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
SwerveMotor angleMotor,
- MotorConfigDouble conversionFactors,
+ ConversionFactorsJson conversionFactors,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
double xMeters,
@@ -139,7 +139,7 @@ public SwerveModuleConfiguration(
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
SwerveMotor angleMotor,
- MotorConfigDouble conversionFactors,
+ ConversionFactorsJson conversionFactors,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
double xMeters,
diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java
index 99360ce1..a626bbb8 100644
--- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java
+++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java
@@ -1,6 +1,6 @@
package swervelib.parser;
-import swervelib.parser.json.MotorConfigDouble;
+import swervelib.parser.json.modules.ConversionFactorsJson;
/**
* Configuration class which stores physical characteristics shared between every swerve module.
@@ -16,20 +16,32 @@ public class SwerveModulePhysicalCharacteristics
* The time it takes for the motor to go from 0 to full throttle in seconds.
*/
public final double driveMotorRampRate, angleMotorRampRate;
+ /**
+ * The minimum voltage to spin the module or wheel.
+ */
+ public final double driveFrictionVoltage, angleFrictionVoltage;
/**
* Wheel grip tape coefficient of friction on carpet, as described by the vendor.
*/
- public final double wheelGripCoefficientOfFriction;
+ public final double wheelGripCoefficientOfFriction;
+ /**
+ * Steer rotational inertia in (KilogramSquareMeters) kg/m_sq.
+ */
+ public final double steerRotationalInertia;
+ /**
+ * Robot mass in Kilograms.
+ */
+ public final double robotMassKg;
/**
* The voltage to use for the smart motor voltage compensation.
*/
- public double optimalVoltage;
+ public double optimalVoltage;
/**
* The conversion factors for the drive and angle motors, created by
* {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}.
*/
- public MotorConfigDouble conversionFactor;
+ public ConversionFactorsJson conversionFactor;
/**
* Construct the swerve module physical characteristics.
@@ -47,15 +59,23 @@ public class SwerveModulePhysicalCharacteristics
* over drawing power from battery)
* @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents
* overdrawing power and power loss).
+ * @param angleFrictionVoltage Angle motor minimum voltage.
+ * @param driveFrictionVoltage Drive motor minimum voltage.
+ * @param steerRotationalInertia Steering rotational inertia in KilogramSquareMeters.
+ * @param robotMassKg Robot mass in kG.
*/
public SwerveModulePhysicalCharacteristics(
- MotorConfigDouble conversionFactors,
+ ConversionFactorsJson conversionFactors,
double wheelGripCoefficientOfFriction,
double optimalVoltage,
int driveMotorCurrentLimit,
int angleMotorCurrentLimit,
double driveMotorRampRate,
- double angleMotorRampRate)
+ double angleMotorRampRate,
+ double driveFrictionVoltage,
+ double angleFrictionVoltage,
+ double steerRotationalInertia,
+ double robotMassKg)
{
this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
this.optimalVoltage = optimalVoltage;
@@ -64,7 +84,7 @@ public SwerveModulePhysicalCharacteristics(
// Set the conversion factors to null if they are both 0.
if (conversionFactors != null)
{
- if (conversionFactors.angle == 0 && conversionFactors.drive == 0)
+ if (conversionFactors.isAngleEmpty() && conversionFactors.isDriveEmpty())
{
this.conversionFactor = null;
}
@@ -74,6 +94,10 @@ public SwerveModulePhysicalCharacteristics(
this.angleMotorCurrentLimit = angleMotorCurrentLimit;
this.driveMotorRampRate = driveMotorRampRate;
this.angleMotorRampRate = angleMotorRampRate;
+ this.driveFrictionVoltage = driveFrictionVoltage;
+ this.angleFrictionVoltage = angleFrictionVoltage;
+ this.steerRotationalInertia = steerRotationalInertia;
+ this.robotMassKg = robotMassKg;
}
/**
@@ -90,7 +114,7 @@ public SwerveModulePhysicalCharacteristics(
* power and power loss).
*/
public SwerveModulePhysicalCharacteristics(
- MotorConfigDouble conversionFactors,
+ ConversionFactorsJson conversionFactors,
double driveMotorRampRate,
double angleMotorRampRate)
{
@@ -101,6 +125,10 @@ public SwerveModulePhysicalCharacteristics(
40,
20,
driveMotorRampRate,
- angleMotorRampRate);
+ angleMotorRampRate,
+ 0.2,
+ 0.3,
+ 0.03,
+ 50);
}
}
diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java
index 573fdd87..aa87c8e8 100644
--- a/src/main/java/swervelib/parser/SwerveParser.java
+++ b/src/main/java/swervelib/parser/SwerveParser.java
@@ -3,6 +3,7 @@
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
import java.io.File;
import java.io.IOException;
import java.util.HashMap;
@@ -138,7 +139,24 @@ public SwerveDrive createSwerveDrive(double maxSpeed)
return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage,
maxSpeed,
physicalPropertiesJson.wheelGripCoefficientOfFriction),
- maxSpeed);
+ maxSpeed, Pose2d.kZero);
+ }
+
+ /**
+ * Create {@link SwerveDrive} from JSON configuration directory.
+ *
+ * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in
+ * {@link swervelib.SwerveController} and drive feedforward in
+ * {@link SwerveMath#createDriveFeedforward(double, double, double)}.
+ * @param initialPose {@link Pose2d} starting point on the field
+ * @return {@link SwerveDrive} instance.
+ */
+ public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose)
+ {
+ return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage,
+ maxSpeed,
+ physicalPropertiesJson.wheelGripCoefficientOfFriction),
+ maxSpeed, initialPose);
}
/**
@@ -157,12 +175,12 @@ public SwerveDrive createSwerveDrive(double maxSpeed)
*/
public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion)
{
- physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor;
- physicalPropertiesJson.conversionFactor.drive = driveMotorConversion;
+ physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor;
+ physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion;
return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage,
maxSpeed,
physicalPropertiesJson.wheelGripCoefficientOfFriction),
- maxSpeed);
+ maxSpeed, Pose2d.kZero);
}
/**
@@ -184,9 +202,9 @@ public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversio
public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed,
double angleMotorConversionFactor, double driveMotorConversion)
{
- physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor;
- physicalPropertiesJson.conversionFactor.drive = driveMotorConversion;
- return createSwerveDrive(driveFeedforward, maxSpeed);
+ physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor;
+ physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion;
+ return createSwerveDrive(driveFeedforward, maxSpeed, Pose2d.kZero);
}
/**
@@ -196,9 +214,10 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do
* {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}.
* @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in
* {@link swervelib.SwerveController} of the robot
+ * @param initialPose {@link Pose2d} initial pose.
* @return {@link SwerveDrive} instance.
*/
- public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed)
+ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed, Pose2d initialPose)
{
SwerveModuleConfiguration[] moduleConfigurations =
new SwerveModuleConfiguration[moduleJsons.length];
@@ -222,6 +241,8 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do
return new SwerveDrive(
swerveDriveConfiguration,
- controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed);
+ controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed),
+ maxSpeed,
+ initialPose);
}
}
diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java
index 7ce6fc3d..ee03ccd1 100644
--- a/src/main/java/swervelib/parser/json/DeviceJson.java
+++ b/src/main/java/swervelib/parser/json/DeviceJson.java
@@ -4,11 +4,8 @@
import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning;
import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
-import com.revrobotics.SparkRelativeEncoder.Type;
+import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.SPI;
-import edu.wpi.first.wpilibj.SerialPort.Port;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
import swervelib.encoders.CANCoderSwerve;
import swervelib.encoders.CanAndMagSwerve;
@@ -27,6 +24,7 @@
import swervelib.imu.SwerveIMU;
import swervelib.motors.SparkFlexSwerve;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
+import swervelib.motors.SparkMaxBrushedMotorSwerve.Type;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonFXSwerve;
@@ -122,7 +120,7 @@ public SwerveIMU createIMU()
return new CanandgyroSwerve(id);
case "navx":
case "navx_spi":
- return new NavXSwerve(SPI.Port.kMXP);
+ return new NavXSwerve(NavXComType.kMXP_SPI);
case "navx_i2c":
DriverStation.reportWarning(
"WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " +
@@ -130,15 +128,17 @@ public SwerveIMU createIMU()
".html#onboard-i2c-causing-system-lockups",
false);
i2cLockupWarning.set(true);
- return new NavXSwerve(I2C.Port.kMXP);
+ return new NavXSwerve(NavXComType.kI2C);
case "navx_usb":
DriverStation.reportWarning("WARNING: There is issues when using USB camera's and the NavX like this!\n" +
"https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", false);
serialCommsIssueWarning.set(true);
- return new NavXSwerve(Port.kUSB);
+ return new NavXSwerve(NavXComType.kUSB1);
case "navx_mxp_serial":
serialCommsIssueWarning.set(true);
- return new NavXSwerve(Port.kMXP);
+ throw new RuntimeException("Studica NavX API does not support MXP Serial communication currently.");
+
+// return new NavXSwerve(Port.kMXP);
case "pigeon":
return new PigeonSwerve(id);
case "pigeon2":
diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java
index 2fa83c89..f26b2e84 100644
--- a/src/main/java/swervelib/parser/json/ModuleJson.java
+++ b/src/main/java/swervelib/parser/json/ModuleJson.java
@@ -1,8 +1,8 @@
package swervelib.parser.json;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.MotorFeedbackSensor;
+import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.util.Units;
+import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.motors.SwerveMotor;
import swervelib.parser.PIDFConfig;
@@ -26,14 +26,6 @@ public class ModuleJson
* Angle motor device configuration.
*/
public DeviceJson angle;
- /**
- * Conversion factor for the module, if different from the one in swervedrive.json
- *
- * Conversion factor applied to the motor controllers PID loops. Can be calculated with
- * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or
- * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors.
- */
- public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0);
/**
* Conversion Factors composition. Auto-calculates the conversion factors.
*/
@@ -81,37 +73,8 @@ public SwerveModuleConfiguration createModuleConfiguration(
SwerveMotor angleMotor = angle.createMotor(false);
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
- // Setup deprecation notice.
-// if (this.conversionFactor.drive != 0 && this.conversionFactor.angle != 0)
-// {
-// new Alert("Configuration",
-// "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
-// "} \nis deprecated, please use\n" +
-// "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
-// conversionFactor.angle + "} }",
-// AlertType.WARNING).set(true);
-// }
-
- // Override with composite conversion factor.
- if (!conversionFactors.isAngleEmpty())
- {
- conversionFactor.angle = conversionFactors.angle.calculate();
- }
- if (!conversionFactors.isDriveEmpty())
- {
- conversionFactor.drive = conversionFactors.drive.calculate();
- }
-
// Set the conversion factors to null if they are both 0.
- if (this.conversionFactor != null)
- {
- if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0)
- {
- this.conversionFactor = null;
- }
- }
-
- if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null)
+ if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null)
{
throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" +
"SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" +
@@ -120,27 +83,27 @@ public SwerveModuleConfiguration createModuleConfiguration(
"OR\n" +
"Set the conversion factor in physicalproperties.json OR the module JSON file." +
"REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n");
- } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null)
+ } else if (physicalCharacteristics.conversionFactor.works() && !conversionFactors.works())
{
- this.conversionFactor = physicalCharacteristics.conversionFactor;
- } else if (physicalCharacteristics.conversionFactor !=
- null) // If both are defined, override 0 with the physical characterstics input.
+ conversionFactors = physicalCharacteristics.conversionFactor;
+ } else if (physicalCharacteristics.conversionFactor.works())
+ // If both are defined, override 0 with the physical characterstics input.
{
- this.conversionFactor.angle = this.conversionFactor.angle == 0 ? physicalCharacteristics.conversionFactor.angle
- : this.conversionFactor.angle;
- this.conversionFactor.drive = this.conversionFactor.drive == 0 ? physicalCharacteristics.conversionFactor.drive
- : this.conversionFactor.drive;
+ conversionFactors.angle = conversionFactors.isAngleEmpty() ? physicalCharacteristics.conversionFactor.angle
+ : conversionFactors.angle;
+ conversionFactors.drive = conversionFactors.isDriveEmpty() ? physicalCharacteristics.conversionFactor.drive
+ : conversionFactors.drive;
}
- if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0)
+ if (conversionFactors.isDriveEmpty() || conversionFactors.isAngleEmpty())
{
throw new RuntimeException(
"Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files.");
}
// Backwards compatibility, auto-optimization.
- if (conversionFactor.angle == 360 && absEncoder != null &&
- absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor && angleMotor.getMotor() instanceof CANSparkMax)
+ if (conversionFactors.angle.factor == 360 && absEncoder != null &&
+ absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax)
{
angleMotor.setAbsoluteEncoder(absEncoder);
}
@@ -148,7 +111,7 @@ public SwerveModuleConfiguration createModuleConfiguration(
return new SwerveModuleConfiguration(
drive.createMotor(true),
angleMotor,
- conversionFactor,
+ conversionFactors,
absEncoder,
absoluteEncoderOffset,
Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x),
diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java
index 60be9d25..eaab95d6 100644
--- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java
+++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java
@@ -1,5 +1,8 @@
package swervelib.parser.json;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
import swervelib.parser.json.modules.ConversionFactorsJson;
@@ -9,13 +12,23 @@
public class PhysicalPropertiesJson
{
-
/**
- * Conversion factor applied to the motor controllers PID loops. Can be calculated with
- * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or
- * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors.
+ * DEPRECATED! Use {@link PhysicalPropertiesJson#conversionFactors} instead.
+ */
+ @Deprecated(since = "2025", forRemoval = true)
+ public MotorConfigDouble conversionFactor = new MotorConfigDouble();
+ /**
+ * Minimum voltage to spin the module or wheel.
*/
- public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0);
+ public MotorConfigDouble friction = new MotorConfigDouble(0.3, 0.2);
+ /**
+ * Steer rotational inertia in KilogramMetersSquare.
+ */
+ public double steerRotationalInertia = 0.03;
+ /**
+ * Robot mass in lb (pounds)
+ */
+ public double robotMass = 110.2311;
/**
* Conversion Factors composition. Auto-calculates the conversion factors.
*/
@@ -45,35 +58,29 @@ public class PhysicalPropertiesJson
public SwerveModulePhysicalCharacteristics createPhysicalProperties()
{
// Setup deprecation notice.
-// if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
-// conversionFactors.isAngleEmpty())
-// {
-// new Alert("Configuration",
-// "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
-// "} \nis deprecated, please use\n" +
-// "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
-// conversionFactor.angle + "} }",
-// AlertType.ERROR_TRACE).set(true);
-// }
-
- if (!conversionFactors.isAngleEmpty())
- {
- conversionFactor.angle = conversionFactors.angle.calculate();
- }
-
- if (!conversionFactors.isDriveEmpty())
+ if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
+ conversionFactors.isAngleEmpty())
{
- conversionFactor.drive = conversionFactors.drive.calculate();
+ new Alert("Configuration",
+ "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
+ "} \nis deprecated, please use\n" +
+ "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
+ conversionFactor.angle + "} }",
+ AlertType.kError).set(true);
}
return new SwerveModulePhysicalCharacteristics(
- conversionFactor,
+ conversionFactors,
wheelGripCoefficientOfFriction,
optimalVoltage,
currentLimit.drive,
currentLimit.angle,
rampRate.drive,
- rampRate.angle);
+ rampRate.angle,
+ friction.drive,
+ friction.angle,
+ steerRotationalInertia,
+ Units.Pounds.of(robotMass).in(Units.Kilogram));
}
}
diff --git a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java
index a7c8613a..5a3e10f9 100644
--- a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java
+++ b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java
@@ -1,8 +1,6 @@
package swervelib.parser.json.modules;
import swervelib.math.SwerveMath;
-import swervelib.telemetry.Alert;
-import swervelib.telemetry.Alert.AlertType;
/**
* Angle motor conversion factors composite JSON parse class.
@@ -13,11 +11,11 @@ public class AngleConversionFactorsJson
/**
* Gear ratio for the angle/steering/azimuth motor on the Swerve Module. Motor rotations to 1 wheel rotation.
*/
- public double gearRatio = 0;
+ public double gearRatio;
/**
* Calculated or given conversion factor.
*/
- public double factor = 0;
+ public double factor = 0;
/**
* Calculate the drive conversion factor.
@@ -26,12 +24,6 @@ public class AngleConversionFactorsJson
*/
public double calculate()
{
- if (factor != 0 && gearRatio != 0)
- {
- new Alert("Configuration",
- "The given angle conversion factor takes precedence over the composite conversion factor, please remove 'factor' if you want to use the composite factor instead.",
- AlertType.WARNING).set(true);
- }
if (factor == 0)
{
factor = SwerveMath.calculateDegreesPerSteeringRotation(gearRatio);
diff --git a/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java
index 85ff25f2..546ad65a 100644
--- a/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java
+++ b/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java
@@ -22,7 +22,8 @@ public class ConversionFactorsJson
*/
public boolean isDriveEmpty()
{
- return drive.factor == 0 && drive.diameter == 0 && drive.gearRatio == 0;
+ drive.calculate();
+ return drive.factor == 0;
}
/**
@@ -32,6 +33,18 @@ public boolean isDriveEmpty()
*/
public boolean isAngleEmpty()
{
- return angle.factor == 0 && angle.gearRatio == 0;
+ angle.calculate();
+ return angle.factor == 0;
+ }
+
+ /**
+ * Check if the conversion factor can be found.
+ *
+ * @return If the conversion factors can be found.
+ */
+ public boolean works()
+ {
+ return (angle.factor != 0 && drive.factor != 0) ||
+ ((drive.gearRatio != 0 && drive.diameter != 0)) && (angle.gearRatio != 0);
}
}
diff --git a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java
index d792d36d..937b78a1 100644
--- a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java
+++ b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java
@@ -2,8 +2,6 @@
import edu.wpi.first.math.util.Units;
import swervelib.math.SwerveMath;
-import swervelib.telemetry.Alert;
-import swervelib.telemetry.Alert.AlertType;
/**
* Drive motor composite JSON parse class.
@@ -14,15 +12,15 @@ public class DriveConversionFactorsJson
/**
* Gear ratio for the drive motor rotations to turn the wheel 1 complete rotation.
*/
- public double gearRatio = 0;
+ public double gearRatio;
/**
* Diameter of the wheel in inches.
*/
- public double diameter = 0;
+ public double diameter;
/**
* Calculated conversion factor.
*/
- public double factor = 0;
+ public double factor = 0;
/**
* Calculate the drive conversion factor.
@@ -31,12 +29,6 @@ public class DriveConversionFactorsJson
*/
public double calculate()
{
- if (factor != 0 && (diameter != 0 || gearRatio != 0))
- {
- new Alert("Configuration",
- "The given drive conversion factor takes precedence over the composite conversion factor, please remove 'factor' if you want to use the composite factor instead.",
- AlertType.WARNING).set(true);
- }
if (factor == 0)
{
factor = SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(this.diameter), this.gearRatio);
diff --git a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java
index b3f2b62d..f9e3b995 100644
--- a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java
+++ b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java
@@ -6,9 +6,9 @@
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import java.util.Optional;
+import org.ironmaple.simulation.drivesims.GyroSimulation;
/**
* Simulation for {@link swervelib.SwerveDrive} IMU.
@@ -16,27 +16,14 @@
public class SwerveIMUSimulation
{
- /**
- * Main timer to control movement estimations.
- */
- private final Timer timer;
- /**
- * The last time the timer was read, used to determine position changes.
- */
- private double lastTime;
- /**
- * Heading of the robot.
- */
- private double angle;
+ private final GyroSimulation gyroSimulation;
/**
* Create the swerve drive IMU simulation.
*/
- public SwerveIMUSimulation()
+ public SwerveIMUSimulation(GyroSimulation gyroSimulation)
{
- timer = new Timer();
- timer.start();
- lastTime = timer.get();
+ this.gyroSimulation = gyroSimulation;
}
/**
@@ -46,7 +33,7 @@ public SwerveIMUSimulation()
*/
public Rotation2d getYaw()
{
- return new Rotation2d(angle);
+ return gyroSimulation.getGyroReading();
}
/**
@@ -76,7 +63,7 @@ public Rotation2d getRoll()
*/
public Rotation3d getGyroRotation3d()
{
- return new Rotation3d(0, 0, angle);
+ return new Rotation3d(0, 0, getYaw().getRadians());
}
/**
@@ -104,9 +91,6 @@ public void updateOdometry(
Pose2d[] modulePoses,
Field2d field)
{
-
- angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);
- lastTime = timer.get();
field.getObject("XModules").setPoses(modulePoses);
}
@@ -117,6 +101,6 @@ public void updateOdometry(
*/
public void setAngle(double angle)
{
- this.angle = angle;
+ this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle));
}
}
diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java
index 2fb06f8f..97b85ba2 100644
--- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java
+++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java
@@ -1,54 +1,35 @@
package swervelib.simulation;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.Radian;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
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.wpilibj.Timer;
+import org.ironmaple.simulation.motorsims.ControlRequest;
/**
- * Class to hold simulation data for {@link swervelib.SwerveModule}
+ * Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
*/
public class SwerveModuleSimulation
{
+ private org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule = null;
/**
- * Main timer to simulate the passage of time.
- */
- private final Timer timer;
- /**
- * Time delta since last update
- */
- private double dt;
- /**
- * Fake motor position.
- */
- private double fakePos;
- /**
- * The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}.
- */
- private double fakeSpeed;
- /**
- * Last time queried.
- */
- private double lastTime;
- /**
- * Current simulated swerve module state.
- */
- private SwerveModuleState state;
-
- /**
- * Create simulation class and initialize module at 0.
+ * Configure the maple sim module
+ *
+ * @param mapleSimModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation
*/
- public SwerveModuleSimulation()
+ public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule)
{
- timer = new Timer();
- timer.start();
- lastTime = timer.get();
- state = new SwerveModuleState(0, Rotation2d.fromDegrees(0));
- fakeSpeed = 0;
- fakePos = 0;
- dt = 0;
+ this.mapleSimModule = mapleSimModule;
+ mapleSimModule.getDriveMotorConfigs()
+ .withDefaultFeedForward(Volts.zero())
+ .withVelocityVoltageController(Volts.per(RPM).ofNative(7.0 / 3000.0), true);
}
/**
@@ -59,13 +40,10 @@ public SwerveModuleSimulation()
*/
public void updateStateAndPosition(SwerveModuleState desiredState)
{
- dt = timer.get() - lastTime;
- lastTime = timer.get();
-
- state = desiredState;
- fakeSpeed = desiredState.speedMetersPerSecond;
- fakePos += (fakeSpeed * dt);
-
+ mapleSimModule.requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure()));
+ mapleSimModule.requestDriveControl(new ControlRequest.VelocityVoltage(
+ RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS.in(Meters))
+ ));
}
/**
@@ -75,8 +53,10 @@ public void updateStateAndPosition(SwerveModuleState desiredState)
*/
public SwerveModulePosition getPosition()
{
-
- return new SwerveModulePosition(fakePos, state.angle);
+ return new SwerveModulePosition(
+ mapleSimModule.getDriveWheelFinalPosition().in(Radian) * mapleSimModule.WHEEL_RADIUS.in(Meters),
+ mapleSimModule.getSteerAbsoluteFacing()
+ );
}
/**
@@ -86,6 +66,12 @@ public SwerveModulePosition getPosition()
*/
public SwerveModuleState getState()
{
+ if (mapleSimModule == null)
+ {
+ return new SwerveModuleState();
+ }
+ SwerveModuleState state = mapleSimModule.getCurrentState();
+ state.angle = state.angle.minus(new Rotation2d());
return state;
}
}
diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java
deleted file mode 100644
index 40f9af78..00000000
--- a/src/main/java/swervelib/telemetry/Alert.java
+++ /dev/null
@@ -1,237 +0,0 @@
-// Copyright (c) 2023 FRC 6328
-// http://github.com/Mechanical-Advantage
-//
-// Use of this source code is governed by an MIT-style
-// license that can be found below.
-
-// MIT License
-//
-// Copyright (c) 2023 FRC 6328
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-package swervelib.telemetry;
-
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-
-/**
- * Class for managing persistent alerts to be sent over NetworkTables.
- */
-public class Alert
-{
-
- /**
- * Group of the alert.
- */
- private static Map groups = new HashMap();
- /**
- * Type of the Alert to raise.
- */
- private final AlertType type;
- /**
- * Activation state of alert.
- */
- private boolean active = false;
- /**
- * When the alert was raised.
- */
- private double activeStartTime = 0.0;
- /**
- * Text of the alert.
- */
- private String text;
-
- /**
- * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate
- * entries will be added to NetworkTables.
- *
- * @param text Text to be displayed when the alert is active.
- * @param type Alert level specifying urgency.
- */
- public Alert(String text, AlertType type)
- {
- this("Alerts", text, type);
- }
-
- /**
- * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate entries will be added to
- * NetworkTables.
- *
- * @param group Group identifier, also used as NetworkTables title
- * @param text Text to be displayed when the alert is active.
- * @param type Alert level specifying urgency.
- */
- public Alert(String group, String text, AlertType type)
- {
- if (!groups.containsKey(group))
- {
- groups.put(group, new SendableAlerts());
- SmartDashboard.putData(group, groups.get(group));
- }
-
- this.text = text;
- this.type = type;
- groups.get(group).alerts.add(this);
- }
-
- /**
- * Sets whether the alert should currently be displayed. When activated, the alert text will also be sent to the
- * console.
- *
- * @param active Set the alert as active and report it to the driver station.
- */
- public void set(boolean active)
- {
- if (active && !this.active)
- {
- activeStartTime = Timer.getFPGATimestamp();
- printAlert(text);
- }
- this.active = active;
- }
-
- /**
- * Updates current alert text.
- *
- * @param text The text for the alert.
- */
- public void setText(String text)
- {
- if (active && !text.equals(this.text))
- {
- printAlert(text);
- }
- this.text = text;
- }
-
-
- /**
- * Print the alert message.
- *
- * @param text Text to print.
- */
- private void printAlert(String text)
- {
- switch (type)
- {
- case ERROR:
- DriverStation.reportError(text, false);
- break;
- case ERROR_TRACE:
- DriverStation.reportError(text, true);
- break;
- case WARNING:
- DriverStation.reportWarning(text, false);
- break;
- case WARNING_TRACE:
- DriverStation.reportWarning(text, true);
- break;
- case INFO:
- System.out.println(text);
- break;
- }
- }
-
- /**
- * Represents an alert's level of urgency.
- */
- public static enum AlertType
- {
- /**
- * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which
- * will seriously affect the robot's functionality and thus require immediate attention.
- */
- ERROR,
- /**
- * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which
- * will seriously affect the robot's functionality and thus require immediate attention. Trace printed to driver
- * station console.
- */
- ERROR_TRACE,
-
- /**
- * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems
- * which could affect the robot's functionality but do not necessarily require immediate attention.
- */
- WARNING,
- /**
- * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems
- * which could affect the robot's functionality but do not necessarily require immediate attention. Trace printed to
- * driver station console.
- */
- WARNING_TRACE,
- /**
- * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type for problems which
- * are unlikely to affect the robot's functionality, or any other alerts which do not fall under "ERROR" or
- * "WARNING".
- */
- INFO
- }
-
- /**
- * Sendable alert for advantage scope.
- */
- private static class SendableAlerts implements Sendable
- {
-
- /**
- * Alert list for sendable.
- */
- public final List alerts = new ArrayList<>();
-
- /**
- * Get alerts based off of type.
- *
- * @param type Type of alert to fetch.
- * @return Active alert strings.
- */
- public String[] getStrings(AlertType type)
- {
- List alertStrings = new ArrayList<>();
- for (Alert alert : alerts)
- {
- if (alert.type == type && alert.active)
- {
- alertStrings.add(alert.text);
- }
- }
- // alertStrings.sort((a1, a2) -> (int) (a2.activeStartTime - a1.activeStartTime));
- return alertStrings.toArray(new String[alertStrings.size()]);
- }
-
- @Override
- public void initSendable(SendableBuilder builder)
- {
- builder.setSmartDashboardType("Alerts");
- builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null);
- builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null);
- builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null);
- builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null);
- builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null);
- }
- }
-}
\ No newline at end of file
diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java
index 3c90fc6f..e4d98380 100644
--- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java
+++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java
@@ -1,8 +1,15 @@
package swervelib.telemetry;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import swervelib.telemetry.Alert.AlertType;
/**
* Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit)
@@ -13,90 +20,186 @@ public class SwerveDriveTelemetry
/**
* An {@link Alert} for if the CAN ID is greater than 40.
*/
- public static final Alert canIdWarning = new Alert("JSON",
- "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
- Alert.AlertType.WARNING);
+ public static final Alert canIdWarning = new Alert("JSON",
+ "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
+ AlertType.kWarning);
/**
* An {@link Alert} for if there is an I2C lockup issue on the roboRIO.
*/
- public static final Alert i2cLockupWarning = new Alert("IMU",
- "I2C lockup issue detected on roboRIO. Check console for more information.",
- Alert.AlertType.WARNING);
+ public static final Alert i2cLockupWarning = new Alert("IMU",
+ "I2C lockup issue detected on roboRIO. Check console for more information.",
+ AlertType.kWarning);
/**
* NavX serial comm issue.
*/
- public static final Alert serialCommsIssueWarning = new Alert("IMU",
- "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
- AlertType.WARNING);
+ public static final Alert serialCommsIssueWarning = new Alert("IMU",
+ "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
+ AlertType.kWarning);
+ /**
+ * Measured swerve module states object.
+ */
+ public static SwerveModuleState[] measuredStatesObj = new SwerveModuleState[4];
+ /**
+ * Desired swerve module states object
+ */
+ public static SwerveModuleState[] desiredStatesObj = new SwerveModuleState[4];
+ /**
+ * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
+ * chassis speeds properties.
+ */
+ public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds();
+ /**
+ * Describes the desired forward, sideways and angular velocity of the robot.
+ */
+ public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
+ /**
+ * The robot's current rotation based on odometry or gyro readings
+ */
+ public static Rotation2d robotRotationObj = new Rotation2d();
/**
* The current telemetry verbosity level.
*/
- public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
+ public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
/**
* State of simulation of the Robot, used to optimize retrieval.
*/
- public static boolean isSimulation = RobotBase.isSimulation();
+ public static boolean isSimulation = RobotBase.isSimulation();
/**
* The number of swerve modules
*/
- public static int moduleCount;
+ public static int moduleCount;
/**
* The Locations of the swerve drive wheels.
*/
- public static double[] wheelLocations;
+ public static double[] wheelLocations;
/**
* An array of rotation and velocity values describing the measured state of each swerve module
*/
- public static double[] measuredStates;
+ public static double[] measuredStates;
/**
* An array of rotation and velocity values describing the desired state of each swerve module
*/
- public static double[] desiredStates;
+ public static double[] desiredStates;
/**
* The robot's current rotation based on odometry or gyro readings
*/
- public static double robotRotation = 0;
+ public static double robotRotation = 0;
/**
* The maximum achievable speed of the modules, used to adjust the size of the vectors.
*/
- public static double maxSpeed;
+ public static double maxSpeed;
/**
* The units of the module rotations and robot rotation
*/
- public static String rotationUnit = "degrees";
+ public static String rotationUnit = "degrees";
/**
* The distance between the left and right modules.
*/
- public static double sizeLeftRight;
+ public static double sizeLeftRight;
/**
* The distance between the front and back modules.
*/
- public static double sizeFrontBack;
+ public static double sizeFrontBack;
/**
* The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to
* align with odometry data or match videos. 'up', 'right', 'down' or 'left'
*/
- public static String forwardDirection = "up";
+ public static String forwardDirection = "up";
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
- public static double maxAngularVelocity;
+ public static double maxAngularVelocity;
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
- public static double[] measuredChassisSpeeds = new double[3];
+ public static double[] measuredChassisSpeeds = new double[3];
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
- public static double[] desiredChassisSpeeds = new double[3];
+ public static double[] desiredChassisSpeeds = new double[3];
+ /**
+ * Struct publisher for AdvantageScope swerve widgets.
+ */
+ private static StructArrayPublisher measuredStatesStruct
+ = NetworkTableInstance.getDefault()
+ .getStructArrayTopic(
+ "swerve/advantagescope/currentStates",
+ SwerveModuleState.struct)
+ .publish();
+ /**
+ * Struct publisher for AdvantageScope swerve widgets.
+ */
+ private static StructArrayPublisher desiredStatesStruct
+ = NetworkTableInstance.getDefault()
+ .getStructArrayTopic(
+ "swerve/advantagescope/desiredStates",
+ SwerveModuleState.struct)
+ .publish();
+ /**
+ * Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
+ */
+ private static StructPublisher measuredChassisSpeedsStruct
+ = NetworkTableInstance.getDefault()
+ .getStructTopic(
+ "swerve/advantagescope/measuredChassisSpeeds",
+ ChassisSpeeds.struct)
+ .publish();
+ /**
+ * Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
+ */
+ private static StructPublisher desiredChassisSpeedsStruct
+ = NetworkTableInstance.getDefault()
+ .getStructTopic(
+ "swerve/advantagescope/measuredChassisSpeeds",
+ ChassisSpeeds.struct)
+ .publish();
+ /**
+ * Robot {@link Rotation2d} for AdvantageScope swerve widgets.
+ */
+ private static StructPublisher robotRotationStruct
+ = NetworkTableInstance.getDefault()
+ .getStructTopic(
+ "swerve/advantagescope/robotRotation",
+ Rotation2d.struct)
+ .publish();
/**
* Upload data to smartdashboard
*/
public static void updateData()
{
+ measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond;
+ measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond;
+ measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond);
+
+ desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond;
+ desiredChassisSpeeds[1] = desiredChassisSpeedsObj.vyMetersPerSecond;
+ desiredChassisSpeeds[2] = Math.toDegrees(desiredChassisSpeedsObj.omegaRadiansPerSecond);
+
+ robotRotation = robotRotationObj.getDegrees();
+
+ for (int i = 0; i < measuredStatesObj.length; i++)
+ {
+ SwerveModuleState state = measuredStatesObj[i];
+ if (state != null)
+ {
+ measuredStates[i * 2] = state.angle.getDegrees();
+ measuredStates[i * 2 + 1] = state.speedMetersPerSecond;
+ }
+ }
+
+ for (int i = 0; i < desiredStatesObj.length; i++)
+ {
+ SwerveModuleState state = desiredStatesObj[i];
+ if (state != null)
+ {
+ desiredStates[i * 2] = state.angle.getDegrees();
+ desiredStates[i * 2 + 1] = state.speedMetersPerSecond;
+ }
+ }
+
SmartDashboard.putNumber("swerve/moduleCount", moduleCount);
SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations);
SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates);
@@ -110,6 +213,12 @@ public static void updateData()
SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity);
SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds);
SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds);
+
+ desiredStatesStruct.set(desiredStatesObj);
+ measuredStatesStruct.set(measuredStatesObj);
+ desiredChassisSpeedsStruct.set(desiredChassisSpeedsObj);
+ measuredChassisSpeedsStruct.set(measuredChassisSpeedsObj);
+ robotRotationStruct.set(robotRotationObj);
}
/**
diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
deleted file mode 100644
index e978a5f7..00000000
--- a/vendordeps/NavX.json
+++ /dev/null
@@ -1,40 +0,0 @@
-{
- "fileName": "NavX.json",
- "name": "NavX",
- "version": "2024.1.0",
- "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
- "frcYear": "2024",
- "mavenUrls": [
- "https://dev.studica.com/maven/release/2024/"
- ],
- "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
- "javaDependencies": [
- {
- "groupId": "com.kauailabs.navx.frc",
- "artifactId": "navx-frc-java",
- "version": "2024.1.0"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": [
- {
- "groupId": "com.kauailabs.navx.frc",
- "artifactId": "navx-frc-cpp",
- "version": "2024.1.0",
- "headerClassifier": "headers",
- "sourcesClassifier": "sources",
- "sharedLibrary": false,
- "libName": "navx_frc",
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "linuxathena",
- "linuxraspbian",
- "linuxarm32",
- "linuxarm64",
- "linuxx86-64",
- "osxuniversal",
- "windowsx86-64"
- ]
- }
- ]
-}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-beta.json
similarity index 81%
rename from vendordeps/PathplannerLib.json
rename to vendordeps/PathplannerLib-beta.json
index 6dc648db..e79fe1ae 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib-beta.json
@@ -1,18 +1,18 @@
{
- "fileName": "PathplannerLib.json",
+ "fileName": "PathplannerLib-beta.json",
"name": "PathplannerLib",
- "version": "2024.2.8",
+ "version": "2025.0.0-beta-5",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
- "frcYear": "2024",
+ "frcYear": "2025",
"mavenUrls": [
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
],
- "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+ "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json",
"javaDependencies": [
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
- "version": "2024.2.8"
+ "version": "2025.0.0-beta-5"
}
],
"jniDependencies": [],
@@ -20,15 +20,15 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
- "version": "2024.2.8",
+ "version": "2025.0.0-beta-5",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
"skipInvalidPlatforms": true,
"binaryPlatforms": [
"windowsx86-64",
- "linuxx86-64",
"osxuniversal",
+ "linuxx86-64",
"linuxathena",
"linuxarm32",
"linuxarm64"
diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5-frc2025-beta-latest.json
similarity index 73%
rename from vendordeps/Phoenix5.json
rename to vendordeps/Phoenix5-frc2025-beta-latest.json
index ff7359e1..65dfcff9 100644
--- a/vendordeps/Phoenix5.json
+++ b/vendordeps/Phoenix5-frc2025-beta-latest.json
@@ -1,43 +1,56 @@
{
- "fileName": "Phoenix5.json",
+ "fileName": "Phoenix5-frc2025-beta-latest.json",
"name": "CTRE-Phoenix (v5)",
- "version": "5.33.1",
- "frcYear": 2024,
+ "version": "5.34.0-beta-3",
+ "frcYear": "2025",
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
"mavenUrls": [
"https://maven.ctr-electronics.com/release/"
],
- "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json",
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json",
"requires": [
{
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.",
- "offlineFileName": "Phoenix6.json",
- "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
+ "offlineFileName": "Phoenix6-frc2025-beta-latest.json",
+ "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json"
+ }
+ ],
+ "conflictsWith": [
+ {
+ "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+ "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.",
+ "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json"
+ },
+ {
+ "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df",
+ "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.",
+ "offlineFileName": "Phoenix5-replay-frc2025-beta-latest.json"
}
],
"javaDependencies": [
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-java",
- "version": "5.33.1"
+ "version": "5.34.0-beta-3"
},
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-java",
- "version": "5.33.1"
+ "version": "5.34.0-beta-3"
}
],
"jniDependencies": [
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
@@ -45,12 +58,13 @@
{
"groupId": "com.ctre.phoenix.sim",
"artifactId": "cci-sim",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -60,7 +74,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "wpiapi-cpp",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"libName": "CTRE_Phoenix_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -68,6 +82,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
@@ -75,7 +90,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "api-cpp",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"libName": "CTRE_Phoenix",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -83,6 +98,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
@@ -90,7 +106,7 @@
{
"groupId": "com.ctre.phoenix",
"artifactId": "cci",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"libName": "CTRE_PhoenixCCI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -98,6 +114,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
@@ -105,7 +122,7 @@
{
"groupId": "com.ctre.phoenix.sim",
"artifactId": "wpiapi-cpp-sim",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"libName": "CTRE_Phoenix_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -113,6 +130,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -120,7 +138,7 @@
{
"groupId": "com.ctre.phoenix.sim",
"artifactId": "api-cpp-sim",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"libName": "CTRE_PhoenixSim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -128,6 +146,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -135,7 +154,7 @@
{
"groupId": "com.ctre.phoenix.sim",
"artifactId": "cci-sim",
- "version": "5.33.1",
+ "version": "5.34.0-beta-3",
"libName": "CTRE_PhoenixCCISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -143,6 +162,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-frc2025-beta-latest.json
similarity index 80%
rename from vendordeps/Phoenix6.json
rename to vendordeps/Phoenix6-frc2025-beta-latest.json
index 5eadb206..ff0b8c9e 100644
--- a/vendordeps/Phoenix6.json
+++ b/vendordeps/Phoenix6-frc2025-beta-latest.json
@@ -1,76 +1,94 @@
{
- "fileName": "Phoenix6.json",
+ "fileName": "Phoenix6-frc2025-beta-latest.json",
"name": "CTRE-Phoenix (v6)",
- "version": "24.3.0",
- "frcYear": 2024,
+ "version": "25.0.0-beta-3",
+ "frcYear": "2025",
"uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
"mavenUrls": [
"https://maven.ctr-electronics.com/release/"
],
- "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json",
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json",
"conflictsWith": [
{
- "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a",
- "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.",
- "offlineFileName": "Phoenix6And5.json"
+ "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+ "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+ "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json"
}
],
"javaDependencies": [
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-java",
- "version": "24.3.0"
+ "version": "25.0.0-beta-3"
}
],
"jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix6",
+ "artifactId": "api-cpp",
+ "version": "25.0.0-beta-3",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxarm64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
- "artifactId": "tools-sim",
- "version": "24.3.0",
+ "artifactId": "api-cpp-sim",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
- "artifactId": "simTalonSRX",
- "version": "24.3.0",
+ "artifactId": "tools-sim",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
},
{
"groupId": "com.ctre.phoenix6.sim",
- "artifactId": "simTalonFX",
- "version": "24.3.0",
+ "artifactId": "simTalonSRX",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -78,12 +96,13 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -91,12 +110,13 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -104,12 +124,13 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -117,12 +138,13 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -130,12 +152,13 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -143,12 +166,13 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -158,7 +182,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "wpiapi-cpp",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_Phoenix6_WPI",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -166,6 +190,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
@@ -173,7 +198,7 @@
{
"groupId": "com.ctre.phoenix6",
"artifactId": "tools",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -181,6 +206,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"linuxathena"
],
"simMode": "hwsim"
@@ -188,7 +214,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "wpiapi-cpp-sim",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_Phoenix6_WPISim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -196,6 +222,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -203,7 +230,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "tools-sim",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -211,6 +238,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -218,7 +246,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simTalonSRX",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -226,21 +254,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenix6.sim",
- "artifactId": "simTalonFX",
- "version": "24.3.0",
- "libName": "CTRE_SimTalonFX",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -248,7 +262,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simVictorSPX",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -256,6 +270,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -263,7 +278,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simPigeonIMU",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -271,6 +286,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -278,7 +294,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simCANCoder",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimCANCoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -286,6 +302,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -293,7 +310,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProTalonFX",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -301,6 +318,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -308,7 +326,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProCANcoder",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -316,6 +334,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
@@ -323,7 +342,7 @@
{
"groupId": "com.ctre.phoenix6.sim",
"artifactId": "simProPigeon2",
- "version": "24.3.0",
+ "version": "25.0.0-beta-3",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -331,6 +350,7 @@
"binaryPlatforms": [
"windowsx86-64",
"linuxx86-64",
+ "linuxarm64",
"osxuniversal"
],
"simMode": "swsim"
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index 731bbbfc..70cdac98 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,25 +1,25 @@
{
"fileName": "REVLib.json",
"name": "REVLib",
- "version": "2024.2.4",
- "frcYear": "2024",
+ "version": "2025.0.0-beta-3",
+ "frcYear": "2025",
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
"mavenUrls": [
"https://maven.revrobotics.com/"
],
- "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json",
"javaDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-java",
- "version": "2024.2.4"
+ "version": "2025.0.0-beta-3"
}
],
"jniDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
- "version": "2024.2.4",
+ "version": "2025.0.0-beta-3",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
@@ -37,7 +37,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-cpp",
- "version": "2024.2.4",
+ "version": "2025.0.0-beta-3",
"libName": "REVLib",
"headerClassifier": "headers",
"sharedLibrary": false,
@@ -55,7 +55,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
- "version": "2024.2.4",
+ "version": "2025.0.0-beta-3",
"libName": "REVLibDriver",
"headerClassifier": "headers",
"sharedLibrary": false,
diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2025.json
similarity index 82%
rename from vendordeps/ReduxLib_2024.json
rename to vendordeps/ReduxLib_2025.json
index f694966d..bd164ce9 100644
--- a/vendordeps/ReduxLib_2024.json
+++ b/vendordeps/ReduxLib_2025.json
@@ -1,25 +1,25 @@
{
- "fileName": "ReduxLib_2024.json",
+ "fileName": "ReduxLib_2025.json",
"name": "ReduxLib",
- "version": "2024.3.1",
- "frcYear": 2024,
+ "version": "2025.0.0-beta0",
+ "frcYear": 2025,
"uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd",
"mavenUrls": [
"https://maven.reduxrobotics.com/"
],
- "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json",
+ "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json",
"javaDependencies": [
{
"groupId": "com.reduxrobotics.frc",
"artifactId": "ReduxLib-java",
- "version": "2024.3.1"
+ "version": "2025.0.0-beta0"
}
],
"jniDependencies": [
{
"groupId": "com.reduxrobotics.frc",
"artifactId": "ReduxLib-driver",
- "version": "2024.3.1",
+ "version": "2025.0.0-beta0",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -36,7 +36,7 @@
{
"groupId": "com.reduxrobotics.frc",
"artifactId": "ReduxLib-cpp",
- "version": "2024.3.1",
+ "version": "2025.0.0-beta0",
"libName": "ReduxLib-cpp",
"headerClassifier": "headers",
"sourcesClassifier": "sources",
diff --git a/vendordeps/Studica-2025.1.1-beta-3.json b/vendordeps/Studica-2025.1.1-beta-3.json
new file mode 100644
index 00000000..2f64aaf4
--- /dev/null
+++ b/vendordeps/Studica-2025.1.1-beta-3.json
@@ -0,0 +1,71 @@
+{
+ "fileName": "Studica-2025.1.1-beta-3.json",
+ "name": "Studica",
+ "version": "2025.1.1-beta-3",
+ "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+ "frcYear": "2025",
+ "mavenUrls": [
+ "https://dev.studica.com/maven/release/2025/"
+ ],
+ "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json",
+ "cppDependencies": [
+ {
+ "artifactId": "Studica-cpp",
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "linuxx86-64",
+ "osxuniversal",
+ "windowsx86-64"
+ ],
+ "groupId": "com.studica.frc",
+ "headerClassifier": "headers",
+ "libName": "Studica",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "version": "2025.1.1-beta-3"
+ },
+ {
+ "artifactId": "Studica-driver",
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "linuxx86-64",
+ "osxuniversal",
+ "windowsx86-64"
+ ],
+ "groupId": "com.studica.frc",
+ "headerClassifier": "headers",
+ "libName": "StudicaDriver",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "version": "2025.1.1-beta-3"
+ }
+ ],
+ "javaDependencies": [
+ {
+ "artifactId": "Studica-java",
+ "groupId": "com.studica.frc",
+ "version": "2025.1.1-beta-3"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "artifactId": "Studica-driver",
+ "groupId": "com.studica.frc",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "linuxx86-64",
+ "osxuniversal",
+ "windowsx86-64"
+ ],
+ "version": "2025.1.1-beta-3"
+ }
+ ]
+}
\ No newline at end of file
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
index 67bf3898..3718e0ac 100644
--- a/vendordeps/WPILibNewCommands.json
+++ b/vendordeps/WPILibNewCommands.json
@@ -3,7 +3,7 @@
"name": "WPILib-New-Commands",
"version": "1.0.0",
"uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
- "frcYear": "2024",
+ "frcYear": "2025",
"mavenUrls": [],
"jsonUrl": "",
"javaDependencies": [
diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json
new file mode 100644
index 00000000..c0b82958
--- /dev/null
+++ b/vendordeps/maple-sim.json
@@ -0,0 +1,26 @@
+{
+ "fileName": "maple-sim.json",
+ "name": "maplesim",
+ "version": "0.2.1",
+ "frcYear": "2025",
+ "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
+ "mavenUrls": [
+ "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases",
+ "https://repo1.maven.org/maven2"
+ ],
+ "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json",
+ "javaDependencies": [
+ {
+ "groupId": "org.ironmaple",
+ "artifactId": "maplesim-java",
+ "version": "0.2.1"
+ },
+ {
+ "groupId": "org.dyn4j",
+ "artifactId": "dyn4j",
+ "version": "5.0.2"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": []
+}
\ No newline at end of file
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 0e80a16c..ad3a530c 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,20 +1,34 @@
{
"fileName": "photonlib.json",
"name": "photonlib",
- "version": "v2024.3.1",
+ "version": "v2025.0.0-beta-5",
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
- "frcYear": "2024",
+ "frcYear": "2025",
"mavenUrls": [
"https://maven.photonvision.org/repository/internal",
"https://maven.photonvision.org/repository/snapshots"
],
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
- "jniDependencies": [],
+ "jniDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "photontargeting-cpp",
+ "version": "v2025.0.0-beta-5",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
"cppDependencies": [
{
"groupId": "org.photonvision",
"artifactId": "photonlib-cpp",
- "version": "v2024.3.1",
+ "version": "v2025.0.0-beta-5",
"libName": "photonlib",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -29,7 +43,7 @@
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-cpp",
- "version": "v2024.3.1",
+ "version": "v2025.0.0-beta-5",
"libName": "photontargeting",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -46,12 +60,12 @@
{
"groupId": "org.photonvision",
"artifactId": "photonlib-java",
- "version": "v2024.3.1"
+ "version": "v2025.0.0-beta-5"
},
{
"groupId": "org.photonvision",
"artifactId": "photontargeting-java",
- "version": "v2024.3.1"
+ "version": "v2025.0.0-beta-5"
}
]
}
\ No newline at end of file
+ 
+