Bayley Wang
/
foc-ed_in_the_bot_compact
robot
MathHelpers/MathHelpers.cpp@155:7c6005933d4c, 2017-05-04 (annotated)
- Committer:
- bwang
- Date:
- Thu May 04 17:35:10 2017 +0000
- Revision:
- 155:7c6005933d4c
- Parent:
- 64:b4175385d718
- Child:
- 82:5e741c5ffd9f
- Child:
- 159:9dbc0657238c
05/04/2017 13:34 - changed logic in PwmIn::handle_fall, which no longer resets timer on invalid edges
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 | 64:b4175385d718 | 19 | *x /= norm; |
bwang | 64:b4175385d718 | 20 | *y /= norm; |
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 | 38:07cb4ae6c1bd | 35 | float acbrt(float x0) { |
bwang | 38:07cb4ae6c1bd | 36 | union { int ix; float x; }; |
bwang | 38:07cb4ae6c1bd | 37 | |
bwang | 38:07cb4ae6c1bd | 38 | x = x0; |
bwang | 38:07cb4ae6c1bd | 39 | ix = 0x2a5137a0 + ix / 3; |
bwang | 38:07cb4ae6c1bd | 40 | x = 0.33333333f*(2.0f*x + x0 / (x * x)); |
bwang | 38:07cb4ae6c1bd | 41 | |
bwang | 38:07cb4ae6c1bd | 42 | return x; |
bwang | 38:07cb4ae6c1bd | 43 | } |
bwang | 38:07cb4ae6c1bd | 44 | |
bwang | 39:80b38a8e1787 | 45 | float aatan2(float q, float d) { |
bwang | 38:07cb4ae6c1bd | 46 | if (q > 0.f && d < 0.f && -d < q) { |
bwang | 38:07cb4ae6c1bd | 47 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 48 | } |
bwang | 38:07cb4ae6c1bd | 49 | else if (q > 0.f && d < 0.f && -d > q) { |
bwang | 38:07cb4ae6c1bd | 50 | return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 51 | } |
bwang | 38:07cb4ae6c1bd | 52 | else if (q < 0.f && d < 0.f && -d > -q) { |
bwang | 38:07cb4ae6c1bd | 53 | return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q); |
bwang | 38:07cb4ae6c1bd | 54 | } |
bwang | 38:07cb4ae6c1bd | 55 | else if (q < 0.f && d < 0.f && -d < -q) { |
bwang | 38:07cb4ae6c1bd | 56 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 57 | } |
bwang | 38:07cb4ae6c1bd | 58 | else if (q < 0.f && d > 0.f && d < -q) { |
bwang | 38:07cb4ae6c1bd | 59 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 60 | } |
bwang | 38:07cb4ae6c1bd | 61 | else if (q < 0.f && d > 0.f && d > -q) { |
bwang | 38:07cb4ae6c1bd | 62 | return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 63 | } |
bwang | 38:07cb4ae6c1bd | 64 | else if (q > 0.f && d > 0.f && d > q) { |
bwang | 38:07cb4ae6c1bd | 65 | return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);; |
bwang | 38:07cb4ae6c1bd | 66 | } |
bwang | 38:07cb4ae6c1bd | 67 | else { |
bwang | 38:07cb4ae6c1bd | 68 | return d * q / (q * q + 0.28125f * d * d);; |
bwang | 38:07cb4ae6c1bd | 69 | } |
bwang | 19:a6cf15f89f3d | 70 | } |