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

[wpilib] Make drive classes follow NWU axes convention (#4079)

Browse Source 
All trigonometric functions and vector classes assume North-West-Up axes
convention, so using North-East-Down convention with them is really
error-prone. We've broken something every time we touched the drive
classes.

We originally used North-East-Down to match the joystick convention, but
the volume of long-lived bugs has made this not worth it in retrospect.

The rest of WPILib also uses North-West-Up, so this makes things
consistent.

KilloughDrive was removed since no one uses it.
This commit is contained in:
Tyler Veness
2022-10-27 21:59:11 -07:00
committed by GitHub
parent 9e22ffbebf
commit 66157397c1
24 changed files with 264 additions and 1463 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
View File

@@ -98,39 +98,19 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
zRotation = std::copysign(zRotation * zRotation, zRotation);
}
double leftSpeed;
double rightSpeed;
double leftSpeed = xSpeed - zRotation;
double rightSpeed = xSpeed + zRotation;
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
// Sign is used because `xSpeed >= 0.0` succeeds for -0.0
if (!std::signbit(xSpeed)) {
// First quadrant, else second quadrant
if (!std::signbit(zRotation)) {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
} else {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
}
} else {
// Third quadrant, else fourth quadrant
if (!std::signbit(zRotation)) {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
} else {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
}
}
// Normalize the wheel speeds
double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
if (maxMagnitude > 1.0) {
leftSpeed /= maxMagnitude;
rightSpeed /= maxMagnitude;
// Find the maximum possible value of (throttle + turn) along the vector that
// the joystick is pointing, then desaturate the wheel speeds
double greaterInput = std::max(std::abs(xSpeed), std::abs(zRotation));
double lesserInput = std::min(std::abs(xSpeed), std::abs(zRotation));
if (greaterInput == 0.0) {
return {0.0, 0.0};
}
double saturatedInput = (greaterInput + lesserInput) / greaterInput;
leftSpeed /= saturatedInput;
rightSpeed /= saturatedInput;
return {leftSpeed, rightSpeed};
}
@@ -144,14 +124,14 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::CurvatureDriveIK(
double rightSpeed = 0.0;
if (allowTurnInPlace) {
leftSpeed = xSpeed + zRotation;
rightSpeed = xSpeed - zRotation;
leftSpeed = xSpeed - zRotation;
rightSpeed = xSpeed + zRotation;
} else {
leftSpeed = xSpeed + std::abs(xSpeed) * zRotation;
rightSpeed = xSpeed - std::abs(xSpeed) * zRotation;
leftSpeed = xSpeed - std::abs(xSpeed) * zRotation;
rightSpeed = xSpeed + std::abs(xSpeed) * zRotation;
}
// Normalize wheel speeds
// Desaturate wheel speeds
double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
if (maxMagnitude > 1.0) {
leftSpeed /= maxMagnitude;