From 98f5aa7ae4d0d5895a20cea2be2063ea3a23fe4c Mon Sep 17 00:00:00 2001 From: Luis Leyva Date: Sat, 14 Dec 2024 23:13:32 -0600 Subject: [PATCH] Implemented #927 and added default constructor --- .../lib/util/swerve/SwerveSetpointGenerator.cpp | 15 +++++++++++---- .../lib/util/swerve/SwerveSetpointGenerator.h | 5 +++++ 2 files changed, 16 insertions(+), 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 index 64ce2518..766bdfa7 100644 --- 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 index f85e041a..fa75c036 100644 --- 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 *