Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Diff: DQMapper/DQMapper.cpp

Revision:
160:6948bb7bcabd
Parent:
124:e70ca81676fc
Child:
161:19eac809c727
--- a/DQMapper/DQMapper.cpp	Sun May 07 17:43:41 2017 +0000
+++ b/DQMapper/DQMapper.cpp	Sat Jul 01 21:16:33 2017 +0000
@@ -75,4 +75,19 @@
         *d = _id;
         *q = 0.0f;
     }
+}
+
+void AutoMapper::map(float torque_percent, float w, float *d, float *q) {
+    if (torque_percent > 0.99f) {
+        _theta += (_phase_high - _phase_low) / _steps;
+        torque_percent = 0.0f;
+    }
+    if (_theta >= _phase_high) {
+        *q = 0.0f;
+        *d = 0.0f;
+        return;
+    }
+    
+    *q = _is * torque_percent * sinf(_theta);
+    *d = _is * torque_percent * cosf(_theta);
 }
\ No newline at end of file