mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
841ef91c0f
The odometry classes previously took in the robot angle as an argument, meaning that users had to take care of offsetting the gyro themselves to accurately report the robot angle. This change will make it so that users will not have to worry about resetting gyros and adding offsets themselves, as this will be handled by the odometry classes.
40 lines
1.5 KiB
C++
40 lines
1.5 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "frc/kinematics/DifferentialDriveOdometry.h"
|
|
|
|
using namespace frc;
|
|
|
|
DifferentialDriveOdometry::DifferentialDriveOdometry(
|
|
DifferentialDriveKinematics kinematics, const Rotation2d& gyroAngle,
|
|
const Pose2d& initialPose)
|
|
: m_kinematics(kinematics), m_pose(initialPose) {
|
|
m_previousAngle = m_pose.Rotation();
|
|
m_gyroOffset = m_pose.Rotation() - gyroAngle;
|
|
}
|
|
|
|
const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
|
|
units::second_t currentTime, const Rotation2d& gyroAngle,
|
|
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
|
|
units::second_t deltaTime =
|
|
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
|
|
m_previousTime = currentTime;
|
|
|
|
auto angle = gyroAngle + m_gyroOffset;
|
|
|
|
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
|
|
static_cast<void>(dtheta);
|
|
|
|
auto newPose = m_pose.Exp(
|
|
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
|
|
|
|
m_previousAngle = angle;
|
|
m_pose = {newPose.Translation(), angle};
|
|
|
|
return m_pose;
|
|
}
|