FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix Swerve Odemetry

This commit is contained in:
Tyler Wilcox 2026-03-24 20:52:56 -04:00
parent 8eb0a76b26
commit aad75b9b55
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
View File

@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.Subsystem;
@ -72,7 +73,7 @@ public class SwerveDriveSystem implements Subsystem{
gyro = new Pigeon2(pigeonId, canBus); gyro = new Pigeon2(pigeonId, canBus);
poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero); poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().in(Units.Revolution))), getModulePositions(), Pose3d.kZero);
chassisSpeeds = new ChassisSpeeds(); chassisSpeeds = new ChassisSpeeds();
} }
@ -84,7 +85,7 @@ public class SwerveDriveSystem implements Subsystem{
moduleStates[i].optimize(modules[i].getSteerRotations()); moduleStates[i].optimize(modules[i].getSteerRotations());
} }
poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions()); poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().in(Units.Rotations))), getModulePositions());
// poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions()); // poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
for(int i=0;i<modules.length;i++){ for(int i=0;i<modules.length;i++){
@ -178,7 +179,7 @@ public class SwerveDriveSystem implements Subsystem{
MAX_LINEAR_SPEED * x, MAX_LINEAR_SPEED * x,
MAX_LINEAR_SPEED * y, MAX_LINEAR_SPEED * y,
MAX_ANGULAR_SPEED * rot, MAX_ANGULAR_SPEED * rot,
Rotation2d.fromDegrees(getCurrentYaw().magnitude()) Rotation2d.fromDegrees(getCurrentYaw().in(Units.Degrees))
), new Translation2d()); ), new Translation2d());
} }
@ -193,7 +194,7 @@ public class SwerveDriveSystem implements Subsystem{
MAX_LINEAR_SPEED * x, MAX_LINEAR_SPEED * x,
MAX_LINEAR_SPEED * y, MAX_LINEAR_SPEED * y,
MAX_ANGULAR_SPEED * rot, MAX_ANGULAR_SPEED * rot,
Rotation2d.fromDegrees(getCurrentYaw().magnitude()) Rotation2d.fromDegrees(getCurrentYaw().in(Units.Degrees))
), new Translation2d()); ), new Translation2d());
} }

2
lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
View File

@ -172,7 +172,7 @@ public class SwerveModule {
} }
private double getModuleDistance(){ private double getModuleDistance(){
double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) * (Math.PI * wheelDiameterInMeters); double distance = (this.driveMotor.getPosition(true).getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
return distance; return distance;
} }