差速驱动测距法

用户可以使用差速驱动运动学类来执行:ref:odometry <docs/software/kinematics-and-odometry/intro-and-chassis-speeds:What is odmetry?>。WPILib中包含了一个``差动驱动里程测量``类，可以用来跟踪差动驱动机器人在现场的位置。

备注

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

创建Odometry对象

The DifferentialDriveOdometry class constructor requires three mandatory arguments and one optional argument. The mandatory arguments are:

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

  • The initial left and right encoder readings. In Java / Python, these are a number that represents the distance traveled by each side in meters. In C++, the units library must be used to represent your wheel positions.

The 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 Coordinate System for more information about the coordinate system.

// Creating my odometry object. 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 forward.
DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
  m_gyro.getRotation2d(),
  m_leftEncoder.getDistance(), m_rightEncoder.getDistance(),
  new Pose2d(5.0, 13.5, new Rotation2d()));

// Creating my odometry object. 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 forward.
frc::DifferentialDriveOdometry m_odometry{
  m_gyro.GetRotation2d(),
  units::meter_t{m_leftEncoder.GetDistance()},
  units::meter_t{m_rightEncoder.GetDistance()},
  frc::Pose2d{5_m, 13.5_m, 0_rad}};

from wpimath.kinematics import DifferentialDriveOdometry
from wpimath.geometry import Pose2d
from wpimath.geometry import Rotation2d

# Creating my odometry object. 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 forward.
m_odometry = DifferentialDriveOdometry(
  m_gyro.getRotation2d(),
  m_leftEncoder.getDistance(), m_rightEncoder.getDistance(),
  Pose2d(5.0, 13.5, Rotation2d()))

更新机器人姿势

``update``方法可用于更新机器人在场地上的位置。该方法必须定期调用，最好是在:ref:`Subsystem <docs/software/commandbased/subsystems:Subsystems>`的``periodic()``方法中调用。``update``方法返回机器人新的更新姿势。该方法获取机器人的陀螺仪角度，以及左编码器距离和右编码器距离。

备注

If the robot is moving forward in a straight line, both distances (left and right) must be increasing positively – the rate of change must be positive.

@Override
public void periodic() {
  // Get the rotation of the robot from the gyro.
  var gyroAngle = m_gyro.getRotation2d();

  // Update the pose
  m_pose = m_odometry.update(gyroAngle,
    m_leftEncoder.getDistance(),
    m_rightEncoder.getDistance());
}

void Periodic() override {
  // Get the rotation of the robot from the gyro.
  frc::Rotation2d gyroAngle = m_gyro.GetRotation2d();

  // Update the pose
  m_pose = m_odometry.Update(gyroAngle,
    units::meter_t{m_leftEncoder.GetDistance()},
    units::meter_t{m_rightEncoder.GetDistance()});
}

def periodic(self):
  # Get the rotation of the robot from the gyro.
  gyroAngle = m_gyro.getRotation2d()

  # Update the pose
  m_pose = m_odometry.update(gyroAngle,
    m_leftEncoder.getDistance(),
    m_rightEncoder.getDistance())

重置机器人姿势

The robot pose can be reset via the resetPosition method. This method accepts four arguments: the current gyro angle, the left and right 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 distances.

备注

A full example of a differential drive robot with odometry is available here: C++ / Java / Python

In addition, the GetPose (C++) / getPoseMeters (Java / Python) methods can be used to retrieve the current robot pose without an update.