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

Remove encoder velocities methods in DifferentialDriveOdometry (#2147)

Browse Source 
It doesn't make sense to continue to provide a less accurate method of performing odometry
when a more accurate method using distances exists.

This also removes the need to pass DifferentialDriveKinematics to the constructor.
This commit is contained in:
Prateek Machiraju
2019-12-01 02:10:29 -05:00
committed by Peter Johnson
parent b8c1024261
commit 5b73c17f25
10 changed files with 41 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,62 +11,15 @@ 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;
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(
new Rotation2d());
@Test
void testOneIteration() {
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);
assertAll(
() -> assertEquals(0.02, pose.getTranslation().getX(), kEpsilon),
() -> assertEquals(0.00, pose.getTranslation().getY(), kEpsilon),
() -> assertEquals(0.00, pose.getRotation().getRadians(), kEpsilon)
);
}
@Test
void testQuarterCircle() {
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);
assertAll(
() -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon),
() -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon),
() -> 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)
);
}
@Test
void testOdometryWithEncoderDistances() {
m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));