Bayley Wang
/
foc-ed_in_the_bot_compact
robot
DQMapper/DQMapper.cpp@185:5c102874b490, 2018-02-09 (annotated)
- Committer:
- bwang
- Date:
- Fri Feb 09 22:19:52 2018 +0000
- Revision:
- 185:5c102874b490
- Parent:
- 171:3f1d1792757c
- Child:
- 186:c18db1e31da6
moved all the hard-coded values to defaults.h
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 | 185:5c102874b490 | 4 | #include "defaults.h" |
bwang | 185:5c102874b490 | 5 | #include "derived.h" |
bwang | 185:5c102874b490 | 6 | |
bwang | 185:5c102874b490 | 7 | /* |
bwang | 42:030e0ec4eac5 | 8 | #include "config_motor.h" |
bwang | 42:030e0ec4eac5 | 9 | #include "config_table.h" |
bwang | 185:5c102874b490 | 10 | */ |
bwang | 42:030e0ec4eac5 | 11 | |
bwang | 48:a1a09c83d42c | 12 | #include "ndtab.h" |
bwang | 48:a1a09c83d42c | 13 | #include "nqtab.h" |
bwang | 48:a1a09c83d42c | 14 | #include "pdtab.h" |
bwang | 48:a1a09c83d42c | 15 | #include "pqtab.h" |
bwang | 48:a1a09c83d42c | 16 | #include "zdtab.h" |
bwang | 48:a1a09c83d42c | 17 | #include "zqtab.h" |
bwang | 42:030e0ec4eac5 | 18 | |
bwang | 42:030e0ec4eac5 | 19 | void LutMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 42:030e0ec4eac5 | 20 | w *= POLE_PAIRS; |
bwang | 48:a1a09c83d42c | 21 | bool neg = false; |
bwang | 48:a1a09c83d42c | 22 | |
bwang | 42:030e0ec4eac5 | 23 | if (w < 0.f) { |
bwang | 42:030e0ec4eac5 | 24 | w = -w; |
bwang | 42:030e0ec4eac5 | 25 | torque_percent = -torque_percent; |
bwang | 48:a1a09c83d42c | 26 | neg = true; |
bwang | 42:030e0ec4eac5 | 27 | } |
bwang | 48:a1a09c83d42c | 28 | if (fabs(torque_percent) < 0.02f) { |
bwang | 48:a1a09c83d42c | 29 | int index = (int) (w / W_STEP); |
bwang | 48:a1a09c83d42c | 30 | if (index >= ROWS) index = ROWS - 1; |
bwang | 48:a1a09c83d42c | 31 | |
bwang | 48:a1a09c83d42c | 32 | *d = (float) zdtab[index] / 128.f; |
bwang | 48:a1a09c83d42c | 33 | *q = (float) zqtab[index] / 128.f; |
bwang | 48:a1a09c83d42c | 34 | } else if (torque_percent >= 0.02f) { |
bwang | 42:030e0ec4eac5 | 35 | int row = (int) (w / W_STEP); |
bwang | 48:a1a09c83d42c | 36 | int col = (int) ((torque_percent - 0.02f) * COLUMNS); |
bwang | 42:030e0ec4eac5 | 37 | |
bwang | 42:030e0ec4eac5 | 38 | if (row >= ROWS) row = ROWS - 1; |
bwang | 42:030e0ec4eac5 | 39 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 42:030e0ec4eac5 | 40 | |
bwang | 48:a1a09c83d42c | 41 | *d = (float) pdtab[row][col] / 128.f; |
bwang | 48:a1a09c83d42c | 42 | *q = (float) pqtab[row][col] / 128.f; |
bwang | 42:030e0ec4eac5 | 43 | } else { |
bwang | 42:030e0ec4eac5 | 44 | int row = (int) (w / W_STEP); |
bwang | 48:a1a09c83d42c | 45 | int col = (int) ((-torque_percent - 0.02f) * COLUMNS); |
bwang | 42:030e0ec4eac5 | 46 | |
bwang | 42:030e0ec4eac5 | 47 | if (row >= ROWS) row = ROWS - 1; |
bwang | 42:030e0ec4eac5 | 48 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 42:030e0ec4eac5 | 49 | |
bwang | 48:a1a09c83d42c | 50 | *d = (float) ndtab[row][col] / 128.f; |
bwang | 48:a1a09c83d42c | 51 | *q = (float) nqtab[row][col] / 128.f; |
bwang | 48:a1a09c83d42c | 52 | } |
bwang | 48:a1a09c83d42c | 53 | |
bwang | 48:a1a09c83d42c | 54 | if (neg) { |
bwang | 48:a1a09c83d42c | 55 | *q = -*q; |
bwang | 42:030e0ec4eac5 | 56 | } |
bwang | 44:3fd6a43b91f0 | 57 | } |
bwang | 44:3fd6a43b91f0 | 58 | |
bwang | 171:3f1d1792757c | 59 | float InterpolatingLutMapper::lookup(short table[][COLUMNS], int row, int col) { |
bwang | 171:3f1d1792757c | 60 | if (row >= ROWS) row = ROWS - 1; |
bwang | 171:3f1d1792757c | 61 | if (col >= COLUMNS) col = COLUMNS - 1; |
bwang | 171:3f1d1792757c | 62 | return (float)table[row][col] / 128.f; |
bwang | 171:3f1d1792757c | 63 | } |
bwang | 171:3f1d1792757c | 64 | |
bwang | 171:3f1d1792757c | 65 | float InterpolatingLutMapper::lookup(short *table, int index) { |
bwang | 171:3f1d1792757c | 66 | if (index >= ROWS) index = ROWS - 1; |
bwang | 171:3f1d1792757c | 67 | return (float)table[index] / 128.f; |
bwang | 171:3f1d1792757c | 68 | } |
bwang | 171:3f1d1792757c | 69 | |
bwang | 171:3f1d1792757c | 70 | float InterpolatingLutMapper::interp(float a, float b, float eps) { |
bwang | 171:3f1d1792757c | 71 | float delta = eps * (b - a); |
bwang | 171:3f1d1792757c | 72 | return a + delta; |
bwang | 171:3f1d1792757c | 73 | } |
bwang | 171:3f1d1792757c | 74 | |
bwang | 171:3f1d1792757c | 75 | float InterpolatingLutMapper::interp(float a, float b, float c, float eps_row, float eps_col) { |
bwang | 171:3f1d1792757c | 76 | float delta_row = eps_row * (b - a); |
bwang | 171:3f1d1792757c | 77 | float delta_col = eps_col * (c - a); |
bwang | 171:3f1d1792757c | 78 | return a + delta_row + delta_col; |
bwang | 171:3f1d1792757c | 79 | } |
bwang | 171:3f1d1792757c | 80 | |
bwang | 171:3f1d1792757c | 81 | void InterpolatingLutMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 171:3f1d1792757c | 82 | w *= POLE_PAIRS; |
bwang | 171:3f1d1792757c | 83 | bool neg = false; |
bwang | 171:3f1d1792757c | 84 | |
bwang | 171:3f1d1792757c | 85 | if (w < 0.f) { |
bwang | 171:3f1d1792757c | 86 | w = -w; |
bwang | 171:3f1d1792757c | 87 | torque_percent = -torque_percent; |
bwang | 171:3f1d1792757c | 88 | neg = true; |
bwang | 171:3f1d1792757c | 89 | } |
bwang | 171:3f1d1792757c | 90 | if (fabs(torque_percent) < 0.02f) { |
bwang | 171:3f1d1792757c | 91 | int index = (int)(w / W_STEP); |
bwang | 171:3f1d1792757c | 92 | float eps = w / W_STEP - index; |
bwang | 171:3f1d1792757c | 93 | |
bwang | 171:3f1d1792757c | 94 | *d = interp(lookup(zdtab, index), lookup(zdtab, index + 1), eps); |
bwang | 171:3f1d1792757c | 95 | *q = interp(lookup(zqtab, index), lookup(zqtab, index + 1), eps); |
bwang | 171:3f1d1792757c | 96 | } |
bwang | 171:3f1d1792757c | 97 | else if (torque_percent >= 0.02f) { |
bwang | 171:3f1d1792757c | 98 | int row = (int)(w / W_STEP); |
bwang | 171:3f1d1792757c | 99 | int col = (int)((torque_percent - 0.02f) * COLUMNS); |
bwang | 171:3f1d1792757c | 100 | |
bwang | 171:3f1d1792757c | 101 | float row_eps = w / W_STEP - row; |
bwang | 171:3f1d1792757c | 102 | float col_eps = (torque_percent - 0.02f) * COLUMNS - col; |
bwang | 171:3f1d1792757c | 103 | |
bwang | 171:3f1d1792757c | 104 | *d = interp(lookup(pdtab, row, col), lookup(pdtab, row + 1, col), lookup(pdtab, row, col + 1), row_eps, col_eps); |
bwang | 171:3f1d1792757c | 105 | *q = interp(lookup(pqtab, row, col), lookup(pqtab, row + 1, col), lookup(pqtab, row, col + 1), row_eps, col_eps); |
bwang | 171:3f1d1792757c | 106 | } |
bwang | 171:3f1d1792757c | 107 | else { |
bwang | 171:3f1d1792757c | 108 | int row = (int)(w / W_STEP); |
bwang | 171:3f1d1792757c | 109 | int col = (int)((-torque_percent - 0.02f) * COLUMNS); |
bwang | 171:3f1d1792757c | 110 | |
bwang | 171:3f1d1792757c | 111 | float row_eps = w / W_STEP - row; |
bwang | 171:3f1d1792757c | 112 | float col_eps = (-torque_percent - 0.02f) * COLUMNS - col; |
bwang | 171:3f1d1792757c | 113 | |
bwang | 171:3f1d1792757c | 114 | *d = interp(lookup(ndtab, row, col), lookup(ndtab, row + 1, col), lookup(ndtab, row, col + 1), row_eps, col_eps); |
bwang | 171:3f1d1792757c | 115 | *q = interp(lookup(nqtab, row, col), lookup(nqtab, row + 1, col), lookup(nqtab, row, col + 1), row_eps, col_eps); |
bwang | 171:3f1d1792757c | 116 | } |
bwang | 171:3f1d1792757c | 117 | |
bwang | 171:3f1d1792757c | 118 | if (neg) { |
bwang | 171:3f1d1792757c | 119 | *q = -*q; |
bwang | 171:3f1d1792757c | 120 | } |
bwang | 171:3f1d1792757c | 121 | } |
bwang | 171:3f1d1792757c | 122 | |
bwang | 44:3fd6a43b91f0 | 123 | void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 45:cf8ad81fb0f0 | 124 | float is = torque_percent * _tmax / _kt; |
bwang | 45:cf8ad81fb0f0 | 125 | *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq)); |
bwang | 45:cf8ad81fb0f0 | 126 | *q = sqrtf(is * is - *d * *d); |
bwang | 98:1051c4103900 | 127 | } |
bwang | 98:1051c4103900 | 128 | |
bwang | 98:1051c4103900 | 129 | void AngleMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 98:1051c4103900 | 130 | *q = _is * torque_percent * sinf(_theta); |
bwang | 98:1051c4103900 | 131 | *d = _is * torque_percent * cosf(_theta); |
bwang | 124:e70ca81676fc | 132 | } |
bwang | 124:e70ca81676fc | 133 | |
bwang | 124:e70ca81676fc | 134 | void DirectMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 124:e70ca81676fc | 135 | *d = _id * torque_percent; |
bwang | 124:e70ca81676fc | 136 | *q = _iq * torque_percent; |
bwang | 124:e70ca81676fc | 137 | } |
bwang | 124:e70ca81676fc | 138 | |
bwang | 124:e70ca81676fc | 139 | void SwapMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 124:e70ca81676fc | 140 | if (torque_percent < 0.5f) { |
bwang | 124:e70ca81676fc | 141 | *d = 0.0f; |
bwang | 124:e70ca81676fc | 142 | *q = _iq; |
bwang | 124:e70ca81676fc | 143 | } else { |
bwang | 124:e70ca81676fc | 144 | *d = _id; |
bwang | 124:e70ca81676fc | 145 | *q = 0.0f; |
bwang | 124:e70ca81676fc | 146 | } |
bwang | 160:6948bb7bcabd | 147 | } |
bwang | 160:6948bb7bcabd | 148 | |
bwang | 160:6948bb7bcabd | 149 | void AutoMapper::map(float torque_percent, float w, float *d, float *q) { |
bwang | 160:6948bb7bcabd | 150 | if (torque_percent > 0.99f) { |
bwang | 160:6948bb7bcabd | 151 | _theta += (_phase_high - _phase_low) / _steps; |
bwang | 160:6948bb7bcabd | 152 | torque_percent = 0.0f; |
bwang | 160:6948bb7bcabd | 153 | } |
bwang | 161:19eac809c727 | 154 | |
bwang | 160:6948bb7bcabd | 155 | if (_theta >= _phase_high) { |
bwang | 160:6948bb7bcabd | 156 | *q = 0.0f; |
bwang | 160:6948bb7bcabd | 157 | *d = 0.0f; |
bwang | 160:6948bb7bcabd | 158 | return; |
bwang | 160:6948bb7bcabd | 159 | } |
bwang | 160:6948bb7bcabd | 160 | |
bwang | 160:6948bb7bcabd | 161 | *q = _is * torque_percent * sinf(_theta); |
bwang | 160:6948bb7bcabd | 162 | *d = _is * torque_percent * cosf(_theta); |
bwang | 42:030e0ec4eac5 | 163 | } |