Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

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?

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 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 }