I have done this and used it in my Framework for RobotC
They are under Math Functions because I use it for more than just controller trimming.
Here is the code for it:
// MathExtesion
// This class is designed to give more math functions in robotc.
int Math_Clamp(int Number, int minNumber, int maxNumber);
int Math_ClampToZero(int Number, int minNumber, int maxNumber);
int Math_Clamp(int Number, int minNumber, int maxNumber)
{
if (Number > maxNumber)
{
Number = maxNumber;
}
if (Number < minNumber)
{
Number = minNumber;
}
return Number;
}
int Math_ClampToZero(int Number, int minNumber, int maxNumber)
{
if (Number < maxNumber && Number > minNumber)
{
Number = 0;
}
return Number;
}