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

Use gyro angle instead of robot angle for odometry (#2081)

Browse Source 
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.
This commit is contained in:
Prateek Machiraju
2019-11-15 20:34:10 -05:00
committed by Peter Johnson
parent 1b66ead49d
commit 841ef91c0f
23 changed files with 258 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -33,19 +33,26 @@ class DifferentialDriveOdometry {
* Constructs a DifferentialDriveOdometry object.
*
* @param kinematics The differential drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
void ResetPosition(const Pose2d& pose) {
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
@@ -63,13 +70,13 @@ class DifferentialDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& angle,
const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds);
/**
@@ -80,14 +87,15 @@ class DifferentialDriveOdometry {
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& angle,
const Pose2d& Update(const Rotation2d& gyroAngle,
const DifferentialDriveWheelSpeeds& wheelSpeeds) {
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
wheelSpeeds);
}
private:
@@ -95,6 +103,7 @@ class DifferentialDriveOdometry {
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
};
} // namespace frc