WPILibC++ 2024.3.2
DifferentialDriveKinematics.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <wpi/SymbolExports.h>
8
11
12
14#include "units/angle.h"
15#include "units/length.h"
16#include "wpimath/MathShared.h"
17
18namespace frc {
19/**
20 * Helper class that converts a chassis velocity (dx and dtheta components) to
21 * left and right wheel velocities for a differential drive.
22 *
23 * Inverse kinematics converts a desired chassis speed into left and right
24 * velocity components whereas forward kinematics converts left and right
25 * component velocities into a linear and angular chassis speed.
26 */
28 : public Kinematics<DifferentialDriveWheelSpeeds,
29 DifferentialDriveWheelPositions> {
30 public:
31 /**
32 * Constructs a differential drive kinematics object.
33 *
34 * @param trackWidth The track width of the drivetrain. Theoretically, this is
35 * the distance between the left wheels and right wheels. However, the
36 * empirical value may be larger than the physical measured value due to
37 * scrubbing effects.
38 */
39 explicit DifferentialDriveKinematics(units::meter_t trackWidth)
40 : trackWidth(trackWidth) {
43 }
44
45 /**
46 * Returns a chassis speed from left and right component velocities using
47 * forward kinematics.
48 *
49 * @param wheelSpeeds The left and right velocities.
50 * @return The chassis speed.
51 */
53 const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
54 return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
55 (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
56 }
57
58 /**
59 * Returns left and right component velocities from a chassis speed using
60 * inverse kinematics.
61 *
62 * @param chassisSpeeds The linear and angular (dx and dtheta) components that
63 * represent the chassis' speed.
64 * @return The left and right velocities.
65 */
67 const ChassisSpeeds& chassisSpeeds) const override {
68 return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
69 chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
70 }
71
72 /**
73 * Returns a twist from left and right distance deltas using
74 * forward kinematics.
75 *
76 * @param leftDistance The distance measured by the left encoder.
77 * @param rightDistance The distance measured by the right encoder.
78 * @return The resulting Twist2d.
79 */
80 constexpr Twist2d ToTwist2d(const units::meter_t leftDistance,
81 const units::meter_t rightDistance) const {
82 return {(leftDistance + rightDistance) / 2, 0_m,
83 (rightDistance - leftDistance) / trackWidth * 1_rad};
84 }
85
87 const DifferentialDriveWheelPositions& end) const override {
89 }
90
91 /// Differential drive trackwidth.
92 units::meter_t trackWidth;
93};
94} // namespace frc
95
#define WPILIB_DLLEXPORT
Definition: SymbolExports.h:36
Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel velo...
Definition: DifferentialDriveKinematics.h:29
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds) const override
Returns left and right component velocities from a chassis speed using inverse kinematics.
Definition: DifferentialDriveKinematics.h:66
units::meter_t trackWidth
Differential drive trackwidth.
Definition: DifferentialDriveKinematics.h:92
Twist2d ToTwist2d(const DifferentialDriveWheelPositions &start, const DifferentialDriveWheelPositions &end) const override
Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions.
Definition: DifferentialDriveKinematics.h:86
constexpr ChassisSpeeds ToChassisSpeeds(const DifferentialDriveWheelSpeeds &wheelSpeeds) const override
Returns a chassis speed from left and right component velocities using forward kinematics.
Definition: DifferentialDriveKinematics.h:52
DifferentialDriveKinematics(units::meter_t trackWidth)
Constructs a differential drive kinematics object.
Definition: DifferentialDriveKinematics.h:39
constexpr Twist2d ToTwist2d(const units::meter_t leftDistance, const units::meter_t rightDistance) const
Returns a twist from left and right distance deltas using forward kinematics.
Definition: DifferentialDriveKinematics.h:80
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual wheel s...
Definition: Kinematics.h:23
static void ReportUsage(MathUsageId id, int count)
Definition: MathShared.h:73
Definition: AprilTagPoseEstimator.h:15
Represents the speed of a robot chassis.
Definition: ChassisSpeeds.h:25
units::meters_per_second_t vx
Velocity along the x-axis.
Definition: ChassisSpeeds.h:29
Represents the angular velocity of the robot frame.
Definition: ChassisSpeeds.h:39
Represents the wheel positions for a differential drive drivetrain.
Definition: DifferentialDriveWheelPositions.h:16
units::meter_t left
Distance driven by the left side.
Definition: DifferentialDriveWheelPositions.h:20
units::meter_t right
Distance driven by the right side.
Definition: DifferentialDriveWheelPositions.h:25
Represents the wheel speeds for a differential drive drivetrain.
Definition: DifferentialDriveWheelSpeeds.h:15
units::meters_per_second_t left
Speed of the left side of the robot.
Definition: DifferentialDriveWheelSpeeds.h:19
units::meters_per_second_t right
Speed of the right side of the robot.
Definition: DifferentialDriveWheelSpeeds.h:24
A change in distance along a 2D arc since the last pose update.
Definition: Twist2d.h:21