Bayley Wang
/
foc-ed_in_the_bot_compact
robot
DQMapper/DQMapper.cpp
- Committer:
- bwang
- Date:
- 2017-05-04
- Revision:
- 155:7c6005933d4c
- Parent:
- 124:e70ca81676fc
- Child:
- 160:6948bb7bcabd
File content as of revision 155:7c6005933d4c:
#include "math.h" #include "DQMapper.h" #include "config_motor.h" #include "config_table.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 (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 - 0.02f) * COLUMNS); if (row >= ROWS) row = ROWS - 1; if (col >= COLUMNS) col = COLUMNS - 1; *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 - 0.02f) * COLUMNS); if (row >= ROWS) row = ROWS - 1; if (col >= COLUMNS) col = COLUMNS - 1; *d = (float) ndtab[row][col] / 128.f; *q = (float) nqtab[row][col] / 128.f; } if (neg) { *q = -*q; } } 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); } void AngleMapper::map(float torque_percent, float w, float *d, float *q) { *q = _is * torque_percent * sinf(_theta); *d = _is * torque_percent * cosf(_theta); } void DirectMapper::map(float torque_percent, float w, float *d, float *q) { *d = _id * torque_percent; *q = _iq * torque_percent; } void SwapMapper::map(float torque_percent, float w, float *d, float *q) { if (torque_percent < 0.5f) { *d = 0.0f; *q = _iq; } else { *d = _id; *q = 0.0f; } }