Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

MathHelpers/MathHelpers.cpp@92:a9dac72d8cac, 2017-04-06 (annotated)

Committer:
bwang
Date:
Thu Apr 06 17:33:47 2017 +0000
Revision:
92:a9dac72d8cac
Parent:
64:b4175385d718
Child:
82:5e741c5ffd9f
Child:
159:9dbc0657238c
--PwmIn now checks lower bounds for sanity and fall without rise; --switched to edge aligned pwm to work around prius module propagation delays

Who changed what in which revision?

UserRevisionLine numberNew 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 }