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

Add MathUtils.clamp() for Java (#1861)

Browse Source 
Also use std::clamp() and MathUtils.clamp() in as many places as
possible in place of custom clamp functions or if statements.
This commit is contained in:
Tyler Veness
2019-08-28 23:24:30 -07:00
committed by Peter Johnson
parent eb3e0c9c95
commit 1fb3011235
12 changed files with 75 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
hal/src/main/native/athena/PWM.cpp
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -266,11 +266,9 @@ void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
DigitalPort* dPort = port.get();
if (speed < -1.0) {
speed = -1.0;
} else if (speed > 1.0) {
speed = 1.0;
} else if (!std::isfinite(speed)) {
if (std::isfinite(speed)) {
speed = std::clamp(speed, -1.0, 1.0);
} else {
speed = 0.0;
}

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -36,10 +36,10 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
reported = true;
}
xSpeed = Limit(xSpeed);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
zRotation = Limit(zRotation);
zRotation = std::clamp(zRotation, -1.0, 1.0);
zRotation = ApplyDeadband(zRotation, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
@@ -75,9 +75,9 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
}
}
m_leftMotor.Set(Limit(leftMotorOutput) * m_maxOutput);
m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
m_rightSideInvertMultiplier);
m_leftMotor.Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
m_rightMotor.Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
Feed();
}
@@ -91,10 +91,10 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
reported = true;
}
xSpeed = Limit(xSpeed);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
zRotation = Limit(zRotation);
zRotation = std::clamp(zRotation, -1.0, 1.0);
zRotation = ApplyDeadband(zRotation, m_deadband);
double angularPower;
@@ -102,8 +102,9 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
if (isQuickTurn) {
if (std::abs(xSpeed) < m_quickStopThreshold) {
m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator +
m_quickStopAlpha * Limit(zRotation) * 2;
m_quickStopAccumulator =
(1 - m_quickStopAlpha) * m_quickStopAccumulator +
m_quickStopAlpha * std::clamp(zRotation, -1.0, 1.0) * 2;
}
overPower = true;
angularPower = zRotation;
@@ -164,10 +165,10 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
reported = true;
}
leftSpeed = Limit(leftSpeed);
leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
rightSpeed = Limit(rightSpeed);
rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control

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

@@ -51,10 +51,10 @@ void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
reported = true;
}
ySpeed = Limit(ySpeed);
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.

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

