001/* Copyright (C) Cross The Road Electronics 2024 */
002package com.ctre.phoenix;
003
004/**
005 * Class with basic utility methods
006 */
007public class Util
008{
009        /**
010         * Caps the value
011         * @param value Value to cap
012         * @param peak Maximum/-Minimum value can be
013         * @return Capped value
014         */
015        public static double cap(double value, double peak)
016        {
017                if(value < -peak) value = -peak;
018                if(value > peak) value = peak;
019                return value;
020        }
021        
022        /**
023         * Scales rotations to native units
024         * @param scalar Value to scale by
025         * @param fullRotations Number of rotations to scale by
026         * @return Scaled units
027         */
028        public static int scaleRotationsToNativeUnits(double scalar, double fullRotations) {
029                /* first assume we don't have config info, prep the default return */
030                int retval = (int) fullRotations;
031                /* apply scalar if its available */
032                if (scalar > 0) {
033                  retval = (int) (fullRotations * scalar);
034                }
035                return retval;
036        }
037        /**
038         * Scales velocity to native units
039         * @param scalar Value to scale by
040         * @param rpm Velocity in rotations per minute
041         * @return Scaled velocity
042         */
043        public static int scaleVelocityToNativeUnits(double scalar, double rpm) {
044                /* first assume we don't have config info, prep the default return */
045                int retval = (int) rpm;
046                /* apply scalar if its available */
047                if (scalar > 0) {
048                  retval = (int) (rpm * scalar);
049                }
050                return retval;
051        }
052        /**
053         * Scales native units to rotations
054         * @param scalar Value to scale by
055         * @param nativePos Native position units
056         * @return Scaled units
057         */
058        public static double scaleNativeUnitsToRotations(double scalar, long nativePos) {
059                /* first assume we don't have config info, prep the default return */
060                double retval = (double) nativePos;
061                /* retrieve scaling info */
062                if (scalar > 0) {
063                  retval = ((double) nativePos) / scalar;
064                }
065                return retval;
066        }
067        /**
068         * Scales Native units to velocity
069         * @param scalar Value to scale by
070         * @param nativeVel Native velocity units
071         * @return Scaled units
072         */
073        public static double scaleNativeUnitsToRpm(double scalar, long nativeVel) {
074                /* first assume we don't have config info, prep the default return */
075                double retval = (double) nativeVel;
076                /* apply scalar if its available */
077                if (scalar > 0) {
078                  retval = (double) (nativeVel) / (scalar);
079                }
080                return retval;
081        }
082}