Add Swerve Setpoint Generator to PPLib C++

Writerside/topics/pplib-Swerve-Setpoint-Generator.md

@@ -15,13 +15,6 @@ This improves on the original version in a few ways:
 * Friction within the module itself is taken into account, which decreases maximum acceleration but increases maximum
   deceleration
 
-> **Note**
->
-> This feature is only available in PathPlannerLib Java and Python. If you would like to see this feature in PPLib C++,
-> feel free to open a pull request to translate it.
->
-{style="note"}
-
 <tabs group="pplib-language">
 <tab title="Java" group-key="java">
 
@@ -75,6 +68,57 @@ public class SwerveSubsystem extends Subsystem {
 ```
 
 </tab>
+
+<tab title="C++" group-key="cpp">
+
+```C++
+#include <pathplanner/lib/config/RobotConfig.h>
+#include <pathplanner/lib/util/swerve/SwerveSetpointGenerator.h>
+#include <pathplanner/lib/util/swerve/SwerveSetpoint.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/kinematics/ChassisSpeeds.h>
+
+using namespace pathplanner;
+
+SwerveSubsystem::SwerveSubsystem(){
+    // All other subsystem initialization
+    // ...
+
+    // Load the RobotConfig from the GUI settings. You should probably
+    // store this in your Constants file
+    RobotConfig config = RobotConfig::fromGUISettings();
+
+    setpointGenerator = SwerveSetpointGenerator(
+		config,
+		10_rad_per_s
+	);
+
+	// Initialize the previous setpoint to the robot's current speeds & module states
+	frc::ChassisSpeeds currentSpeeds = getCurrentSpeeds(); // Method to get current robot-relative chassis speeds
+	std::vector<frc::SwerveModuleState> currentStates = getCurrentModuleStates(); // Method to get the current swerve module states
+	previousSetpoint = SwerveSetpoint(currentSpeeds, currentStates, DriveFeedforwards::zeros(config.numModules));
+}
+
+/**
+ * This method will take in desired robot-relative chassis speeds,
+ * generate a swerve setpoint, then set the target state for each module
+ *
+ * @param speeds The desired robot-relative speeds
+ */
+void SwerveSubsystem::driveRobotRelative(frc::ChassisSpeeds speeds) {
+	// Note: it is important to not discretize speeds before or after
+	// using the setpoint generator, as it will discretize them for you
+	previousSetpoint = setpointGenerator.generateSetpoint(
+		previousSetpoint, // The previous setpoint
+		speeds, // The desired target speeds
+		0.02_s // The loop time of the robot code, in seconds
+	);
+	setModuleStates(previousSetpoint.moduleStates()); // Method that will drive the robot given target module states
+}
+```
+
+</tab>
+
 <tab title="Python" group-key="python">
 
 ```Python

pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp

@@ -6,6 +6,11 @@
 
 using namespace pathplanner;
 
+RobotConfig::RobotConfig() : swerveKinematics(frc::Translation2d(0_m, 0_m),
+		frc::Translation2d(0_m, 0_m), frc::Translation2d(0_m, 0_m),
+		frc::Translation2d(0_m, 0_m)), diffKinematics(0.7_m) {
+}
+
 RobotConfig::RobotConfig(units::kilogram_t mass,
 		units::kilogram_square_meter_t MOI, ModuleConfig moduleConfig,
 		std::vector<frc::Translation2d> moduleOffsets) : mass(mass), MOI(MOI), moduleConfig(
@@ -155,6 +160,30 @@ frc::ChassisSpeeds RobotConfig::toChassisSpeeds(
 	}
 }
 
+frc::ChassisSpeeds RobotConfig::toChassisSpeeds(
+		std::vector<frc::SwerveModuleState> states) const {
+	if (isHolonomic) {
+		wpi::array < frc::SwerveModuleState, 4 > wpiStates { states.at(0),
+				states.at(1), states.at(2), states.at(3) };
+		return swerveKinematics.ToChassisSpeeds(wpiStates);
+	} else {
+		frc::DifferentialDriveWheelSpeeds wheelSpeeds { states.at(0).speed,
+				states.at(1).speed };
+		return diffKinematics.ToChassisSpeeds(wheelSpeeds);
+	}
+}
+
+std::vector<frc::SwerveModuleState> RobotConfig::desaturateWheelSpeeds(
+		std::vector<frc::SwerveModuleState> moduleStates,
+		units::meters_per_second_t maxSpeed) const {
+	wpi::array < frc::SwerveModuleState, 4 > wpiStates { moduleStates.at(0),
+			moduleStates.at(1), moduleStates.at(2), moduleStates.at(3) };
+	swerveKinematics.DesaturateWheelSpeeds(&wpiStates, maxSpeed);
+
+	return std::vector < frc::SwerveModuleState
+			> (wpiStates.begin(), wpiStates.end());
+}
+
 std::vector<frc::Translation2d> RobotConfig::chassisForcesToWheelForceVectors(
 		frc::ChassisSpeeds chassisForces) const {
 	Eigen::Vector3d chassisForceVector { chassisForces.vx.value(),