Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

DQMapper/DQMapper.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
192:3152a86cd108 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 45:cf8ad81fb0f0 1 #include "math.h"
bwang 42:030e0ec4eac5 2
bwang 192:3152a86cd108 3 #include "DQMapper.h"
bwang 192:3152a86cd108 4 #include "prefs.h"
bwang 185:5c102874b490 5
bwang 191:66861311bdcd 6 #include "tables.h"
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 192:3152a86cd108 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 192:3152a86cd108 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 192:3152a86cd108 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 192:3152a86cd108 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 171:3f1d1792757c 54 float InterpolatingLutMapper::lookup(short table[][COLUMNS], int row, int col) {
bwang 171:3f1d1792757c 55 if (row >= ROWS) row = ROWS - 1;
bwang 171:3f1d1792757c 56 if (col >= COLUMNS) col = COLUMNS - 1;
bwang 171:3f1d1792757c 57 return (float)table[row][col] / 128.f;
bwang 171:3f1d1792757c 58 }
bwang 171:3f1d1792757c 59
bwang 171:3f1d1792757c 60 float InterpolatingLutMapper::lookup(short *table, int index) {
bwang 171:3f1d1792757c 61 if (index >= ROWS) index = ROWS - 1;
bwang 171:3f1d1792757c 62 return (float)table[index] / 128.f;
bwang 171:3f1d1792757c 63 }
bwang 171:3f1d1792757c 64
bwang 171:3f1d1792757c 65 float InterpolatingLutMapper::interp(float a, float b, float eps) {
bwang 171:3f1d1792757c 66 float delta = eps * (b - a);
bwang 171:3f1d1792757c 67 return a + delta;
bwang 171:3f1d1792757c 68 }
bwang 171:3f1d1792757c 69
bwang 171:3f1d1792757c 70 float InterpolatingLutMapper::interp(float a, float b, float c, float eps_row, float eps_col) {
bwang 171:3f1d1792757c 71 float delta_row = eps_row * (b - a);
bwang 171:3f1d1792757c 72 float delta_col = eps_col * (c - a);
bwang 171:3f1d1792757c 73 return a + delta_row + delta_col;
bwang 171:3f1d1792757c 74 }
bwang 171:3f1d1792757c 75
bwang 171:3f1d1792757c 76 void InterpolatingLutMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 192:3152a86cd108 77 w *= _POLE_PAIRS;
bwang 171:3f1d1792757c 78 bool neg = false;
bwang 171:3f1d1792757c 79
bwang 171:3f1d1792757c 80 if (w < 0.f) {
bwang 171:3f1d1792757c 81 w = -w;
bwang 171:3f1d1792757c 82 torque_percent = -torque_percent;
bwang 171:3f1d1792757c 83 neg = true;
bwang 171:3f1d1792757c 84 }
bwang 171:3f1d1792757c 85 if (fabs(torque_percent) < 0.02f) {
bwang 192:3152a86cd108 86 int index = (int)(w / _W_STEP);
bwang 192:3152a86cd108 87 float eps = w / _W_STEP - index;
bwang 171:3f1d1792757c 88
bwang 171:3f1d1792757c 89 *d = interp(lookup(zdtab, index), lookup(zdtab, index + 1), eps);
bwang 171:3f1d1792757c 90 *q = interp(lookup(zqtab, index), lookup(zqtab, index + 1), eps);
bwang 171:3f1d1792757c 91 }
bwang 171:3f1d1792757c 92 else if (torque_percent >= 0.02f) {
bwang 192:3152a86cd108 93 int row = (int)(w / _W_STEP);
bwang 171:3f1d1792757c 94 int col = (int)((torque_percent - 0.02f) * COLUMNS);
bwang 171:3f1d1792757c 95
bwang 192:3152a86cd108 96 float row_eps = w / _W_STEP - row;
bwang 171:3f1d1792757c 97 float col_eps = (torque_percent - 0.02f) * COLUMNS - col;
bwang 171:3f1d1792757c 98
bwang 171:3f1d1792757c 99 *d = interp(lookup(pdtab, row, col), lookup(pdtab, row + 1, col), lookup(pdtab, row, col + 1), row_eps, col_eps);
bwang 171:3f1d1792757c 100 *q = interp(lookup(pqtab, row, col), lookup(pqtab, row + 1, col), lookup(pqtab, row, col + 1), row_eps, col_eps);
bwang 171:3f1d1792757c 101 }
bwang 171:3f1d1792757c 102 else {
bwang 192:3152a86cd108 103 int row = (int)(w / _W_STEP);
bwang 171:3f1d1792757c 104 int col = (int)((-torque_percent - 0.02f) * COLUMNS);
bwang 171:3f1d1792757c 105
bwang 192:3152a86cd108 106 float row_eps = w / _W_STEP - row;
bwang 171:3f1d1792757c 107 float col_eps = (-torque_percent - 0.02f) * COLUMNS - col;
bwang 171:3f1d1792757c 108
bwang 171:3f1d1792757c 109 *d = interp(lookup(ndtab, row, col), lookup(ndtab, row + 1, col), lookup(ndtab, row, col + 1), row_eps, col_eps);
bwang 171:3f1d1792757c 110 *q = interp(lookup(nqtab, row, col), lookup(nqtab, row + 1, col), lookup(nqtab, row, col + 1), row_eps, col_eps);
bwang 171:3f1d1792757c 111 }
bwang 171:3f1d1792757c 112
bwang 171:3f1d1792757c 113 if (neg) {
bwang 171:3f1d1792757c 114 *q = -*q;
bwang 171:3f1d1792757c 115 }
bwang 171:3f1d1792757c 116 }
bwang 171:3f1d1792757c 117
bwang 44:3fd6a43b91f0 118 void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 45:cf8ad81fb0f0 119 float is = torque_percent * _tmax / _kt;
bwang 192:3152a86cd108 120 *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (_Ld - _Lq) * (_Ld - _Lq) * is * is)) / (4.f * (_Ld - _Lq));
bwang 45:cf8ad81fb0f0 121 *q = sqrtf(is * is - *d * *d);
bwang 98:1051c4103900 122 }
bwang 98:1051c4103900 123
bwang 98:1051c4103900 124 void AngleMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 98:1051c4103900 125 *q = _is * torque_percent * sinf(_theta);
bwang 98:1051c4103900 126 *d = _is * torque_percent * cosf(_theta);
bwang 124:e70ca81676fc 127 }
bwang 124:e70ca81676fc 128
bwang 124:e70ca81676fc 129 void DirectMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 124:e70ca81676fc 130 *d = _id * torque_percent;
bwang 124:e70ca81676fc 131 *q = _iq * torque_percent;
bwang 124:e70ca81676fc 132 }
bwang 124:e70ca81676fc 133
bwang 124:e70ca81676fc 134 void SwapMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 124:e70ca81676fc 135 if (torque_percent < 0.5f) {
bwang 124:e70ca81676fc 136 *d = 0.0f;
bwang 124:e70ca81676fc 137 *q = _iq;
bwang 124:e70ca81676fc 138 } else {
bwang 124:e70ca81676fc 139 *d = _id;
bwang 124:e70ca81676fc 140 *q = 0.0f;
bwang 124:e70ca81676fc 141 }
bwang 160:6948bb7bcabd 142 }
bwang 160:6948bb7bcabd 143
bwang 160:6948bb7bcabd 144 void AutoMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 160:6948bb7bcabd 145 if (torque_percent > 0.99f) {
bwang 160:6948bb7bcabd 146 _theta += (_phase_high - _phase_low) / _steps;
bwang 160:6948bb7bcabd 147 torque_percent = 0.0f;
bwang 160:6948bb7bcabd 148 }
bwang 161:19eac809c727 149
bwang 160:6948bb7bcabd 150 if (_theta >= _phase_high) {
bwang 160:6948bb7bcabd 151 *q = 0.0f;
bwang 160:6948bb7bcabd 152 *d = 0.0f;
bwang 160:6948bb7bcabd 153 return;
bwang 160:6948bb7bcabd 154 }
bwang 160:6948bb7bcabd 155
bwang 160:6948bb7bcabd 156 *q = _is * torque_percent * sinf(_theta);
bwang 160:6948bb7bcabd 157 *d = _is * torque_percent * cosf(_theta);
bwang 42:030e0ec4eac5 158 }