mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
f405582f86
Co-authored-by: Tyler Veness <calcmogul@gmail.com> Co-authored-by: Matt <matthew.morley.ca@gmail.com>
27 lines
1.2 KiB
C++
27 lines
1.2 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
|
|
|
using namespace frc;
|
|
|
|
DifferentialDriveKinematics::DifferentialDriveKinematics(
|
|
units::meter_t trackWidth)
|
|
: m_trackWidth(trackWidth) {}
|
|
|
|
ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds(
|
|
const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
|
|
return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
|
|
(wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
|
|
}
|
|
|
|
DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds(
|
|
const ChassisSpeeds& chassisSpeeds) const {
|
|
return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
|
|
chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
|
|
}
|