GenericOdometry

Generic odometry implementation that can use an instance of Kinematics to determine a robot's position. This class integrates velocity data (derived from a Kinematics object) over time, along with gyroscope readings, to track the robot's PointXYZ position.

Methods

GenericOdometry(Kinematics<T> kinematics, Angle gyroAngle, PointXYZ initialPosition)

Constructs a new GenericOdometry instance with a default update interval of 0 milliseconds.

• kinematics: An instance of Kinematics that provides the robot's velocity based on its state.
• gyroAngle: The initial angle of the gyroscope.
• initialPosition: The initial PointXYZ of the robot.

GenericOdometry(Kinematics<T> kinematics, Angle gyroAngle, PointXYZ initialPosition, double updateIntervalMs)

Constructs a new GenericOdometry instance with a specified update interval.

• kinematics: An instance of Kinematics that provides the robot's velocity based on its state.
• gyroAngle: The initial angle of the gyroscope.
• initialPosition: The initial PointXYZ of the robot.
• updateIntervalMs: The minimum time (in milliseconds) that must elapse before the position is updated. If less time has passed, the previous position is returned.

updateWithTime(double currentTimeMs, Angle gyroAngle, T state)

Updates the robot's position based on the system's current time, the gyroscope angle, and the current state of the robot. This method integrates the velocity of the robot (as determined by the state parameter and the kinematics object) over the elapsed time to determine the new position.

• currentTimeMs: The system's current time in milliseconds (e.g., from Time.ms() or System.currentTimeMillis()).
• gyroAngle: The current angle of the robot's gyroscope.
• state: The current state of the robot, typically derived from encoder readings to determine velocity.

Returns the robot's newly updated PointXYZ position.