Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
View File

@@ -49,7 +49,8 @@ MecanumDrive::MecanumDrive(std::function<void(double)> frontLeftMotor,
}
void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed,
double zRotation, wpi::math::Rotation2d gyroAngle) {
double zRotation,
wpi::math::Rotation2d gyroAngle) {
if (!reported) {
HAL_ReportUsage("RobotDrive", "MecanumCartesian");
reported = true;
@@ -99,17 +100,17 @@ void MecanumDrive::StopMotor() {
Feed();
}
MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double xSpeed,
double ySpeed,
double zRotation,
wpi::math::Rotation2d gyroAngle) {
MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(
double xSpeed, double ySpeed, double zRotation,
wpi::math::Rotation2d gyroAngle) {
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
// Compensate for gyro angle.
auto input =
wpi::math::Translation2d{wpi::units::meter_t{xSpeed}, wpi::units::meter_t{ySpeed}}.RotateBy(
-gyroAngle);
auto input = wpi::math::Translation2d{
wpi::units::meter_t{xSpeed},
wpi::units::meter_t{
ySpeed}}.RotateBy(-gyroAngle);
double wheelSpeeds[4];
wheelSpeeds[kFrontLeft] = input.X().value() + input.Y().value() + zRotation;