Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

DQMapper/DQMapper.cpp@92:a9dac72d8cac, 2017-04-06 (annotated)

Committer:
bwang
Date:
Thu Apr 06 17:33:47 2017 +0000
Revision:
92:a9dac72d8cac
Parent:
48:a1a09c83d42c
Child:
98:1051c4103900
--PwmIn now checks lower bounds for sanity and fall without rise; --switched to edge aligned pwm to work around prius module propagation delays

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 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 42:030e0ec4eac5 58 }