?

mjansen4857 · Sep 26, 2024 · a453d5f · a453d5f
1 parent 1a68ad1
commit a453d5f
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 20 deletions.
diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/config/RobotConfig.cpp
@@ -17,12 +17,10 @@ RobotConfig::RobotConfig(units::kilogram_t mass,
 		frc::Translation2d(wheelbase / 2, -trackwidth / 2),
 		frc::Translation2d(-wheelbase / 2, trackwidth / 2),
 		frc::Translation2d(-wheelbase / 2, -trackwidth / 2)), diffKinematics(
-		frc::Translation2d(0_m, trackwidth / 2),
-		frc::Translation2d(0_m, -trackwidth / 2)), isHolonomic(true), numModules(
-		4), modulePivotDistance { moduleLocations[0].Norm(),
-		moduleLocations[1].Norm(), moduleLocations[2].Norm(),
-		moduleLocations[3].Norm() }, wheelFrictionForce { moduleConfig.wheelCOF
-		* ((mass() / numModules) * 9.8) }, maxTorqueFriction(
+		trackwidth), isHolonomic(true), numModules(4), modulePivotDistance {
+		moduleLocations[0].Norm(), moduleLocations[1].Norm(),
+		moduleLocations[2].Norm(), moduleLocations[3].Norm() }, wheelFrictionForce {
+		moduleConfig.wheelCOF * ((mass() / numModules) * 9.8) }, maxTorqueFriction(
 		wheelFrictionForce * moduleConfig.wheelRadius) {
 }
 
@@ -35,11 +33,9 @@ RobotConfig::RobotConfig(units::kilogram_t mass,
 		frc::Translation2d(trackwidth / 2, -trackwidth / 2),
 		frc::Translation2d(-trackwidth / 2, trackwidth / 2),
 		frc::Translation2d(-trackwidth / 2, -trackwidth / 2)), diffKinematics(
-		frc::Translation2d(0_m, trackwidth / 2),
-		frc::Translation2d(0_m, -trackwidth / 2)), isHolonomic(false), numModules(
-		2), modulePivotDistance { moduleLocations[0].Norm(),
-		moduleLocations[1].Norm() }, wheelFrictionForce { moduleConfig.wheelCOF
-		* ((mass() / numModules) * 9.8) }, maxTorqueFriction(
+		trackwidth), isHolonomic(false), numModules(2), modulePivotDistance {
+		moduleLocations[0].Norm(), moduleLocations[1].Norm() }, wheelFrictionForce {
+		moduleConfig.wheelCOF * ((mass() / numModules) * 9.8) }, maxTorqueFriction(
 		wheelFrictionForce * moduleConfig.wheelRadius) {
 }
 

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/config/RobotConfig.h b/pathplannerlib/src/main/native/include/pathplanner/lib/config/RobotConfig.h
@@ -7,6 +7,7 @@
 #include <units/moment_of_inertia.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <vector>
 #include "pathplanner/lib/config/ModuleConfig.h"
 #include "pathplanner/lib/trajectory/SwerveModuleTrajectoryState.h"
@@ -23,7 +24,7 @@ class RobotConfig {
 	// Need two different kinematics objects since the class is templated but having
 	// RobotConfig also templated would be pretty bad to work with
 	frc::SwerveDriveKinematics<4> swerveKinematics;
-	frc::SwerveDriveKinematics<2> diffKinematics;
+	frc::DifferentialDriveKinematics diffKinematics;
 	bool isHolonomic;
 
 	size_t numModules;

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/trajectory/PathPlannerTrajectory.h b/pathplannerlib/src/main/native/include/pathplanner/lib/trajectory/PathPlannerTrajectory.h
@@ -187,10 +187,12 @@ class PathPlannerTrajectory {
 			return std::vector < frc::SwerveModuleState
 					> (states.begin(), states.end());
 		} else {
-			auto states = config.diffKinematics.ToSwerveModuleStates(
+			auto wheelSpeeeds = config.diffKinematics.ToWheelSpeeds(
 					chassisSpeeds);
-			return std::vector < frc::SwerveModuleState
-					> (states.begin(), states.end());
+			return std::vector<frc::SwerveModuleState> {
+					frc::SwerveModuleState { wheelSpeeeds.left,
+							frc::Rotation2d() }, frc::SwerveModuleState {
+							wheelSpeeeds.right, frc::Rotation2d() }, };
 		}
 	}
 
@@ -206,11 +208,9 @@ class PathPlannerTrajectory {
 									states[3].speed, states[3].angle } };
 			return config.swerveKinematics.ToChassisSpeeds(wpiStates);
 		} else {
-			wpi::array < frc::SwerveModuleState, 2
-					> wpiStates { frc::SwerveModuleState { states[0].speed,
-							states[0].angle }, frc::SwerveModuleState {
-							states[1].speed, states[1].angle } };
-			return config.diffKinematics.ToChassisSpeeds(wpiStates);
+			frc::DifferentialDriveWheelSpeeds speeds { states[0].speed,
+					states[1].speed };
+			return config.diffKinematics.ToChassisSpeeds(speeds);
 		}
 	}
 };