Package me.wobblyyyy.pathfinder.kinematics

Class SwerveOdometry

java.lang.Object

me.wobblyyyy.pathfinder.kinematics.SwerveOdometry

public class SwerveOdometry
extends java.lang.Object

Helper class for determining the position of a robot that uses a swerve drive. This class uses linear algebra and inverse kinematics to determine a theoretical position from real or unreal velocity vectors representative of the robot's current translation. This class is designed to mesh with the SwerveKinematics class - in fact, you need to use an instance of the swerve kinematics class in order to make this class do anything at all. Unfortunately, this class doesn't handle EVERYTHING for you. You'll need to convert between swerve module states that indicate percent output power and swerve module states that indicate actual velocity vectors.

It's strongly suggested that you make use of the SpeedConverter class to handle conversion between motor percent output and actual velocity. This conversion system utilizes motor encoders and velocities to determine the actual velocity of the robot.

This class is pretty confusing, I can't lie. Don't be discouraged if you don't understand this at all - none of us do, let's be honest. However, if you're confused and want to not be confused, you can check out the online documentation for Pathfinder, shoot me an email, or post something on a forum maybe? Up to you. Good luck!

Since:

0.3.0

Author:

Colin Robertson

Field Summary

Fields

Modifier and Type	Field	Description
private Angle	gyroOffset
private SwerveKinematics	kinematics
private HeadingPoint	position
private double	previousSeconds

Constructor Summary

Constructors

Constructor	Description
SwerveOdometry(SwerveKinematics kinematics)	Create a new SwerveOdometry instance without using an angle offset or a positional offset.
SwerveOdometry(SwerveKinematics kinematics, Angle gyroOffset, HeadingPoint initialPosition)	Create a new SwerveOdometry tracker based on a kinematic representation of the swerve chassis, an angle offset, and the current or initial position of the robot.

Method Summary

Modifier and Type	Method	Description
private Angle	applyAngleOffset(Angle gyroAngle)
private double	deltaTheta(RTransform transformation)
private double	deltaX(RTransform transformation, double time)
private double	deltaY(RTransform transformation, double time)
private RTransform	getChassisStateFromStates(me.wobblyyyy.edt.StaticArray<SwerveState> states)
private double	getGapAndUpdateTime(double currentTime)
HeadingPoint	getPosition()	Get the current reported position of the tracker.
private HeadingPoint	transformCurrentPosition(double time, RTransform transformation)
HeadingPoint	update(double currentSeconds, Angle gyroAngle, me.wobblyyyy.edt.StaticArray<SwerveState> states)	Update the positional tracker's internal position.
HeadingPoint	update(Angle gyroAngle, me.wobblyyyy.edt.StaticArray<SwerveState> states)	Update the positional tracker's internal position.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Details

kinematics

private final SwerveKinematics kinematics

position

private HeadingPoint position

previousSeconds

private double previousSeconds

gyroOffset

private final Angle gyroOffset

Constructor Details

SwerveOdometry

public SwerveOdometry(SwerveKinematics kinematics,
Angle gyroOffset,
HeadingPoint initialPosition)

Create a new SwerveOdometry tracker based on a kinematic representation of the swerve chassis, an angle offset, and the current or initial position of the robot.

Parameters:

kinematics - a kinematic representation of the robot's swerve drive chassis. See the SwerveKinematics class to learn more.

gyroOffset - the offset that should be applied to the angle.

initialPosition - the starting position of the robot.

SwerveOdometry

public SwerveOdometry(SwerveKinematics kinematics)

Create a new SwerveOdometry instance without using an angle offset or a positional offset. This means that whatever angle the gyro reports at the time of initialization is the "base" angle, and the robot's starting position is set to (0, 0).

Parameters:

kinematics - a kinematic representation of the robot's swerve drive chassis. See the SwerveKinematics to learn more.

Method Details

getPosition

public HeadingPoint getPosition()

Get the current reported position of the tracker. This position is represented in whatever units you're using natively - meters, inches, feet, miles, whatever you want, really?

