mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
34 lines
1.1 KiB
C++
34 lines
1.1 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cmath>
|
|
|
|
#include "units/math.h"
|
|
|
|
using namespace frc;
|
|
|
|
void MecanumDriveWheelSpeeds::Desaturate(
|
|
units::meters_per_second_t attainableMaxSpeed) {
|
|
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
|
|
rearLeft, rearRight};
|
|
units::meters_per_second_t realMaxSpeed = units::math::abs(*std::max_element(
|
|
wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
|
|
return units::math::abs(a) < units::math::abs(b);
|
|
}));
|
|
|
|
if (realMaxSpeed > attainableMaxSpeed) {
|
|
for (int i = 0; i < 4; ++i) {
|
|
wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
|
|
}
|
|
frontLeft = wheelSpeeds[0];
|
|
frontRight = wheelSpeeds[1];
|
|
rearLeft = wheelSpeeds[2];
|
|
rearRight = wheelSpeeds[3];
|
|
}
|
|
}
|