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

[wpilib] Check for signedness in ArcadeDriveIK() (#4028)

Browse Source 
If xSpeed == -0.0 and zRotation > 0, the algorithm assumes it's in the
third quadrant instead of the first since +0.0 == -0.0.

Also added tests for inverse kinematic functions, fixed some
MecanumDrive test bugs, and added Java MecanumDrive.driveCartesianIK()
and KilloughDrive.driveCartesianIK() overloads with defaulted gyro angle
that C++ already had.

Fixes #4022.
This commit is contained in:
Tyler Veness
2022-02-17 18:03:59 -08:00
committed by GitHub
parent a19d1133b1
commit 49adac9564
10 changed files with 844 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -112,9 +112,10 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
if (xSpeed >= 0.0) {
// Sign is used because `xSpeed >= 0.0` succeeds for -0.0
if (!std::signbit(xSpeed)) {
// First quadrant, else second quadrant
if (zRotation >= 0.0) {
if (!std::signbit(zRotation)) {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
} else {
@@ -123,7 +124,7 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
}
} else {
// Third quadrant, else fourth quadrant
if (zRotation >= 0.0) {
if (!std::signbit(zRotation)) {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
} else {