Commit | Line | Data |
---|---|---|
38a404b3 KZ |
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 + 2 * (minSpeed - maxSpeed) | |
42 | * 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 | */ | |
33141cdd | 60 | public static double constrain(double value, double low, double high) { |
38a404b3 KZ |
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] | |
33141cdd | 68 | * |
38a404b3 KZ |
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 | } |