###### tags: `程式組教程`

# Mecanum Drive Kinematics

The **MecanumDriveKinematics** class 45 is a useful tool that ==converts== between a **ChassisSpeeds** object and a **MecanumDriveWheelSpeeds** object, which contains ==velocities== for each of the four wheels on a mecanum drive.

---

:::spoiler Chinese

> converts ➨ 轉換
> velocities ➨ 速度

:::

---

# ==Constructing== the Kinematics Object

The **MecanumDriveKinematics** class accepts four ==constructor== ==arguments==, with each argument being the location of a wheel relative to the robot center（as a **Translation2d**）. The ==order== for the arguments is front left, front right, back left, and back right. The locations for the wheels must be ==relative to== the center of the robot. Positive x values ==represent== moving toward the front of the robot ==whereas== positive y values ==represent== moving toward the left of the robot.

---

:::spoiler Chinese

> Constructing ➨ 建構
> constructor ➨ 構造函數
> arguments ➨ 參數
> order ➨ 指令
> relative to ➨ 關係到；連接到
> represent ➨ 代表
> whereas ➨ 然而
>
:::

---

==**API**==

Constructor and Description：

  Translation2d( double x, double y )
  
Explanation：

  The X and Y values are derived from
  their respective relative distances from the chassis
  i.e. Assume the center of the site as the origin
  
----------

Constructor：

  MecanumDriveKinematics(
    Translation2d frontLeftWheelMeters,
    Translation2d frontRightWheelMeters,
    Translation2d rearLeftWheelMeters,
    Translation2d rearRightWheelMeters
  )
  
Description：

  Constructs a mecanum drive kinematics object.

==**JAVA**==

// 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
);

==**C++**==

// Locations of the wheels relative to the robot center.
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};

// Creating my kinematics object using the wheel locations.
frc::MecanumDriveKinematics m_kinematics{
  m_frontLeftLocation,
  m_frontRightLocation,
  m_backLeftLocation,
  m_backRightLocation
};

# Converting Chassis Speeds to Wheel Speeds

The **toWheelSpeeds（ChassisSpeeds speed）**（Java） / **ToWheelSpeeds（ChassisSpeeds speeds）** （C++） method should be used to ==convert== a **ChassisSpeeds** object to a **MecanumDriveWheelSpeeds** object. This is useful in ==situations== where you have to convert a forward velocity, sideways velocity, and an ==angular velocity== into ==individual== wheel speeds.

---

:::spoiler Chinese

> convert ➨ 改變；轉變
> situations ➨ 情況
> angular velocity ➨ 角速度
> individual ➨ 單一的

:::

---

==**API**==

Constructor：

  ChassisSpeeds(
    double vxMetersPerSecond,
    double vyMetersPerSecond, 
    double omegaRadiansPerSecond
  )

Description：

  Constructs a ChassisSpeeds object.

==**JAVA**==

// Example chassis speeds：
// 1 meter per second forward, 3 meters per second to the left,
// and rotation at 1.5 radians per second counterclockwise.
ChassisSpeeds speeds = new ChassisSpeeds(1.0, 3.0, 1.5);

// Convert to wheel speeds
MecanumDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds);

// Get the individual wheel speeds
double frontLeft = wheelSpeeds.frontLeftMetersPerSecond
double frontRight = wheelSpeeds.frontRightMetersPerSecond
double backLeft = wheelSpeeds.rearLeftMetersPerSecond
double backRight = wheelSpeeds.rearRightMetersPerSecond

==**C++**==

// Example chassis speeds：
// 1 meter per second forward, 3 meters per second to the left,
// and rotation at 1.5 radians per second counterclockwise.
frc::ChassisSpeeds speeds{ 1_mps, 3_mps, 1.5_rad_per_s };

// Convert to wheel speeds.
// Here, we can use C++17's structuredbindings feature
// to automatically split up the MecanumDriveWheelSpeeds
// struct into it's individual components
auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(speeds);

# Field-oriented drive

[Recall](https://hackmd.io/@FRC-7130-4th/rkgRtJHeq) that a **ChassisSpeeds** object can be created from a set of ==desired== ==field-oriented== speeds. This feature can be used to get wheel speeds from ==a set of== desired field-oriented speeds.

---

:::spoiler Chinese

> desired ➨ 所需的
> field-oriented ➨ 向量控制
> a set of ➨ 一系列的

:::

---

==**API**==

Modifier and Type：

  static ChassisSpeeds

Method：

  fromFieldRelativeSpeeds(
    double vxMetersPerSecond, 
    double vyMetersPerSecond, 
    double omegaRadiansPerSecond, 
    Rotation2d robotAngle
  )

Description：

  Constructs a ChassisSpeeds object.

==**JAVA**==

// The desired field relative speed here is：
// 2 meters per second toward the opponent's alliance station wall,
// and 2 meters per second toward the left field boundary.
// The desired rotation is
// a quarter of a rotation per second counterclockwise.
// The current robot angle is 45 degrees.
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
  2.0, 2.0, Math.PI / 2.0, Rotation2d.fromDegrees(45.0));

