Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use immutable member functions in ChassisSpeeds (#7545)

Browse Source
This commit is contained in:
Tyler Veness
2024-12-15 16:09:34 -08:00
committed by GitHub
parent 945d416d07
commit 03f0fc4dea
24 changed files with 163 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
View File

@@ -4,6 +4,8 @@
#include "Drivetrain.h"
#include <frc/kinematics/ChassisSpeeds.h>
frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
@@ -51,13 +53,12 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
auto wheelSpeeds = m_kinematics.ToWheelSpeeds(frc::ChassisSpeeds::Discretize(
fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
: frc::ChassisSpeeds{xSpeed, ySpeed, rot},
period));
wheelSpeeds.Desaturate(kMaxSpeed);
SetSpeeds(wheelSpeeds);
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_gyro.GetRotation2d());
}
SetSpeeds(m_kinematics.ToWheelSpeeds(chassisSpeeds.Discretize(period))
.Desaturate(kMaxSpeed));
}
void Drivetrain::UpdateOdometry() {