Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Diff: DQMapper/DQMapper.cpp

Revision:
45:cf8ad81fb0f0
Parent:
44:3fd6a43b91f0
Child:
48:a1a09c83d42c
--- a/DQMapper/DQMapper.cpp	Sun Jan 08 09:03:15 2017 +0000
+++ b/DQMapper/DQMapper.cpp	Sun Jan 08 09:07:39 2017 +0000
@@ -1,3 +1,4 @@
+#include "math.h"
 #include "DQMapper.h"
 
 #include "config_motor.h"
@@ -36,4 +37,7 @@
 }
 
 void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) {
+    float is = torque_percent * _tmax / _kt;
+    *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq));
+    *q = sqrtf(is * is - *d * *d);
 }
\ No newline at end of file