DifferentialDriveOdometry

Odometry for a differential drive train. This class uses a gyroscope and two encoders to determine the robot's position. Unlike some other odometry implementations, this one does not rely on velocity, meaning time is not a factor in its calculations.

Key Concepts

• Gyroscope: Used to track the robot's heading.
• Encoders: Two encoders (left and right) are used to measure the distance traveled by each side of the robot.
• Position Tracking: The class continuously updates the robot's PointXYZ position based on encoder readings and gyroscope data.

Methods

DifferentialDriveOdometry(Angle gyroAngle, PointXYZ initialPosition)

Constructs a new DifferentialDriveOdometry instance.

• gyroAngle: A Supplier<Angle> that provides the current angle of the gyroscope. This is used to calculate the initial gyroscope offset.
• initialPosition: The starting PointXYZ of the robot for the odometry.

resetPosition(PointXYZ position, Angle gyroAngle)

Resets the odometry's position to a new PointXYZ and recalibrates the gyroscope offset based on the provided gyroAngle.

getPosition()

Returns the robot's current PointXYZ position without updating it.

update(Angle gyroAngle, double rightDistance, double leftDistance)

Updates the robot's position based on new gyroscope and encoder readings. The rightDistance and leftDistance parameters should be the total accumulated distances from the encoders, not just the elapsed distance since the last update.

• gyroAngle: The current angle reported by the gyroscope.
• rightDistance: The total distance measured by the right encoder.
• leftDistance: The total distance measured by the left encoder.

Returns the newly updated PointXYZ position of the robot.