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

[wpimath] Move RobotDriveBase::ApplyDeadband() to MathUtil (#3529)

Browse Source 
It's a useful function outside of the drive classes.

For backwards compatibility, deprecate (rather than remove) RobotDriveBase.applyDeadband()
This commit is contained in:
Tyler Veness
2021-08-28 20:52:05 -07:00
committed by GitHub
parent 3b5d0d141a
commit aa3848b2c8
16 changed files with 162 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,6 +11,7 @@
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/MathUtil.h"
#include "frc/SpeedController.h"
using namespace frc;

1
wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
View File

@@ -12,6 +12,7 @@
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/MathUtil.h"
#include "frc/SpeedController.h"
using namespace frc;

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

@@ -12,6 +12,7 @@
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/MathUtil.h"
#include "frc/SpeedController.h"
#include "frc/drive/Vector2d.h"

11
wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
View File

@@ -10,6 +10,7 @@
#include <hal/FRCUsageReporting.h>
#include "frc/MathUtil.h"
#include "frc/motorcontrol/MotorController.h"
using namespace frc;
@@ -31,15 +32,7 @@ void RobotDriveBase::FeedWatchdog() {
}
double RobotDriveBase::ApplyDeadband(double value, double deadband) {
if (std::abs(value) > deadband) {
if (value > 0.0) {
return (value - deadband) / (1.0 - deadband);
} else {
return (value + deadband) / (1.0 - deadband);
}
} else {
return 0.0;
}
return frc::ApplyDeadband(value, deadband);
}
void RobotDriveBase::Normalize(wpi::span<double> wheelSpeeds) {

5
wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
View File

@@ -7,6 +7,7 @@
#include <memory>
#include <string>
#include <wpi/deprecated.h>
#include <wpi/span.h>
#include "frc/MotorSafety.h"
@@ -42,7 +43,7 @@ class RobotDriveBase : public MotorSafety {
*
* The default value is 0.02. Inputs smaller than the deadband are set to 0.0
* while inputs larger than the deadband are scaled from 0.0 to 1.0. See
* ApplyDeadband().
* frc::ApplyDeadband().
*
* @param deadband The deadband to set.
*/
@@ -75,7 +76,9 @@ class RobotDriveBase : public MotorSafety {
*
* @param value value to clip
* @param deadband range around zero
* @deprecated Use ApplyDeadband() in frc/MathUtil.h.
*/
WPI_DEPRECATED("Use ApplyDeadband() in frc/MathUtil.h")
static double ApplyDeadband(double number, double deadband);
/**

10
wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
View File

@@ -2,6 +2,7 @@
// 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/MathUtil.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#include <frc/filter/SlewRateLimiter.h>
@@ -30,20 +31,23 @@ class Robot : public frc::TimedRobot {
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
const auto xSpeed = -m_xspeedLimiter.Calculate(
frc::ApplyDeadband(m_controller.GetLeftY(), 0.02)) *
Drivetrain::kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
const auto ySpeed = -m_yspeedLimiter.Calculate(
frc::ApplyDeadband(m_controller.GetLeftX(), 0.02)) *
Drivetrain::kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
const auto rot = -m_rotLimiter.Calculate(
frc::ApplyDeadband(m_controller.GetRightX(), 0.02)) *
Drivetrain::kMaxAngularSpeed;
m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);

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

@@ -170,8 +170,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
m_reported = true;
}
xSpeed = applyDeadband(xSpeed, m_deadband);
zRotation = applyDeadband(zRotation, m_deadband);
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
@@ -203,8 +203,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
m_reported = true;
}
xSpeed = applyDeadband(xSpeed, m_deadband);
zRotation = applyDeadband(zRotation, m_deadband);
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
@@ -241,8 +241,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
m_reported = true;
}
leftSpeed = applyDeadband(leftSpeed, m_deadband);
rightSpeed = applyDeadband(rightSpeed, m_deadband);
leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);

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

@@ -191,8 +191,8 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
m_reported = true;
}
ySpeed = applyDeadband(ySpeed, m_deadband);
xSpeed = applyDeadband(xSpeed, m_deadband);
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);

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

@@ -169,8 +169,8 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
m_reported = true;
}
ySpeed = applyDeadband(ySpeed, m_deadband);
xSpeed = applyDeadband(xSpeed, m_deadband);
ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);

15
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.drive;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.MotorSafety;
/** Common base class for drive platforms. */
@@ -41,7 +42,7 @@ public abstract class RobotDriveBase extends MotorSafety {
*
* <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
* 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link
* #applyDeadband}.
* edu.wpi.first.math.MathUtil#applyDeadband}.
*
* @param deadband The deadband to set.
*/
@@ -83,17 +84,11 @@ public abstract class RobotDriveBase extends MotorSafety {
* @param value value to clip
* @param deadband range around zero
* @return The value after the deadband is applied.
* @deprecated Use MathUtil.applyDeadband(double,double).
*/
@Deprecated(since = "2021", forRemoval = true)
protected static double applyDeadband(double value, double deadband) {
if (Math.abs(value) > deadband) {
if (value > 0.0) {
return (value - deadband) / (1.0 - deadband);
} else {
return (value + deadband) / (1.0 - deadband);
}
} else {
return 0.0;
}
return MathUtil.applyDeadband(value, deadband);
}
/**

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.examples.swervebot;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
@@ -31,18 +32,24 @@ public class Robot extends TimedRobot {
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
// negative values when we push forward.
final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
final var xSpeed =
-m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
* Drivetrain.kMaxSpeed;
// Get the y speed or sideways/strafe speed. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
final var ySpeed =
-m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
* Drivetrain.kMaxSpeed;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
final var rot =
-m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
* Drivetrain.kMaxAngularSpeed;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
}

20
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -33,6 +33,26 @@ public final class MathUtil {
return Math.max(low, Math.min(value, high));
}
/**
* Returns 0.0 if the given value is within the specified range around zero. The remaining range
* between the deadband and 1.0 is scaled from 0.0 to 1.0.
*
* @param value Value to clip.
* @param deadband Range around zero.
* @return The value after the deadband is applied.
*/
public static double applyDeadband(double value, double deadband) {
if (Math.abs(value) > deadband) {
if (value > 0.0) {
return (value - deadband) / (1.0 - deadband);
} else {
return (value + deadband) / (1.0 - deadband);
}
} else {
return 0.0;
}
}
/**
* Returns modulus of input.
*

23
wpimath/src/main/native/cpp/MathUtil.cpp Normal file
View File

@@ -0,0 +1,23 @@
// 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/MathUtil.h"
#include <cmath>
namespace frc {
double ApplyDeadband(double value, double deadband) {
if (std::abs(value) > deadband) {
if (value > 0.0) {
return (value - deadband) / (1.0 - deadband);
} else {
return (value + deadband) / (1.0 - deadband);
}
} else {
return 0.0;
}
}
} // namespace frc

9
wpimath/src/main/native/include/frc/MathUtil.h
View File

@@ -10,6 +10,15 @@
namespace frc {
/**
* Returns 0.0 if the given value is within the specified range around zero.
* The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
*
* @param value Value to clip.
* @param deadband Range around zero.
*/
double ApplyDeadband(double value, double deadband);
/**
* Returns modulus of input.
*

18
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
View File

@@ -9,6 +9,24 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class MathUtilTest {
@Test
void testApplyDeadband() {
// < 0
assertEquals(-1.0, MathUtil.applyDeadband(-1.0, 0.02));
assertEquals((-0.03 + 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(-0.03, 0.02));
assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02));
assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02));
// == 0
assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02));
// > 0
assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02));
assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02));
assertEquals((0.03 - 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(0.03, 0.02));
assertEquals(1.0, MathUtil.applyDeadband(1.0, 0.02));
}
@Test
void testInputModulus() {
// These tests check error wrapping. That is, the result of wrapping the

80
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -7,57 +7,79 @@
#include "units/angle.h"
#define EXPECT_UNITS_EQ(a, b) \
EXPECT_FLOAT_EQ((a).to<double>(), (b).to<double>())
EXPECT_DOUBLE_EQ((a).to<double>(), (b).to<double>())
#define EXPECT_UNITS_NEAR(a, b, c) \
EXPECT_NEAR((a).to<double>(), (b).to<double>(), c)
TEST(MathUtilTest, ApplyDeadband) {
// < 0
EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
frc::ApplyDeadband(-0.03, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02));
// == 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02));
// > 0
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02));
EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02));
EXPECT_DOUBLE_EQ((0.03 - 0.02) / (1.0 - 0.02),
frc::ApplyDeadband(0.03, 0.02));
EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
}
TEST(MathUtilTest, InputModulus) {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.
// Test symmetric range
EXPECT_FLOAT_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
EXPECT_FLOAT_EQ(-20.0,
frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
EXPECT_FLOAT_EQ(-20.0,
frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
EXPECT_FLOAT_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
EXPECT_FLOAT_EQ(20.0,
frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
EXPECT_FLOAT_EQ(20.0,
frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0,
frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
EXPECT_DOUBLE_EQ(20.0,
frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
// Test range starting at zero
EXPECT_FLOAT_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
EXPECT_FLOAT_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
EXPECT_FLOAT_EQ(340.0,
frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
EXPECT_DOUBLE_EQ(340.0,
frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
// Test asymmetric range that doesn't start at zero
EXPECT_FLOAT_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
// Test range with both positive endpoints
EXPECT_FLOAT_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
EXPECT_FLOAT_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
// Test all supported types
EXPECT_FLOAT_EQ(-20.0,
frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_DOUBLE_EQ(-20.0,
frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
EXPECT_EQ(-20, frc::InputModulus<int>(170 - (-170), -170, 190));
EXPECT_EQ(-20_deg, frc::InputModulus<units::degree_t>(170_deg - (-170_deg),
-170_deg, 190_deg));
}
TEST(MathUtilTest, AngleModulus) {
EXPECT_UNITS_EQ(
EXPECT_UNITS_NEAR(
frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}),
units::radian_t{160 * wpi::numbers::pi / 180});
EXPECT_UNITS_EQ(
units::radian_t{160 * wpi::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(
frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}),
units::radian_t{-2 * wpi::numbers::pi / 180});
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
0_rad);
units::radian_t{-2 * wpi::numbers::pi / 180}, 1e-10);
EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
0_rad, 1e-10);
EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
units::radian_t(wpi::numbers::pi));