Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

MathHelpers/MathHelpers.cpp@39:80b38a8e1787, 2016-12-03 (annotated)

Committer:
bwang
Date:
Sat Dec 03 17:49:02 2016 +0000
Revision:
39:80b38a8e1787
Parent:
38:07cb4ae6c1bd
Child:
64:b4175385d718
lastest updates; loads of stubs for broken FW and optimization functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 19:a6cf15f89f3d 1 #include "MathHelpers.h"
bwang 19:a6cf15f89f3d 2
bwang 19:a6cf15f89f3d 3 float map(float x, float in_min, float in_max, float out_min, float out_max)
bwang 19:a6cf15f89f3d 4 {
bwang 19:a6cf15f89f3d 5 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
bwang 19:a6cf15f89f3d 6 }
bwang 19:a6cf15f89f3d 7
bwang 19:a6cf15f89f3d 8 float constrain(float in, float min, float max)
bwang 19:a6cf15f89f3d 9 {
bwang 19:a6cf15f89f3d 10 if(in > max) return max;
bwang 19:a6cf15f89f3d 11 if(in < min) return min;
bwang 19:a6cf15f89f3d 12 return in;
bwang 19:a6cf15f89f3d 13 }
bwang 19:a6cf15f89f3d 14
bwang 19:a6cf15f89f3d 15 float fminf(float a, float b) {
bwang 19:a6cf15f89f3d 16 if(a < b) return a;
bwang 19:a6cf15f89f3d 17 return b;
bwang 19:a6cf15f89f3d 18 }
bwang 19:a6cf15f89f3d 19
bwang 19:a6cf15f89f3d 20 float fmaxf(float a, float b) {
bwang 19:a6cf15f89f3d 21 if(a > b) return a;
bwang 19:a6cf15f89f3d 22 return b;
bwang 38:07cb4ae6c1bd 23 }
bwang 38:07cb4ae6c1bd 24
bwang 38:07cb4ae6c1bd 25 float acbrt(float x0) {
bwang 38:07cb4ae6c1bd 26 union { int ix; float x; };
bwang 38:07cb4ae6c1bd 27
bwang 38:07cb4ae6c1bd 28 x = x0;
bwang 38:07cb4ae6c1bd 29 ix = 0x2a5137a0 + ix / 3;
bwang 38:07cb4ae6c1bd 30 x = 0.33333333f*(2.0f*x + x0 / (x * x));
bwang 38:07cb4ae6c1bd 31
bwang 38:07cb4ae6c1bd 32 return x;
bwang 38:07cb4ae6c1bd 33 }
bwang 38:07cb4ae6c1bd 34
bwang 39:80b38a8e1787 35 float aatan2(float q, float d) {
bwang 38:07cb4ae6c1bd 36 if (q > 0.f && d < 0.f && -d < q) {
bwang 38:07cb4ae6c1bd 37 return d * q / (q * q + 0.28125f * d * d);;
bwang 38:07cb4ae6c1bd 38 }
bwang 38:07cb4ae6c1bd 39 else if (q > 0.f && d < 0.f && -d > q) {
bwang 38:07cb4ae6c1bd 40 return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
bwang 38:07cb4ae6c1bd 41 }
bwang 38:07cb4ae6c1bd 42 else if (q < 0.f && d < 0.f && -d > -q) {
bwang 38:07cb4ae6c1bd 43 return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);
bwang 38:07cb4ae6c1bd 44 }
bwang 38:07cb4ae6c1bd 45 else if (q < 0.f && d < 0.f && -d < -q) {
bwang 38:07cb4ae6c1bd 46 return d * q / (q * q + 0.28125f * d * d);;
bwang 38:07cb4ae6c1bd 47 }
bwang 38:07cb4ae6c1bd 48 else if (q < 0.f && d > 0.f && d < -q) {
bwang 38:07cb4ae6c1bd 49 return d * q / (q * q + 0.28125f * d * d);;
bwang 38:07cb4ae6c1bd 50 }
bwang 38:07cb4ae6c1bd 51 else if (q < 0.f && d > 0.f && d > -q) {
bwang 38:07cb4ae6c1bd 52 return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
bwang 38:07cb4ae6c1bd 53 }
bwang 38:07cb4ae6c1bd 54 else if (q > 0.f && d > 0.f && d > q) {
bwang 38:07cb4ae6c1bd 55 return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
bwang 38:07cb4ae6c1bd 56 }
bwang 38:07cb4ae6c1bd 57 else {
bwang 38:07cb4ae6c1bd 58 return d * q / (q * q + 0.28125f * d * d);;
bwang 38:07cb4ae6c1bd 59 }
bwang 19:a6cf15f89f3d 60 }