Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

MathHelpers/MathHelpers.cpp@82:5e741c5ffd9f, 2017-03-10 (annotated)

Committer:
bwang
Date:
Fri Mar 10 08:29:13 2017 +0000
Revision:
82:5e741c5ffd9f
Parent:
64:b4175385d718 
things somewhat work?

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 82:5e741c5ffd9f 35 float update_filter(float old, float x, float str) {
bwang 82:5e741c5ffd9f 36 return str * old + (1.0f - str) * x;
bwang 82:5e741c5ffd9f 37 }
bwang 82:5e741c5ffd9f 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 }