1 package org
.usfirst
.frc
.team3501
.robot
;
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.
11 * the starting and ending speed, in range [0, 1]
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
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
;
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.
32 * the starting and ending speed, in range [0, 1]
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.
39 public static double getSpeedForConstantAccel(double minSpeed
,
40 double maxSpeed
, double percentComplete
) {
41 return maxSpeed
+ 2 * (minSpeed
- maxSpeed
)
42 * Math
.abs(percentComplete
- 0.5);
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.
49 * This method is used for defensive programming for inputs to motors to
50 * restrict them to valid ranges.
53 * the value to restrict.
55 * the smallest acceptable value.
57 * the largest acceptable value.
58 * @return returns value restricted to be within the range [low, high].
60 public static double restrictToRange(double value
, double low
, double high
) {
61 value
= Math
.max(value
, low
);
62 value
= Math
.min(value
, high
);
67 * Returns true if val is in the range [low, high]
75 * @return boolean return true if val is in the range [low, high]
77 public static boolean inRange(double val
, double low
, double high
) {
78 return (val
<= high
) && (val
>= low
);