Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Position Delta Odometry for Swerve (#4493)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:36 -04:00
committed by GitHub
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
View File

@@ -23,7 +23,7 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
m_frontRight.GetState(), m_backLeft.GetState(),
m_backRight.GetState());
m_odometry.Update(m_gyro.GetRotation2d(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()});
}

5
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
View File

@@ -41,6 +41,11 @@ frc::SwerveModuleState SwerveModule::GetState() const {
units::radian_t{m_turningEncoder.GetDistance()}};
}
frc::SwerveModulePosition SwerveModule::GetPosition() const {
return {units::meter_t{m_driveEncoder.GetDistance()},
units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
// Optimize the reference state to avoid spinning further than 90 degrees

6
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
View File

@@ -47,5 +47,9 @@ class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, m_gyro.GetRotation2d()};
frc::SwerveDriveOdometry<4> m_odometry{
m_kinematics,
m_gyro.GetRotation2d(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()}};
};

2
wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
View File

@@ -10,6 +10,7 @@
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/angular_velocity.h>
@@ -23,6 +24,7 @@ class SwerveModule {
int driveEncoderChannelA, int driveEncoderChannelB,
int turningEncoderChannelA, int turningEncoderChannelB);
frc::SwerveModuleState GetState() const;
frc::SwerveModulePosition GetPosition() const;
void SetDesiredState(const frc::SwerveModuleState& state);
private:

17
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -36,13 +36,17 @@ DriveSubsystem::DriveSubsystem()
kRearRightDriveEncoderPorts, kRearRightTurningEncoderPorts,
kRearRightDriveEncoderReversed, kRearRightTurningEncoderReversed},
m_odometry{kDriveKinematics, m_gyro.GetRotation2d(), frc::Pose2d{}} {}
m_odometry{kDriveKinematics,
m_gyro.GetRotation2d(),
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
frc::Pose2d{}} {}
void DriveSubsystem::Periodic() {
// Implementation of subsystem periodic method goes here.
m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
m_rearLeft.GetState(), m_frontRight.GetState(),
m_rearRight.GetState());
m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetPosition(),
m_rearLeft.GetPosition(), m_frontRight.GetPosition(),
m_rearRight.GetPosition());
}
void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
@@ -98,5 +102,8 @@ frc::Pose2d DriveSubsystem::GetPose() {
}
void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
m_odometry.ResetPosition(pose, units::degree_t{GetHeading()});
m_odometry.ResetPosition(
pose, units::degree_t{GetHeading()},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
}

5
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
View File

@@ -44,6 +44,11 @@ frc::SwerveModuleState SwerveModule::GetState() {
units::radian_t{m_turningEncoder.GetDistance()}};
}
frc::SwerveModulePosition SwerveModule::GetPosition() {
return {units::meter_t{m_driveEncoder.GetDistance()},
units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
// Optimize the reference state to avoid spinning further than 90 degrees

3
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
View File

@@ -10,6 +10,7 @@
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/motorcontrol/Spark.h>
#include <frc/trajectory/TrapezoidProfile.h>
@@ -24,6 +25,8 @@ class SwerveModule {
frc::SwerveModuleState GetState();
frc::SwerveModulePosition GetPosition();
void SetDesiredState(const frc::SwerveModuleState& state);
void ResetEncoders();

8
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
View File

@@ -27,9 +27,11 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
m_frontRight.GetState(), m_backLeft.GetState(),
m_backRight.GetState());
m_poseEstimator.Update(m_gyro.GetRotation2d(),
{m_frontLeft.GetState(), m_frontRight.GetState(),
m_backLeft.GetState(), m_backRight.GetState()},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()});
// Also apply vision measurements. We use 0.3 seconds in the past as an
// example -- on a real robot, this must be calculated based either on latency

5
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
View File

@@ -41,6 +41,11 @@ frc::SwerveModuleState SwerveModule::GetState() const {
units::radian_t{m_turningEncoder.GetDistance()}};
}
frc::SwerveModulePosition SwerveModule::GetPosition() const {
return {units::meter_t{m_driveEncoder.GetDistance()},
units::radian_t{m_turningEncoder.GetDistance()}};
}
void SwerveModule::SetDesiredState(
const frc::SwerveModuleState& referenceState) {
// Optimize the reference state to avoid spinning further than 90 degrees

10
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
View File

@@ -50,6 +50,12 @@ class Drivetrain {
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SwerveDrivePoseEstimator<4> m_poseEstimator{
frc::Rotation2d{}, frc::Pose2d{}, m_kinematics,
{0.1, 0.1, 0.1}, {0.05}, {0.1, 0.1, 0.1}};
frc::Rotation2d{},
frc::Pose2d{},
{m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
m_backLeft.GetPosition(), m_backRight.GetPosition()},
m_kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
{0.1, 0.1, 0.1}};
};

2
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h
View File

@@ -10,6 +10,7 @@
#include <frc/controller/PIDController.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/angular_velocity.h>
@@ -23,6 +24,7 @@ class SwerveModule {
int driveEncoderChannelA, int driveEncoderChannelB,
int turningEncoderChannelA, int turningEncoderChannelB);
frc::SwerveModuleState GetState() const;
frc::SwerveModulePosition GetPosition() const;
void SetDesiredState(const frc::SwerveModuleState& state);
private: