Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Diff: MathHelpers/MathHelpers.cpp

Revision:
64:b4175385d718
Parent:
39:80b38a8e1787
Child:
82:5e741c5ffd9f
Child:
159:9dbc0657238c
--- a/MathHelpers/MathHelpers.cpp	Sat Jan 28 09:10:07 2017 +0000
+++ b/MathHelpers/MathHelpers.cpp	Sat Jan 28 23:21:07 2017 +0000
@@ -1,4 +1,5 @@
 #include "MathHelpers.h"
+#include "math.h"
 
 float map(float x, float in_min, float in_max, float out_min, float out_max)
 {
@@ -12,6 +13,15 @@
     return in;
 }
 
+float constrain_norm(float *x, float *y, float coeffx, float coeffy, float max) {
+    float norm = sqrtf(coeffx * *x * *x + coeffy * *y * *y);
+    if (norm > max) {
+        *x /= norm;
+        *y /= norm;
+    }
+    return norm;
+}
+
 float fminf(float a, float b) {
     if(a < b) return a;
     return b;