Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +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

30
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
View File

@@ -30,18 +30,21 @@ public class DifferentialDriveOdometry {
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param kinematics The differential drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle,
Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
}
@@ -49,24 +52,31 @@ public class DifferentialDriveOdometry {
* Constructs a DifferentialDriveOdometry object with the default pose at the origin.
*
* @param kinematics The differential drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
*/
public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics) {
this(kinematics, new Pose2d());
public DifferentialDriveOdometry(DifferentialDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field. Do NOT zero your encoders if you
* call this function at any other time except initialization.
*
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters) {
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
@@ -82,15 +92,17 @@ public class DifferentialDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param angle The current robot angle.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
DifferentialDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
@@ -111,13 +123,13 @@ public class DifferentialDriveOdometry {
* 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.
*/
public Pose2d update(Rotation2d angle,
public Pose2d update(Rotation2d gyroAngle,
DifferentialDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(Timer.getFPGATimestamp(),
angle, wheelSpeeds);
gyroAngle, wheelSpeeds);
}
}

31
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
View File

@@ -26,17 +26,21 @@ public class MecanumDriveOdometry {
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
/**
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Pose2d initialPoseMeters) {
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
}
@@ -44,23 +48,30 @@ public class MecanumDriveOdometry {
* Constructs a MecanumDriveOdometry object with the default pose at the origin.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics) {
this(kinematics, new Pose2d());
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters) {
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
@@ -76,15 +87,17 @@ public class MecanumDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @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.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
@@ -104,13 +117,13 @@ public class MecanumDriveOdometry {
* 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.
*/
public Pose2d update(Rotation2d angle,
public Pose2d update(Rotation2d gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(Timer.getFPGATimestamp(), angle,
return updateWithTime(Timer.getFPGATimestamp(), gyroAngle,
wheelSpeeds);
}
}

33
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
View File

@@ -26,17 +26,21 @@ public class SwerveDriveOdometry {
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
/**
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Pose2d initialPose) {
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPose;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPose.getRotation();
}
@@ -44,23 +48,30 @@ public class SwerveDriveOdometry {
* Constructs a SwerveDriveOdometry object with the default pose at the origin.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics) {
this(kinematics, new Pose2d());
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* @param pose The position on the field that your robot is at.
* <p>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.
*/
public void resetPosition(Pose2d pose) {
public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
m_poseMeters = pose;
m_previousAngle = pose.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
@@ -76,17 +87,19 @@ public class SwerveDriveOdometry {
* angular rate that is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param angle The angle of the robot.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated your
* SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d angle,
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
SwerveModuleState... moduleStates) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
var newPose = m_poseMeters.exp(
new Twist2d(chassisState.vxMetersPerSecond * period,
@@ -107,13 +120,13 @@ public class SwerveDriveOdometry {
* 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 moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated your
* SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d angle, SwerveModuleState... moduleStates) {
return updateWithTime(Timer.getFPGATimestamp(), angle, moduleStates);
public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
return updateWithTime(Timer.getFPGATimestamp(), gyroAngle, moduleStates);
}
}