Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41: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

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
View File

@@ -280,9 +280,9 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
if (xSpeed >= 0.0) {
if (Double.compare(xSpeed, 0.0) >= 0) {
// First quadrant, else second quadrant
if (zRotation >= 0.0) {
if (Double.compare(zRotation, 0.0) >= 0) {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
} else {
@@ -291,7 +291,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
}
} else {
// Third quadrant, else fourth quadrant
if (zRotation >= 0.0) {
if (Double.compare(zRotation, 0.0) >= 0) {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
} else {

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
View File

@@ -233,6 +233,23 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
0.0);
}
/**
* Cartesian inverse kinematics for Killough platform.
*
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
* from its angle or rotation rate.
*
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
* positive.
* @return Wheel speeds [-1.0..1.0].
*/
@SuppressWarnings("ParameterName")
public WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation) {
return driveCartesianIK(ySpeed, xSpeed, zRotation, 0.0);
}
/**
* Cartesian inverse kinematics for Killough platform.
*

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
View File

@@ -209,6 +209,23 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
0.0);
}
/**
* Cartesian inverse kinematics for Mecanum platform.
*
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
* from its angle or rotation rate.
*
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
* positive.
* @return Wheel speeds [-1.0..1.0].
*/
@SuppressWarnings("ParameterName")
public static WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation) {
return driveCartesianIK(ySpeed, xSpeed, zRotation, 0.0);
}
/**
* Cartesian inverse kinematics for Mecanum platform.
*