WPILibC++  2020.3.2
DifferentialDriveKinematics.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <hal/FRCUsageReporting.h>
11 #include <units/units.h>
12 
13 #include "frc/kinematics/ChassisSpeeds.h"
14 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
15 
16 namespace frc {
26  public:
35  explicit DifferentialDriveKinematics(units::meter_t trackWidth)
36  : trackWidth(trackWidth) {
37  HAL_Report(HALUsageReporting::kResourceType_Kinematics,
38  HALUsageReporting::kKinematics_DifferentialDrive);
39  }
40 
49  const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
50  return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
51  (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
52  }
53 
63  const ChassisSpeeds& chassisSpeeds) const {
64  return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
65  chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
66  }
67 
68  units::meter_t trackWidth;
69 };
70 } // namespace frc
frc::DifferentialDriveKinematics::ToWheelSpeeds
constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(const ChassisSpeeds &chassisSpeeds) const
Returns left and right component velocities from a chassis speed using inverse kinematics.
Definition: DifferentialDriveKinematics.h:62
frc::DifferentialDriveWheelSpeeds::right
units::meters_per_second_t right
Speed of the right side of the robot.
Definition: DifferentialDriveWheelSpeeds.h:25
frc::DifferentialDriveKinematics::ToChassisSpeeds
constexpr ChassisSpeeds ToChassisSpeeds(const DifferentialDriveWheelSpeeds &wheelSpeeds) const
Returns a chassis speed from left and right component velocities using forward kinematics.
Definition: DifferentialDriveKinematics.h:48
frc::ChassisSpeeds
Represents the speed of a robot chassis.
Definition: ChassisSpeeds.h:26
frc::DifferentialDriveWheelSpeeds
Represents the wheel speeds for a differential drive drivetrain.
Definition: DifferentialDriveWheelSpeeds.h:16
frc::ChassisSpeeds::omega
units::radians_per_second_t omega
Represents the angular velocity of the robot frame.
Definition: ChassisSpeeds.h:40
frc::DifferentialDriveKinematics
Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel velo...
Definition: DifferentialDriveKinematics.h:25
frc::DifferentialDriveKinematics::DifferentialDriveKinematics
DifferentialDriveKinematics(units::meter_t trackWidth)
Constructs a differential drive kinematics object.
Definition: DifferentialDriveKinematics.h:35
frc::DifferentialDriveWheelSpeeds::left
units::meters_per_second_t left
Speed of the left side of the robot.
Definition: DifferentialDriveWheelSpeeds.h:20
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::ChassisSpeeds::vx
units::meters_per_second_t vx
Represents forward velocity w.r.t the robot frame of reference.
Definition: ChassisSpeeds.h:30