麦克纳姆驱动测距法

用户可以使用麦克纳姆驱动器运动学类来执行 :ref odometry <docs/software/kinematics-and-odometry/intro-and-chassis-speeds:What is odometry?>。 WPILib包含一个``MecanumDriveOdometry’’类，可用于跟踪麦克纳姆驱动机器人在现场的位置。

备注

由于这种方法只使用编码器和陀螺仪，机器人在场上的位置估计会随着时间的推移而漂移，特别是当你的机器人在游戏过程中与其他机器人接触时。不过，在自主期，测距法通常非常准确。

创建测距法对象

The MecanumDriveOdometry class constructor requires three mandatory arguments and one optional argument.

The mandatory arguments are:

• The kinematics object that represents your mecanum drive (as a MecanumDriveKinematics instance)

• The angle reported by your gyroscope (as a Rotation2d)

• The initial positions of the wheels (as MecanumDriveWheelPositions). In Java, this must be constructed with each wheel position in meters. In C++, the units library must be used to represent your wheel positions.

The fourth optional argument is the starting pose of your robot on the field (as a Pose2d). By default, the robot will start at x = 0, y = 0, theta = 0.

备注

0 degrees / radians represents the robot angle when the robot is facing directly toward your opponent’s alliance station. As your robot turns to the left, your gyroscope angle should increase. The Gyro interface supplies getRotation2d/GetRotation2d that you can use for this purpose. See Field Coordinate System for more information about the coordinate system.

JavaC++

// Locations of the wheels relative to the robot center.
Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);

// Creating my kinematics object using the wheel locations.
MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
  m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
);

// Creating my odometry object from the kinematics object and the initial wheel positions.
// Here, our starting pose is 5 meters along the long end of the field and in the
// center of the field along the short end, facing the opposing alliance wall.
MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(
  m_kinematics,
  m_gyro.getRotation2d(),
  new MecanumDriveWheelPositions(
    m_frontLeftEncoder.getDistance(), m_frontRightEncoder.getDistance(),
    m_backLeftEncoder.getDistance(), m_backRightEncoder.getDistance()
  ),
  new Pose2d(5.0, 13.5, new Rotation2d())
);

更新机器人姿势

The update method of the odometry class updates the robot position on the field. The update method takes in the gyro angle of the robot, along with a MecanumDriveWheelPositions object representing the position of each of the 4 wheels on the robot. This update method must be called periodically, preferably in the periodic() method of a Subsystem. The update method returns the new updated pose of the robot.

JavaC++

@Override
public void periodic() {
  // Get my wheel positions
  var wheelPositions = new MecanumDriveWheelPositions(
    m_frontLeftEncoder.getDistance(), m_frontRightEncoder.getDistance(),
    m_backLeftEncoder.getDistance(), m_backRightEncoder.getDistance());

  // Get the rotation of the robot from the gyro.
  var gyroAngle = m_gyro.getRotation2d();

  // Update the pose
  m_pose = m_odometry.update(gyroAngle, wheelPositions);
}

重置机器人姿势

The robot pose can be reset via the resetPosition method. This method accepts three arguments: the current gyro angle, the current wheel positions, and the new field-relative pose.

重要

If at any time, you decide to reset your gyroscope or encoders, the resetPosition method MUST be called with the new gyro angle and wheel positions.

备注

此处提供了具有测距法的麦克纳姆驱动机器人的完整示例：C ++ /Java。

此外，可以使用GetPose（C ++）/ getPoseMeters（Java）方法来检索当前的机器人姿态，而无需进行更新。