@@ -44,10 +44,10 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
reported = true;
}
ySpeed = Limit(ySpeed);
ySpeed = std::clamp(ySpeed, -1.0, 1.0);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
xSpeed = Limit(xSpeed);
xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -26,16 +26,6 @@ void RobotDriveBase::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
void RobotDriveBase::FeedWatchdog() { Feed(); }
double RobotDriveBase::Limit(double value) {
if (value > 1.0) {
return 1.0;
}
if (value < -1.0) {
return -1.0;
}
return value;
}
double RobotDriveBase::ApplyDeadband(double value, double deadband) {
if (std::abs(value) > deadband) {
if (value > 0.0) {

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -75,11 +75,6 @@ class RobotDriveBase : public MotorSafety, public SendableBase {
void GetDescription(wpi::raw_ostream& desc) const override = 0;
protected:
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
double Limit(double number);
/**
* 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.

15
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -11,6 +11,7 @@ import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.SendableBase;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpiutil.math.MathUtils;
/**
* Implements a PID control loop.
@@ -198,7 +199,7 @@ public class PIDController extends SendableBase {
*/
public void setSetpoint(double setpoint) {
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(setpoint, m_minimumInput, m_maximumInput);
m_setpoint = MathUtils.clamp(setpoint, m_minimumInput, m_maximumInput);
} else {
m_setpoint = setpoint;
}
@@ -257,7 +258,7 @@ public class PIDController extends SendableBase {
// Clamp setpoint to new input
if (m_maximumInput > m_minimumInput) {
m_setpoint = clamp(m_setpoint, m_minimumInput, m_maximumInput);
m_setpoint = MathUtils.clamp(m_setpoint, m_minimumInput, m_maximumInput);
}
}
@@ -375,11 +376,11 @@ public class PIDController extends SendableBase {
m_velocityError = (m_positionError - m_prevError) / m_period;
if (m_Ki != 0) {
m_totalError = clamp(m_totalError + m_positionError * m_period, m_minimumOutput / m_Ki,
m_maximumOutput / m_Ki);
m_totalError = MathUtils.clamp(m_totalError + m_positionError * m_period,
m_minimumOutput / m_Ki, m_maximumOutput / m_Ki);
}
return clamp(
return MathUtils.clamp(
m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError,
m_minimumOutput, m_maximumOutput);
}
@@ -421,8 +422,4 @@ public class PIDController extends SendableBase {
}
return error;
}
private static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
}

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,6 +15,7 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpiutil.math.MathUtils;
/**
* A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
@@ -176,10 +177,10 @@ public class DifferentialDrive extends RobotDriveBase {
m_reported = true;
}
xSpeed = limit(xSpeed);
xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
zRotation = limit(zRotation);
zRotation = MathUtils.clamp(zRotation, -1.0, 1.0);
zRotation = applyDeadband(zRotation, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
@@ -214,8 +215,9 @@ public class DifferentialDrive extends RobotDriveBase {
}
}
m_leftMotor.set(limit(leftMotorOutput) * m_maxOutput);
m_rightMotor.set(limit(rightMotorOutput) * m_maxOutput * m_rightSideInvertMultiplier);
m_leftMotor.set(MathUtils.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
m_rightMotor.set(MathUtils.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
feed();
}
@@ -242,10 +244,10 @@ public class DifferentialDrive extends RobotDriveBase {
m_reported = true;
}
xSpeed = limit(xSpeed);
xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
zRotation = limit(zRotation);
zRotation = MathUtils.clamp(zRotation, -1.0, 1.0);
zRotation = applyDeadband(zRotation, m_deadband);
double angularPower;
@@ -254,7 +256,7 @@ public class DifferentialDrive extends RobotDriveBase {
if (isQuickTurn) {
if (Math.abs(xSpeed) < m_quickStopThreshold) {
m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator
+ m_quickStopAlpha * limit(zRotation) * 2;
+ m_quickStopAlpha * MathUtils.clamp(zRotation, -1.0, 1.0) * 2;
}
overPower = true;
angularPower = zRotation;
@@ -333,10 +335,10 @@ public class DifferentialDrive extends RobotDriveBase {
m_reported = true;
}
leftSpeed = limit(leftSpeed);
leftSpeed = MathUtils.clamp(leftSpeed, -1.0, 1.0);
leftSpeed = applyDeadband(leftSpeed, m_deadband);
rightSpeed = limit(rightSpeed);
rightSpeed = MathUtils.clamp(rightSpeed, -1.0, 1.0);
rightSpeed = applyDeadband(rightSpeed, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,6 +14,7 @@ import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpiutil.math.MathUtils;
/**
* A class for driving Killough drive platforms.
@@ -171,10 +172,10 @@ public class KilloughDrive extends RobotDriveBase {
m_reported = true;
}
ySpeed = limit(ySpeed);
ySpeed = MathUtils.clamp(ySpeed, -1.0, 1.0);
ySpeed = applyDeadband(ySpeed, m_deadband);
xSpeed = limit(xSpeed);
xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,6 +14,7 @@ import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpiutil.math.MathUtils;
/**
* A class for driving Mecanum drive platforms.
@@ -156,10 +157,10 @@ public class MecanumDrive extends RobotDriveBase {
m_reported = true;
}
ySpeed = limit(ySpeed);
ySpeed = MathUtils.clamp(ySpeed, -1.0, 1.0);
ySpeed = applyDeadband(ySpeed, m_deadband);
xSpeed = limit(xSpeed);
xSpeed = MathUtils.clamp(xSpeed, -1.0, 1.0);
xSpeed = applyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.

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

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -143,19 +143,6 @@ public abstract class RobotDriveBase extends MotorSafety implements Sendable, Au
@Override
public abstract String getDescription();
/**
* Limit motor values to the -1.0 to +1.0 range.
*/
protected double limit(double value) {
if (value > 1.0) {
return 1.0;
}
if (value < -1.0) {
return -1.0;
}
return value;
}
/**
* 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.

25
wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java Normal file
View File

@@ -0,0 +1,25 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
public final class MathUtils {
private MathUtils() {
throw new AssertionError("utility class");
}
/**
* Returns value clamped between low and high boundaries.
*
* @param value Value to clamp.
* @param low The lower boundary to which to clamp value.
* @param high The higher boundary to which to clamp value.
*/
public static double clamp(double value, double low, double high) {
return Math.max(low, Math.min(value, high));
}
}