###### 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**== ```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++**== ```cpp= // 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**== ```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++**== ```cpp= // 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**== ```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++**== ```cpp= // 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**== ```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++**== ```cpp= // 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)