diff --git a/hal/src/main/native/athena/PWM.cpp b/hal/src/main/native/athena/PWM.cpp index 06b527cd6f..a3b141c617 100644 --- a/hal/src/main/native/athena/PWM.cpp +++ b/hal/src/main/native/athena/PWM.cpp @@ -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; } diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp index 5ad65b8398..eab55a8444 100644 --- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp @@ -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 diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp index ba5f691f22..736683bbdc 100644 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp @@ -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. diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index c62435499d..41ca2eefd3 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -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. diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp index c22cb2a68e..bae8868d8f 100644 --- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp +++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp @@ -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) { diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h index 206434ef29..348a62e47d 100644 --- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h +++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h @@ -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. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java index 6d34d67335..a4bd70a93d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java @@ -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)); - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 9355adccc3..2c148fc40e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -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 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java index 165ba70d17..07d3ba5268 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java @@ -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. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index acdc4d7854..c6a67d51bc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -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. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java index 2e474340b1..f82d6ade89 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java @@ -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. diff --git a/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java new file mode 100644 index 0000000000..2f910656e5 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/wpiutil/math/MathUtils.java @@ -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)); + } +}