Returns:

the robot's reported position. This position is based on a series of inverse kinematic operations that are performed on a constantly-transformed internal position. Please note that this position value will be very inaccurate if your timing isn't at least relatively consistent and small.

getGapAndUpdateTime

private double getGapAndUpdateTime(double currentTime)

applyAngleOffset

private Angle applyAngleOffset(Angle gyroAngle)

getChassisStateFromStates

private RTransform getChassisStateFromStates(me.wobblyyyy.edt.StaticArray<SwerveState> states)

deltaX

private double deltaX(RTransform transformation,
double time)

deltaY

private double deltaY(RTransform transformation,
double time)

deltaTheta

private double deltaTheta(RTransform transformation)

transformCurrentPosition

private HeadingPoint transformCurrentPosition(double time,
RTransform transformation)

update

public HeadingPoint update(double currentSeconds,
Angle gyroAngle,
me.wobblyyyy.edt.StaticArray<SwerveState> states)

Update the positional tracker's internal position. This method should be called as frequently to ensure the highest degree of accuracy when it comes to positional tracking. The angle should be reported directly from a gyroscope. In most cases, these states should be passed through a speed conversion system prior to this method's invocation. Regular swerve module states use a (-1, 1) range for power - NOT actual movement velocity, meaning that positional tracking is rendered inaccurate.

On these states: in most cases, the swerve module states that are set to your robot are within the range of (-1, 1) for power. These values are set to the robot's motor controllers or motors, and thus, the motors are powered accordingly. This works fine, but there's one major issue. If you attempt to use these values to determine the position of the robot, the robot's assumed position will be terribly inaccurate. This is because these states aren't representative of the robot's actual movement, but rather, the robot's desired movement. There are already provided methods that will allow you to get the robot's actual position instead of the robot's theoretical position - check out the online documentation or shoot me an email if you're confused or need any help.

Parameters:

currentSeconds - the current time, in seconds. This time can be fetched using the Time.relativeTime() method. Using this method will ensure the best degree of accuracy.

gyroAngle - the angle of the gyroscope. This angle should come right from the gyroscope without any intervention.

states - swerve module states that represent the robot's current movement vector.

update

public HeadingPoint update(Angle gyroAngle,
me.wobblyyyy.edt.StaticArray<SwerveState> states)

Update the positional tracker's internal position. This method should be called as frequently to ensure the highest degree of accuracy when it comes to positional tracking. The angle should be reported directly from a gyroscope. In most cases, these states should be passed through a speed conversion system prior to this method's invocation. Regular swerve module states use a (-1, 1) range for power - NOT actual movement velocity, meaning that positional tracking is rendered inaccurate.

On these states: in most cases, the swerve module states that are set to your robot are within the range of (-1, 1) for power. These values are set to the robot's motor controllers or motors, and thus, the motors are powered accordingly. This works fine, but there's one major issue. If you attempt to use these values to determine the position of the robot, the robot's assumed position will be terribly inaccurate. This is because these states aren't representative of the robot's actual movement, but rather, the robot's desired movement. There are already provided methods that will allow you to get the robot's actual position instead of the robot's theoretical position - check out the online documentation or shoot me an email if you're confused or need any help.

This method works by multiplying the amount of time since the last update by the robot's current velocity vector, denoted as d(vX), d(vY), d(vT), where T is theta, or the robot's current gyroscope angle. If the amount of time is wildly inconsistent, such as a whole second or so, this method will then be very inaccurate, and the robot's position will seem to teleport a significant amount. Such, it's very important to call this method as frequently as possible, thus ensuring the odometry's accuracy.

Just as a side note: this method is an overloaded method for the other update method. In almost all cases, you should use THIS method, and not the other method. This ensures standardization of time - calling the other method can cause confusing results, as time measurements might not be as accurate as this method ensures.

Parameters:

gyroAngle - the angle of the gyroscope. This angle should come right from the gyroscope without any intervention.

states - swerve module states that represent the robot's current movement vector.