Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

DQMapper/DQMapper.cpp@191:66861311bdcd, 2018-02-10 (annotated)

Committer:
bwang
Date:
Sat Feb 10 03:42:07 2018 +0000
Revision:
191:66861311bdcd
Parent:
186:c18db1e31da6
Child:
192:3152a86cd108
02/09/2018 22:41 - removed ROW, COLUMNS from configurable values, for now. removed a bunch of extra #include's

Who changed what in which revision?

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