| 1 | package org.usfirst.frc.team3501.robot; |
| 2 | |
| 3 | public class MathLib { |
| 4 | |
| 5 | /*** |
| 6 | * This method gives speed as a function of % distance covered so the speed |
| 7 | * forms a parabola starting and ending at minSpeed when you start and end and |
| 8 | * achieving maxSpeed exactly halfway. |
| 9 | * |
| 10 | * @param minSpeed |
| 11 | * the starting and ending speed, in range [0, 1] |
| 12 | * @param maxSpeed |
| 13 | * the max speed, achieved at percentComplete = 1/2. |
| 14 | * @param percentComplete |
| 15 | * should be currentDistance / targetDistance |
| 16 | * @return the speed (motor value) to set motors to for smooth acceleration. |
| 17 | * Note that since velocity is a parabola, acceleration is linear. It |
| 18 | * may exceed the maximum value robot can accelerate without wheel |
| 19 | * slipping. |
| 20 | */ |
| 21 | public static double getSpeedForLinearAccel(double minSpeed, double maxSpeed, |
| 22 | double percentComplete) { |
| 23 | return 4 * (minSpeed - maxSpeed) * (percentComplete - 0.5) |
| 24 | * (percentComplete - 0.5) + maxSpeed; |
| 25 | } |
| 26 | |
| 27 | /*** |
| 28 | * This method gives speed as a function of % distance covered so the speed |
| 29 | * increases linearly from minSpeed to maxSpeed and then back down again. |
| 30 | * |
| 31 | * @param minSpeed |
| 32 | * the starting and ending speed, in range [0, 1] |
| 33 | * @param maxSpeed |
| 34 | * the max speed, achieved at percentComplete = 1/2. |
| 35 | * @param percentComplete |
| 36 | * should be currentDistance / targetDistance |
| 37 | * @return the speed (motor value) to set motors to. |
| 38 | */ |
| 39 | public static double getSpeedForConstantAccel(double minSpeed, |
| 40 | double maxSpeed, double percentComplete) { |
| 41 | return maxSpeed |
| 42 | + 2 * (minSpeed - maxSpeed) * Math.abs(percentComplete - 0.5); |
| 43 | } |
| 44 | |
| 45 | /*** |
| 46 | * Restricts an input value to the range [low, high]. If value > high it will |
| 47 | * be set to high. If value < low it will be set to low. |
| 48 | * |
| 49 | * This method is used for defensive programming for inputs to motors to |
| 50 | * restrict them to valid ranges. |
| 51 | * |
| 52 | * @param value |
| 53 | * the value to restrict. |
| 54 | * @param low |
| 55 | * the smallest acceptable value. |
| 56 | * @param high |
| 57 | * the largest acceptable value. |
| 58 | * @return returns value restricted to be within the range [low, high]. |
| 59 | */ |
| 60 | public static double restrictToRange(double value, double low, double high) { |
| 61 | value = Math.max(value, low); |
| 62 | value = Math.min(value, high); |
| 63 | return value; |
| 64 | } |
| 65 | |
| 66 | /*** |
| 67 | * Returns true if val is in the range [low, high] |
| 68 | * |
| 69 | * @param val |
| 70 | * value to test |
| 71 | * @param low |
| 72 | * low end of range |
| 73 | * @param high |
| 74 | * high end of range |
| 75 | * @return boolean return true if val is in the range [low, high] |
| 76 | */ |
| 77 | public static boolean inRange(double val, double low, double high) { |
| 78 | return (val <= high) && (val >= low); |
| 79 | } |
| 80 | |
| 81 | public static double limitValue(double val) { |
| 82 | return limitValue(val, 1.0); |
| 83 | } |
| 84 | |
| 85 | public static double limitValue(double val, double max) { |
| 86 | if (val > max) { |
| 87 | return max; |
| 88 | } else if (val < -max) { |
| 89 | return -max; |
| 90 | } else { |
| 91 | return val; |
| 92 | } |
| 93 | } |
| 94 | |
| 95 | public static double limitValue(double val, double min, double max) { |
| 96 | if (val > max) { |
| 97 | return max; |
| 98 | } else if (val < min) { |
| 99 | return min; |
| 100 | } else { |
| 101 | return val; |
| 102 | } |
| 103 | } |
| 104 | |
| 105 | public static double calcLeftTankDrive(double x, double y) { |
| 106 | return limitValue(y + x); |
| 107 | } |
| 108 | |
| 109 | public static double calcRightTankDrive(double x, double y) { |
| 110 | return limitValue(y - x); |
| 111 | } |
| 112 | } |