SwerveLocalizer

class SwerveLocalizer @JvmOverloads constructor(val hardwareMap: HardwareMap, val cpr: Int, var inPerTick: Double, val imu: IMU, val kinematics: SwerveKinematics, driveEncoders: List<Encoder>, steeringEncoders: List<Encoder>, val initialPose: Pose2d = Pose2d.zero) : Localizer

Localizer based on swerve drive encoders and an IMU. This localizer is not recommended for use with dead wheels.

Parameters

hardwareMap

hardware map

cpr

counts per revolution of the steering encoders

inPerTick

inches per tick of the drive encoders

imu

IMU

kinematics

swerve kinematics

driveEncoders

drive encoders

steeringEncoders

steering encoders

initialPose

initial pose

Constructors

Link copied to clipboard
constructor(hardwareMap: HardwareMap, cpr: Int, inPerTick: Double, imu: IMU, kinematics: SwerveKinematics, driveNames: List<String> = listOf("lfDrive", "frDrive", "blDrive", "brDrive"), steeringNames: List<String> = listOf("lfSteering", "rfSteering", "lbSteering", "rbSteering"), driveDirections: List<DcMotorSimple.Direction> = driveNames.map { DcMotorSimple.Direction.FORWARD }, steeringDirections: List<DcMotorSimple.Direction> = steeringNames.map { DcMotorSimple.Direction.FORWARD }, initialPose: Pose2d = Pose2d.zero)

Creates a new localizer.

constructor(hardwareMap: HardwareMap, cpr: Int, inPerTick: Double, imu: IMU, kinematics: SwerveKinematics, driveEncoders: List<Encoder>, steeringEncoders: List<Encoder>, initialPose: Pose2d = Pose2d.zero)

Properties

Link copied to clipboard
val cpr: Int
Link copied to clipboard
val hardwareMap: HardwareMap
Link copied to clipboard
val imu: IMU
Link copied to clipboard
Link copied to clipboard
Link copied to clipboard
Link copied to clipboard
open override var pose: Pose2d

The current robot pose.

Link copied to clipboard
open override val poseHistory: ArrayList<Pose2d>

A list of recent poses.

Link copied to clipboard
open override var vel: PoseVelocity2d

The current robot velocity.

Functions

Link copied to clipboard
fun fromMotors(driveMotors: List<DcMotorEx>, steeringMotors: List<DcMotorEx>): SwerveLocalizer

Returns a new localizer with the given motors.

Link copied to clipboard
open override fun update(): PoseVelocity2d

Updates the localizer and returns the new velocity.

Link copied to clipboard
fun withDirections(driveDirections: List<DcMotorSimple.Direction>, steeringDirections: List<DcMotorSimple.Direction>): SwerveLocalizer

Returns a new localizer with the given motor directions.

Link copied to clipboard

Returns a new localizer with the given initial pose.

Link copied to clipboard
fun withNames(driveNames: List<String>, steeringNames: List<String>): SwerveLocalizer

Returns a new localizer with the given motor names.