Diff: DQMapper/DQMapper.cpp

Revision:

42:030e0ec4eac5

Child:

44:3fd6a43b91f0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DQMapper/DQMapper.cpp	Fri Jan 06 07:54:22 2017 +0000
@@ -0,0 +1,36 @@
+#include "DQMapper.h"
+
+#include "config_motor.h"
+#include "config_table.h"
+
+#include "dtab_n_w25.h"
+#include "dtab_p_w25.h"
+#include "qtab_n_w25.h"
+#include "qtab_p_w25.h"
+
+void LutMapper::map(float torque_percent, float w, float *d, float *q) {
+    w *= POLE_PAIRS;
+    if (w < 0.f) {
+        w = -w;
+        torque_percent = -torque_percent;
+    }
+    if (torque_percent > 0) {
+        int row = (int) (w / W_STEP);
+        int col = (int) (torque_percent * COLUMNS);
+        
+        if (row >= ROWS) row = ROWS - 1;
+        if (col >= COLUMNS) col = COLUMNS - 1;
+        
+        *d = (float) dtab_p_w25[row][col] / 128.f;
+        *q = (float) qtab_p_w25[row][col] / 128.f; 
+    } else {
+        int row = (int) (w / W_STEP);
+        int col = (int) (-torque_percent * COLUMNS);
+        
+        if (row >= ROWS) row = ROWS - 1;
+        if (col >= COLUMNS) col = COLUMNS - 1;
+        
+        *d = (float) dtab_n_w25[row][col] / 128.f;
+        *q = (float) qtab_n_w25[row][col] / 128.f;
+    }
+}
\ No newline at end of file