Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

drivetrain odometry -> pose estimator

Browse Source
This commit is contained in:
Tylr-J42 2025-01-30 04:14:57 -05:00
parent 3af046f058
commit 89c1914d11
2 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -13,11 +13,11 @@ import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
@ -38,7 +38,7 @@ public class Drivetrain extends SubsystemBase {
private AHRS ahrs;
// Odometry class for tracking robot pose
private SwerveDriveOdometry m_odometry;
private SwerveDrivePoseEstimator m_estimator;
/** Creates a new DriveSubsystem. */
public Drivetrain() {
@ -68,7 +68,7 @@ public class Drivetrain extends SubsystemBase {
ahrs = new AHRS(NavXComType.kMXP_SPI);
m_odometry = new SwerveDriveOdometry(
m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics,
Rotation2d.fromDegrees(ahrs.getAngle()),
new SwerveModulePosition[] {
@ -76,7 +76,9 @@ public class Drivetrain extends SubsystemBase {
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
});
},
new Pose2d()
);
AutoBuilder.configure(
this::getPose,
@ -99,7 +101,7 @@ public class Drivetrain extends SubsystemBase {
@Override
public void periodic() {
// Update the odometry in the periodic block
m_odometry.update(
m_estimator.update(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
@ -132,7 +134,7 @@ public class Drivetrain extends SubsystemBase {
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
return m_estimator.getEstimatedPosition();
}
/**
@ -141,14 +143,7 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry.
*/
public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
m_estimator.resetPose(
pose
);
}
@ -257,4 +252,8 @@ public class Drivetrain extends SubsystemBase {
public double getTurnRate() {
return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
}
public void addVisionMeasurement(Pose2d pose, double timestamp){
m_estimator.addVisionMeasurement(pose, timestamp);
}
}

4
src/main/java/frc/robot/subsystems/TylerVision.java → src/main/java/frc/robot/subsystems/Vision.java
View File

@ -5,7 +5,7 @@ import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
public class TylerVision{
public class Vision{
private DoubleSubscriber cam1GlobalPose;
private DoubleSubscriber cam1ClosestTag;
@ -13,7 +13,7 @@ public class TylerVision{
private DoubleSubscriber framerate;
public TylerVision(){
public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault();
NetworkTable visionTable = inst.getTable("Fiducial");