Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: DQMapper/DQMapper.cpp
- Revision:
- 48:a1a09c83d42c
- Parent:
- 45:cf8ad81fb0f0
- Child:
- 98:1051c4103900
--- a/DQMapper/DQMapper.cpp Tue Jan 10 05:35:24 2017 +0000 +++ b/DQMapper/DQMapper.cpp Fri Jan 13 08:07:34 2017 +0000 @@ -4,35 +4,50 @@ #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" +#include "ndtab.h" +#include "nqtab.h" +#include "pdtab.h" +#include "pqtab.h" +#include "zdtab.h" +#include "zqtab.h" void LutMapper::map(float torque_percent, float w, float *d, float *q) { w *= POLE_PAIRS; + bool neg = false; + if (w < 0.f) { w = -w; torque_percent = -torque_percent; + neg = true; } - if (torque_percent > 0) { + if (fabs(torque_percent) < 0.02f) { + int index = (int) (w / W_STEP); + if (index >= ROWS) index = ROWS - 1; + + *d = (float) zdtab[index] / 128.f; + *q = (float) zqtab[index] / 128.f; + } else if (torque_percent >= 0.02f) { int row = (int) (w / W_STEP); - int col = (int) (torque_percent * COLUMNS); + int col = (int) ((torque_percent - 0.02f) * 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; + *d = (float) pdtab[row][col] / 128.f; + *q = (float) pqtab[row][col] / 128.f; } else { int row = (int) (w / W_STEP); - int col = (int) (-torque_percent * COLUMNS); + int col = (int) ((-torque_percent - 0.02f) * 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; + *d = (float) ndtab[row][col] / 128.f; + *q = (float) nqtab[row][col] / 128.f; + } + + if (neg) { + *q = -*q; } }