Implemented #927 and added default constructor

mjansen4857 · Dec 15, 2024 · 98f5aa7 · 98f5aa7
1 parent 026b33d
commit 98f5aa7
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 deletions.
diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp
@@ -1,5 +1,10 @@
 #include "pathplanner/lib/util/swerve/SwerveSetpointGenerator.h"
 
+SwerveSetpointGenerator::SwerveSetpointGenerator() {
+	this->config = nullptr;
+	this->maxSteerVelocity = 0_rad_per_s;
+}
+
 SwerveSetpointGenerator::SwerveSetpointGenerator(RobotConfig *config,
 		units::radians_per_second_t maxSteerVelocity) {
 	this->config = config;
@@ -213,10 +218,12 @@ SwerveSetpoint SwerveSetpointGenerator::generateSetpoint(
 		chassisForceVec = chassisForceVec + moduleForceVec;
 
 		// Calculate the torque this module will apply to the chassis
-		frc::Rotation2d angleToModule = config->moduleLocations[m].Angle();
-		frc::Rotation2d theta = moduleForceVec.Angle() - angleToModule;
-		chassisTorque += forceAtCarpet * config->modulePivotDistance[m]
-				* theta.Sin();
+		if (!epsilonEquals(0, moduleForceVec.Norm().value())) {
+			frc::Rotation2d angleToModule = config->moduleLocations[m].Angle();
+			frc::Rotation2d theta = moduleForceVec.Angle() - angleToModule;
+			chassisTorque += forceAtCarpet * config->modulePivotDistance[m]
+					* theta.Sin();
+		}
 	}
 
 	frc::Translation2d chassisAccelVec = chassisForceVec / config->mass.value();

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpointGenerator.h b/pathplannerlib/src/main/native/include/pathplanner/lib/util/swerve/SwerveSetpointGenerator.h
@@ -18,6 +18,11 @@ using namespace pathplanner;
 class SwerveSetpointGenerator {
 public:
 
+	/**
+	 * Create a new swerve setpoint generator
+	 */
+	SwerveSetpointGenerator();
+
 	/**
 	 * Create a new swerve setpoint generator
 	 *