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

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.