Bayley Wang
/
foc-ed_in_the_bot_compact
robot
DQMapper/DQMapper.cpp@161:19eac809c727, 2017-07-01 (annotated)
- Committer:
- bwang
- Date:
- Sat Jul 01 22:29:19 2017 +0000
- Revision:
- 161:19eac809c727
- Parent:
- 160:6948bb7bcabd
- Child:
- 171:3f1d1792757c
back to driving code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 45:cf8ad81fb0f0 | 1 | #include "math.h" |
bwang | 42:030e0ec4eac5 | 2 | #include "DQMapper.h" |
bwang | 42:030e0ec4eac5 | 3 | |
bwang | 42:030e0ec4eac5 | 4 | #include "config_motor.h" |
bwang | 42:030e0ec4eac5 | 5 | #include "config_table.h" |
bwang | 42:030e0ec4eac5 | 6 | |
bwang | 48:a1a09c83d42c | 7 | #include "ndtab.h" |
bwang | 48:a1a09c83d42c | 8 | #include "nqtab.h" |
bwang | 48:a1a09c83d42c | 9 | #include "pdtab.h" |
bwang | 48:a1a09c83d42c | 10 | #include "pqtab.h" |
bwang | 48:a1a09c83d42c | 11 | #include "zdtab.h" |
bwang | 48:a1a09c83d42c | 12 | #include "zqtab.h" |
bwang | 42:030e0ec4eac5 | 13 | |
bwang | 42:030e0ec4eac5 | 14 | void LutMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 42:030e0ec4eac5 | 15 | w *= POLE_PAIRS; |
bwang | 48:a1a09c83d42c | 16 | bool neg = false; |
bwang | 48:a1a09c83d42c | 17 | |
bwang | 42:030e0ec4eac5 | 18 | if (w < 0.f) { |
bwang | 42:030e0ec4eac5 | 19 | w = -w; |
bwang | 42:030e0ec4eac5 | 20 | torque_percent = -torque_percent; |
bwang | 48:a1a09c83d42c | 21 | neg = true; |
bwang | 42:030e0ec4eac5 | 22 | } |
bwang | 48:a1a09c83d42c | 23 | if (fabs(torque_percent) < 0.02f) { |
bwang | 48:a1a09c83d42c | 24 | int index = (int) (w / W_STEP); |
bwang | 48:a1a09c83d42c | 25 | if (index >= ROWS) index = ROWS - 1; |
bwang | 48:a1a09c83d42c | 26 | |
bwang | 48:a1a09c83d42c | 27 | *d = (float) zdtab[index] / 128.f; |
bwang | 48:a1a09c83d42c | 28 | *q = (float) zqtab[index] / 128.f; |
bwang | 48:a1a09c83d42c | 29 | } else if (torque_percent >= 0.02f) { |
bwang | 42:030e0ec4eac5 | 30 | int row = (int) (w / W_STEP); |
bwang | 48:a1a09c83d42c | 31 | int col = (int) ((torque_percent - 0.02f) * COLUMNS); |
bwang | 42:030e0ec4eac5 | 32 | |
bwang | 42:030e0ec4eac5 | 33 | if (row >= ROWS) row = ROWS - 1; |
bwang | 42:030e0ec4eac5 | 34 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 42:030e0ec4eac5 | 35 | |
bwang | 48:a1a09c83d42c | 36 | *d = (float) pdtab[row][col] / 128.f; |
bwang | 48:a1a09c83d42c | 37 | *q = (float) pqtab[row][col] / 128.f; |
bwang | 42:030e0ec4eac5 | 38 | } else { |
bwang | 42:030e0ec4eac5 | 39 | int row = (int) (w / W_STEP); |
bwang | 48:a1a09c83d42c | 40 | int col = (int) ((-torque_percent - 0.02f) * COLUMNS); |
bwang | 42:030e0ec4eac5 | 41 | |
bwang | 42:030e0ec4eac5 | 42 | if (row >= ROWS) row = ROWS - 1; |
bwang | 42:030e0ec4eac5 | 43 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 42:030e0ec4eac5 | 44 | |
bwang | 48:a1a09c83d42c | 45 | *d = (float) ndtab[row][col] / 128.f; |
bwang | 48:a1a09c83d42c | 46 | *q = (float) nqtab[row][col] / 128.f; |
bwang | 48:a1a09c83d42c | 47 | } |
bwang | 48:a1a09c83d42c | 48 | |
bwang | 48:a1a09c83d42c | 49 | if (neg) { |
bwang | 48:a1a09c83d42c | 50 | *q = -*q; |
bwang | 42:030e0ec4eac5 | 51 | } |
bwang | 44:3fd6a43b91f0 | 52 | } |
bwang | 44:3fd6a43b91f0 | 53 | |
bwang | 44:3fd6a43b91f0 | 54 | void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 45:cf8ad81fb0f0 | 55 | float is = torque_percent * _tmax / _kt; |
bwang | 45:cf8ad81fb0f0 | 56 | *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq)); |
bwang | 45:cf8ad81fb0f0 | 57 | *q = sqrtf(is * is - *d * *d); |
bwang | 98:1051c4103900 | 58 | } |
bwang | 98:1051c4103900 | 59 | |
bwang | 98:1051c4103900 | 60 | void AngleMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 98:1051c4103900 | 61 | *q = _is * torque_percent * sinf(_theta); |
bwang | 98:1051c4103900 | 62 | *d = _is * torque_percent * cosf(_theta); |
bwang | 124:e70ca81676fc | 63 | } |
bwang | 124:e70ca81676fc | 64 | |
bwang | 124:e70ca81676fc | 65 | void DirectMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 124:e70ca81676fc | 66 | *d = _id * torque_percent; |
bwang | 124:e70ca81676fc | 67 | *q = _iq * torque_percent; |
bwang | 124:e70ca81676fc | 68 | } |
bwang | 124:e70ca81676fc | 69 | |
bwang | 124:e70ca81676fc | 70 | void SwapMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 124:e70ca81676fc | 71 | if (torque_percent < 0.5f) { |
bwang | 124:e70ca81676fc | 72 | *d = 0.0f; |
bwang | 124:e70ca81676fc | 73 | *q = _iq; |
bwang | 124:e70ca81676fc | 74 | } else { |
bwang | 124:e70ca81676fc | 75 | *d = _id; |
bwang | 124:e70ca81676fc | 76 | *q = 0.0f; |
bwang | 124:e70ca81676fc | 77 | } |
bwang | 160:6948bb7bcabd | 78 | } |
bwang | 160:6948bb7bcabd | 79 | |
bwang | 160:6948bb7bcabd | 80 | void AutoMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 160:6948bb7bcabd | 81 | if (torque_percent > 0.99f) { |
bwang | 160:6948bb7bcabd | 82 | _theta += (_phase_high - _phase_low) / _steps; |
bwang | 160:6948bb7bcabd | 83 | torque_percent = 0.0f; |
bwang | 160:6948bb7bcabd | 84 | } |
bwang | 161:19eac809c727 | 85 | |
bwang | 160:6948bb7bcabd | 86 | if (_theta >= _phase_high) { |
bwang | 160:6948bb7bcabd | 87 | *q = 0.0f; |
bwang | 160:6948bb7bcabd | 88 | *d = 0.0f; |
bwang | 160:6948bb7bcabd | 89 | return; |
bwang | 160:6948bb7bcabd | 90 | } |
bwang | 160:6948bb7bcabd | 91 | |
bwang | 160:6948bb7bcabd | 92 | *q = _is * torque_percent * sinf(_theta); |
bwang | 160:6948bb7bcabd | 93 | *d = _is * torque_percent * cosf(_theta); |
bwang | 42:030e0ec4eac5 | 94 | } |