Bayley Wang
/
foc-ed_in_the_bot_compact
robot
DQMapper/DQMapper.cpp@44:3fd6a43b91f0, 2017-01-08 (annotated)
- Committer:
- bwang
- Date:
- Sun Jan 08 09:03:15 2017 +0000
- Revision:
- 44:3fd6a43b91f0
- Parent:
- 42:030e0ec4eac5
- Child:
- 45:cf8ad81fb0f0
temp commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 42:030e0ec4eac5 | 1 | #include "DQMapper.h" |
bwang | 42:030e0ec4eac5 | 2 | |
bwang | 42:030e0ec4eac5 | 3 | #include "config_motor.h" |
bwang | 42:030e0ec4eac5 | 4 | #include "config_table.h" |
bwang | 42:030e0ec4eac5 | 5 | |
bwang | 42:030e0ec4eac5 | 6 | #include "dtab_n_w25.h" |
bwang | 42:030e0ec4eac5 | 7 | #include "dtab_p_w25.h" |
bwang | 42:030e0ec4eac5 | 8 | #include "qtab_n_w25.h" |
bwang | 42:030e0ec4eac5 | 9 | #include "qtab_p_w25.h" |
bwang | 42:030e0ec4eac5 | 10 | |
bwang | 42:030e0ec4eac5 | 11 | void LutMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 42:030e0ec4eac5 | 12 | w *= POLE_PAIRS; |
bwang | 42:030e0ec4eac5 | 13 | if (w < 0.f) { |
bwang | 42:030e0ec4eac5 | 14 | w = -w; |
bwang | 42:030e0ec4eac5 | 15 | torque_percent = -torque_percent; |
bwang | 42:030e0ec4eac5 | 16 | } |
bwang | 42:030e0ec4eac5 | 17 | if (torque_percent > 0) { |
bwang | 42:030e0ec4eac5 | 18 | int row = (int) (w / W_STEP); |
bwang | 42:030e0ec4eac5 | 19 | int col = (int) (torque_percent * COLUMNS); |
bwang | 42:030e0ec4eac5 | 20 | |
bwang | 42:030e0ec4eac5 | 21 | if (row >= ROWS) row = ROWS - 1; |
bwang | 42:030e0ec4eac5 | 22 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 42:030e0ec4eac5 | 23 | |
bwang | 42:030e0ec4eac5 | 24 | *d = (float) dtab_p_w25[row][col] / 128.f; |
bwang | 42:030e0ec4eac5 | 25 | *q = (float) qtab_p_w25[row][col] / 128.f; |
bwang | 42:030e0ec4eac5 | 26 | } else { |
bwang | 42:030e0ec4eac5 | 27 | int row = (int) (w / W_STEP); |
bwang | 42:030e0ec4eac5 | 28 | int col = (int) (-torque_percent * COLUMNS); |
bwang | 42:030e0ec4eac5 | 29 | |
bwang | 42:030e0ec4eac5 | 30 | if (row >= ROWS) row = ROWS - 1; |
bwang | 42:030e0ec4eac5 | 31 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 42:030e0ec4eac5 | 32 | |
bwang | 42:030e0ec4eac5 | 33 | *d = (float) dtab_n_w25[row][col] / 128.f; |
bwang | 42:030e0ec4eac5 | 34 | *q = (float) qtab_n_w25[row][col] / 128.f; |
bwang | 42:030e0ec4eac5 | 35 | } |
bwang | 44:3fd6a43b91f0 | 36 | } |
bwang | 44:3fd6a43b91f0 | 37 | |
bwang | 44:3fd6a43b91f0 | 38 | void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 42:030e0ec4eac5 | 39 | } |