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

trying to fuse apriltag pose with odometry

Browse Source
This commit is contained in:
Team 2648 2025-03-07 19:04:53 -05:00
parent 445ce9bf6f
commit 649660ade6
1 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,6 +17,7 @@ import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType; import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
@ -26,6 +27,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.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Filesystem;
@ -104,6 +106,8 @@ public class Drivetrain extends SubsystemBase {
new Pose2d() new Pose2d()
); );
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0); pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
pidHeading.enableContinuousInput(-180, 180); pidHeading.enableContinuousInput(-180, 180);