Bayley Wang
/
foc-ed_in_the_bot_compact
robot
MathHelpers/MathHelpers.cpp@252:38644631ed97, 2018-11-13 (annotated)
- Committer:
- bwang
- Date:
- Tue Nov 13 17:46:23 2018 +0000
- Revision:
- 252:38644631ed97
- Parent:
- 181:d3510c8beab6
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 19:a6cf15f89f3d | 1 | #include "MathHelpers.h" |
bwang | 64:b4175385d718 | 2 | #include "math.h" |
bwang | 19:a6cf15f89f3d | 3 | |
bwang | 19:a6cf15f89f3d | 4 | float map(float x, float in_min, float in_max, float out_min, float out_max) |
bwang | 19:a6cf15f89f3d | 5 | { |
bwang | 19:a6cf15f89f3d | 6 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
bwang | 19:a6cf15f89f3d | 7 | } |
bwang | 19:a6cf15f89f3d | 8 | |
bwang | 19:a6cf15f89f3d | 9 | float constrain(float in, float min, float max) |
bwang | 19:a6cf15f89f3d | 10 | { |
bwang | 19:a6cf15f89f3d | 11 | if(in > max) return max; |
bwang | 19:a6cf15f89f3d | 12 | if(in < min) return min; |
bwang | 19:a6cf15f89f3d | 13 | return in; |
bwang | 19:a6cf15f89f3d | 14 | } |
bwang | 19:a6cf15f89f3d | 15 | |
bwang | 64:b4175385d718 | 16 | float constrain_norm(float *x, float *y, float coeffx, float coeffy, float max) { |
bwang | 64:b4175385d718 | 17 | float norm = sqrtf(coeffx * *x * *x + coeffy * *y * *y); |
bwang | 64:b4175385d718 | 18 | if (norm > max) { |
bwang | 159:9dbc0657238c | 19 | *x /= (norm / max); |
bwang | 159:9dbc0657238c | 20 | *y /= (norm / max); |
bwang | 64:b4175385d718 | 21 | } |
bwang | 64:b4175385d718 | 22 | return norm; |
bwang | 64:b4175385d718 | 23 | } |
bwang | 64:b4175385d718 | 24 | |
bwang | 19:a6cf15f89f3d | 25 | float fminf(float a, float b) { |
bwang | 19:a6cf15f89f3d | 26 | if(a < b) return a; |
bwang | 19:a6cf15f89f3d | 27 | return b; |
bwang | 19:a6cf15f89f3d | 28 | } |
bwang | 19:a6cf15f89f3d | 29 | |
bwang | 19:a6cf15f89f3d | 30 | float fmaxf(float a, float b) { |
bwang | 19:a6cf15f89f3d | 31 | if(a > b) return a; |
bwang | 19:a6cf15f89f3d | 32 | return b; |
bwang | 38:07cb4ae6c1bd | 33 | } |
bwang | 38:07cb4ae6c1bd | 34 | |
bwang | 181:d3510c8beab6 | 35 | float update_filter(float old, float x, float str) { |
bwang | 181:d3510c8beab6 | 36 | return str * old + (1.0f - str) * x; |
bwang | 181:d3510c8beab6 | 37 | } |
bwang | 181:d3510c8beab6 | 38 | |
bwang | 38:07cb4ae6c1bd | 39 | float acbrt(float x0) { |
bwang | 38:07cb4ae6c1bd | 40 | union { int ix; float x; }; |
bwang | 38:07cb4ae6c1bd | 41 | |
bwang | 38:07cb4ae6c1bd | 42 | x = x0; |
bwang | 38:07cb4ae6c1bd | 43 | ix = 0x2a5137a0 + ix / 3; |
bwang | 38:07cb4ae6c1bd | 44 | x = 0.33333333f*(2.0f*x + x0 / (x * x)); |
bwang | 38:07cb4ae6c1bd | 45 | |
bwang | 38:07cb4ae6c1bd | 46 | return x; |
bwang | 38:07cb4ae6c1bd | 47 | } |
bwang | 38:07cb4ae6c1bd | 48 | |
bwang | 39:80b38a8e1787 | 49 | float aatan2(float q, float d) { |
bwang | 38:07cb4ae6c1bd | 50 | if (q > 0.f && d < 0.f && -d < q) { |
bwang | 38:07cb4ae6c1bd | 51 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 52 | } |
bwang | 38:07cb4ae6c1bd | 53 | else if (q > 0.f && d < 0.f && -d > q) { |
bwang | 38:07cb4ae6c1bd | 54 | return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 55 | } |
bwang | 38:07cb4ae6c1bd | 56 | else if (q < 0.f && d < 0.f && -d > -q) { |
bwang | 38:07cb4ae6c1bd | 57 | return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q); |
bwang | 38:07cb4ae6c1bd | 58 | } |
bwang | 38:07cb4ae6c1bd | 59 | else if (q < 0.f && d < 0.f && -d < -q) { |
bwang | 38:07cb4ae6c1bd | 60 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 61 | } |
bwang | 38:07cb4ae6c1bd | 62 | else if (q < 0.f && d > 0.f && d < -q) { |
bwang | 38:07cb4ae6c1bd | 63 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 64 | } |
bwang | 38:07cb4ae6c1bd | 65 | else if (q < 0.f && d > 0.f && d > -q) { |
bwang | 38:07cb4ae6c1bd | 66 | return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 67 | } |
bwang | 38:07cb4ae6c1bd | 68 | else if (q > 0.f && d > 0.f && d > q) { |
bwang | 38:07cb4ae6c1bd | 69 | return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 70 | } |
bwang | 38:07cb4ae6c1bd | 71 | else { |
bwang | 38:07cb4ae6c1bd | 72 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 73 | } |
bwang | 19:a6cf15f89f3d | 74 | } |