10 #include <hal/FRCUsageReporting.h>
11 #include <units/units.h>
13 #include "frc/kinematics/ChassisSpeeds.h"
14 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
36 : trackWidth(trackWidth) {
37 HAL_Report(HALUsageReporting::kResourceType_Kinematics,
38 HALUsageReporting::kKinematics_DifferentialDrive);
50 return {(wheelSpeeds.
left + wheelSpeeds.
right) / 2.0, 0_mps,
51 (wheelSpeeds.
right - wheelSpeeds.
left) / trackWidth * 1_rad};
64 return {chassisSpeeds.
vx - trackWidth / 2 * chassisSpeeds.
omega / 1_rad,
65 chassisSpeeds.
vx + trackWidth / 2 * chassisSpeeds.
omega / 1_rad};
68 units::meter_t trackWidth;
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds) const
Returns left and right component velocities from a chassis speed using inverse kinematics.
Definition: DifferentialDriveKinematics.h:62
units::meters_per_second_t right
Speed of the right side of the robot.
Definition: DifferentialDriveWheelSpeeds.h:25
constexpr ChassisSpeeds ToChassisSpeeds(const DifferentialDriveWheelSpeeds &wheelSpeeds) const
Returns a chassis speed from left and right component velocities using forward kinematics.
Definition: DifferentialDriveKinematics.h:48
Represents the speed of a robot chassis.
Definition: ChassisSpeeds.h:26
Represents the wheel speeds for a differential drive drivetrain.
Definition: DifferentialDriveWheelSpeeds.h:16
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition: ChassisSpeeds.h:40
Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel velo...
Definition: DifferentialDriveKinematics.h:25
DifferentialDriveKinematics(units::meter_t trackWidth)
Constructs a differential drive kinematics object.
Definition: DifferentialDriveKinematics.h:35
units::meters_per_second_t left
Speed of the left side of the robot.
Definition: DifferentialDriveWheelSpeeds.h:20
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
units::meters_per_second_t vx
Represents forward velocity w.r.t the robot frame of reference.
Definition: ChassisSpeeds.h:30