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

25
wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
View File

@@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
@@ -19,11 +20,12 @@ class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(0.381 * 2);
private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics);
private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics,
new Rotation2d());
@Test
void testOneIteration() {
m_odometry.resetPosition(new Pose2d());
m_odometry.resetPosition(new Pose2d(), new Rotation2d());
var speeds = new DifferentialDriveWheelSpeeds(0.02, 0.02);
m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, new Rotation2d(), speeds);
@@ -37,7 +39,7 @@ class DifferentialDriveOdometryTest {
@Test
void testQuarterCircle() {
m_odometry.resetPosition(new Pose2d());
m_odometry.resetPosition(new Pose2d(), new Rotation2d());
var speeds = new DifferentialDriveWheelSpeeds(0.0, 5 * Math.PI);
m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), speeds);
@@ -48,4 +50,21 @@ class DifferentialDriveOdometryTest {
() -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
);
}
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var speeds = new DifferentialDriveWheelSpeeds(1.0, 1.0);
m_odometry.updateWithTime(0.0, gyro, new DifferentialDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
assertAll(
() -> assertEquals(1.0, pose.getTranslation().getX(), kEpsilon),
() -> assertEquals(0.0, pose.getTranslation().getY(), kEpsilon),
() -> assertEquals(0.0, pose.getRotation().getRadians(), kEpsilon)
);
}
}

22
wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
View File

@@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -21,10 +22,12 @@ class MecanumDriveOdometryTest {
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
new Rotation2d());
@Test
void testMultipleConsecutiveUpdates() {
@@ -71,4 +74,21 @@ class MecanumDriveOdometryTest {
);
}
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
3.536, 3.536);
m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
assertAll(
() -> assertEquals(5.0, pose.getTranslation().getX(), 0.1),
() -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
);
}
}

22
wpilibj/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
View File

@@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
@@ -24,7 +25,8 @@ class SwerveDriveOdometryTest {
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
new Rotation2d());
@Test
void testTwoIterations() {
@@ -73,4 +75,22 @@ class SwerveDriveOdometryTest {
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
);
}
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var state = new SwerveModuleState();
m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
assertAll(
() -> assertEquals(1.0, pose.getTranslation().getX(), 0.1),
() -> assertEquals(0.00, pose.getTranslation().getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
);
}
}