MecanumDriveLocalizer

class MecanumDriveLocalizer @JvmOverloads constructor(val hardwareMap: HardwareMap, var inPerTick: Double, val imu: IMU, val kinematics: MecanumKinematics, val leftFront: Encoder, val leftBack: Encoder, val rightFront: Encoder, val rightBack: Encoder, val initialPose: Pose2d = Pose2d.zero) : Localizer

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

Parameters

hardwareMap

hardware map

inPerTick

inches per tick

imu

IMU

kinematics

mecanum kinematics

leftFront

left front encoder

leftBack

left back encoder

rightFront

right front encoder

rightBack

right back encoder

initialPose

initial pose

Constructors

Link copied to clipboard
constructor(hardwareMap: HardwareMap, inPerTick: Double, imu: IMU, kinematics: MecanumKinematics, lfName: String = "leftFront", lbName: String = "leftBack", rfName: String = "rightFront", rbName: String = "rightBack", lfDirection: DcMotorSimple.Direction = DcMotorSimple.Direction.FORWARD, lbDirection: DcMotorSimple.Direction = DcMotorSimple.Direction.FORWARD, rfDirection: DcMotorSimple.Direction = DcMotorSimple.Direction.FORWARD, rbDirection: DcMotorSimple.Direction = DcMotorSimple.Direction.FORWARD, initialPose: Pose2d = Pose2d.zero)

Creates a new localizer.

constructor(hardwareMap: HardwareMap, inPerTick: Double, imu: IMU, kinematics: MecanumKinematics, leftFront: Encoder, leftBack: Encoder, rightFront: Encoder, rightBack: Encoder, initialPose: Pose2d = Pose2d.zero)

Properties

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
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: MutableList<Pose2d>

A list of recent poses.

Link copied to clipboard
Link copied to clipboard
Link copied to clipboard
open override var vel: PoseVelocity2d

The current robot velocity.

Functions

Link copied to clipboard
fun fromMotors(lf: DcMotorEx, lb: DcMotorEx, rf: DcMotorEx, rb: DcMotorEx): MecanumDriveLocalizer

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(lfDirection: DcMotorSimple.Direction, lbDirection: DcMotorSimple.Direction, rfDirection: DcMotorSimple.Direction, rbDirection: DcMotorSimple.Direction): MecanumDriveLocalizer

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(lfName: String, lbName: String, rfName: String, rbName: String): MecanumDriveLocalizer

Returns a new localizer with the given motor names.