// Now use this in our kinematics
MecanumDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(speeds);

==**C++**==

// The desired field relative speed here is：
// 2 meters per second toward the opponent's alliance station wall,
// and 2 meters per second toward the left field boundary.
// The desired rotation is
// a quarter of a rotation per second counterclockwise. 
// The current robot angle is 45 degrees.
frc::ChassisSpeeds speeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
  2_mps, 2_mps,
  units::radians_per_second_t(wpi::math::pi / 2.0),
  Rotation2d(45_deg)
);

// Now use this in our kinematics
auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(speeds);

# Using custom centers of rotation

Sometimes, ==rotating== around one ==specific== corner might be ==desirable== for certain ==evasive maneuvers==. This type of ==behavior== is also ==supported by== the WPILib classes. The same **ToWheelSpeeds()** method accepts a second ==parameter== for the center of rotation （as a **Translation2d**）. Just like the wheel locations, the **Translation2d** ==representing== the center of ==rotation== should be ==relative to== the robot center.

:::info
**Note：**

Because all robots are a ==rigid frame==, the ==provided== **vx** and **vy** velocities from the **ChassisSpeeds** object will still ==apply for== the ==entirety== of the robot. However, the **omega** from the **ChassisSpeeds** object will be measured from the center of rotation.
:::

For example, one can set the center of rotation on a certain wheel and if the ==provided== **ChassisSpeeds** object has a **vx** and **vy** of zero and a ==non-zero== **omega**, the robot will ==appear to== rotate around that ==particular== wheel.

---

:::spoiler Chinese

> rotating ➨ 旋轉
> specific ➨ 特定的
> desirable ➨ 可取的
> evasive maneuvers ➨ 規避動作
> behavior ➨ 行為
> supported by ➨ 支持
> parameter ➨ 範圍
> representing ➨ 代表
> rotation ➨ 旋轉
> relative to ➨ 相對於
> rigid frame ➨ 難以調整的框架
> provided ➨ 提供
> apply for ➨ 適用於
> entirety ➨ 整體的；整個的
> provided ➨ 提供
> non-zero ➨ 非零的
> appear to ➨ 可能
> particular ➨ 特定的

:::

---

# Converting wheel speeds to chassis speeds

One can also use the kinematics object to convert a **MecanumDriveWheelSpeeds** object to a ==singular== **ChassisSpeeds** object. The **toChassisSpeeds（MecanumDriveWheelSpeeds speeds）** （Java） / **ToChassisSpeeds（MecanumDriveWheelSpeeds speeds）** （C++） method can be used to ==achieve== this.

---

:::spoiler Chinese

> singular ➨ 單數的
> achieve ➨ 實現

:::

---

==**API**==

Constructor：

  MecanumDriveWheelSpeeds(
    frontLeftMetersPerSecond, 
    frontRightMetersPerSecond, 
    rearLeftMetersPerSecond, 
    rearRightMetersPerSecond
  )

==**JAVA**==

// Example wheel speeds
var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.67, 20.51, -13.44, 16.26);

// Convert to chassis speeds
ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds);

// Getting individual speeds
double forward = chassisSpeeds.vxMetersPerSecond;
double sideways = chassisSpeeds.vyMetersPerSecond;
double angular = chassisSpeeds.omegaRadiansPerSecond;

==**C++**==

// Example wheel speeds
frc::MecanumDriveWheelSpeeds wheelSpeeds{
  -17.67_mps, 
  20.51_mps, 
  -13.44_mps, 
  16.26_mps
};

// Convert to chassis speeds. Here, we can use C++17's structured bindings
// feature to automatically break up the ChassisSpeeds struct into its
// three components.
auto [forward, sideways, angular] = kinematics.ToChassisSpeeds(wheelSpeeds);

# Original link and Reference website

* [FRC Official - Mecanum Drive Kinematics](https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/mecanum-drive-kinematics.html)
* [API - Translation2d](https://www.team2052.com/KnightLib/index.html?com/first/team2052/lib/vec/Translation2d.html)
* [API - ChassisSpeeds](https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/math/kinematics/ChassisSpeeds.html)
* [API - MecanumDriveKinematics](https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.html)
* [Hackmd（Moyu） - Introduction to Kinematics and The Chassis Speeds Class](https://hackmd.io/@Mo-Yu/HJfJHxZxc)
* [Hackmd（7130） - Introduction to Kinematics and The Chassis Speeds Class](https://hackmd.io/@FRC-7130-4th/rkgRtJHeq)