robot

Dependencies:   FastPWM3 mbed

Revision:
38:07cb4ae6c1bd
Parent:
19:a6cf15f89f3d
Child:
39:80b38a8e1787
--- a/MathHelpers/MathHelpers.cpp	Sun Nov 27 11:36:54 2016 +0000
+++ b/MathHelpers/MathHelpers.cpp	Tue Nov 29 00:31:18 2016 +0000
@@ -20,4 +20,41 @@
 float fmaxf(float a, float b) {
     if(a > b) return a;
     return b;
+}
+
+float acbrt(float x0) {
+    union { int ix; float x; };
+
+    x = x0;
+    ix = 0x2a5137a0 + ix / 3;
+    x = 0.33333333f*(2.0f*x + x0 / (x * x));
+
+    return x;
+}
+
+float aatan(float d, float q) {
+    if (q > 0.f && d < 0.f && -d < q) {
+        return d * q / (q * q + 0.28125f * d * d);;
+    }
+    else if (q > 0.f && d < 0.f && -d > q) {
+        return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
+    }
+    else if (q < 0.f && d < 0.f && -d > -q) {
+        return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);
+    }
+    else if (q < 0.f && d < 0.f && -d < -q) {
+        return d * q / (q * q + 0.28125f * d * d);;
+    }
+    else if (q < 0.f && d > 0.f && d < -q) {
+        return d * q / (q * q + 0.28125f * d * d);;
+    }
+    else if (q < 0.f && d > 0.f && d > -q) {
+        return -M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
+    }
+    else if (q > 0.f && d > 0.f && d > q) {
+        return M_PI / 2.f - d * q / (d * d + 0.28125f * q * q);;
+    }
+    else {
+        return d * q / (q * q + 0.28125f * d * d);;
+    }
 }
\ No newline at end of file