Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add 3D odometry and pose estimation (#7119)

Browse Source
This commit is contained in:
Joseph Eng
2024-11-16 07:56:14 -08:00
committed by GitHub
parent aa7dd258c4
commit 2acf111f56
49 changed files with 6716 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -11,9 +11,8 @@ using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose)
: Odometry<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>(
m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
: Odometry(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}

18
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp Normal file
View File

@@ -0,0 +1,18 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/DifferentialDriveOdometry3d.h"
#include "wpimath/MathShared.h"
using namespace frc;
DifferentialDriveOdometry3d::DifferentialDriveOdometry3d(
const Rotation3d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose3d& initialPose)
: Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
initialPose) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1);
}

3
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -11,8 +11,7 @@ using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
: Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>(
m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
: Odometry(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);

18
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp Normal file
View File

@@ -0,0 +1,18 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/MecanumDriveOdometry3d.h"
#include "wpimath/MathShared.h"
using namespace frc;
MecanumDriveOdometry3d::MecanumDriveOdometry3d(
MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose)
: Odometry3d(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose),
m_kinematicsImpl(kinematics) {
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kOdometry_MecanumDrive, 1);
}

12
wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp Normal file
View File

@@ -0,0 +1,12 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/kinematics/SwerveDriveOdometry3d.h"
namespace frc {
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
SwerveDriveOdometry3d<4>;
} // namespace frc