Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

DQMapper/DQMapper.cpp@45:cf8ad81fb0f0, 2017-01-08 (annotated)

Committer:
bwang
Date:
Sun Jan 08 09:07:39 2017 +0000
Revision:
45:cf8ad81fb0f0
Parent:
44:3fd6a43b91f0
Child:
48:a1a09c83d42c
this revision uses the naive (no FW) d/q code and a linear throttle map

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 42:030e0ec4eac5 7 #include "dtab_n_w25.h"
bwang 42:030e0ec4eac5 8 #include "dtab_p_w25.h"
bwang 42:030e0ec4eac5 9 #include "qtab_n_w25.h"
bwang 42:030e0ec4eac5 10 #include "qtab_p_w25.h"
bwang 42:030e0ec4eac5 11
bwang 42:030e0ec4eac5 12 void LutMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 42:030e0ec4eac5 13 w *= POLE_PAIRS;
bwang 42:030e0ec4eac5 14 if (w < 0.f) {
bwang 42:030e0ec4eac5 15 w = -w;
bwang 42:030e0ec4eac5 16 torque_percent = -torque_percent;
bwang 42:030e0ec4eac5 17 }
bwang 42:030e0ec4eac5 18 if (torque_percent > 0) {
bwang 42:030e0ec4eac5 19 int row = (int) (w / W_STEP);
bwang 42:030e0ec4eac5 20 int col = (int) (torque_percent * COLUMNS);
bwang 42:030e0ec4eac5 21
bwang 42:030e0ec4eac5 22 if (row >= ROWS) row = ROWS - 1;
bwang 42:030e0ec4eac5 23 if (col >= COLUMNS) col = COLUMNS - 1;
bwang 42:030e0ec4eac5 24
bwang 42:030e0ec4eac5 25 *d = (float) dtab_p_w25[row][col] / 128.f;
bwang 42:030e0ec4eac5 26 *q = (float) qtab_p_w25[row][col] / 128.f;
bwang 42:030e0ec4eac5 27 } else {
bwang 42:030e0ec4eac5 28 int row = (int) (w / W_STEP);
bwang 42:030e0ec4eac5 29 int col = (int) (-torque_percent * COLUMNS);
bwang 42:030e0ec4eac5 30
bwang 42:030e0ec4eac5 31 if (row >= ROWS) row = ROWS - 1;
bwang 42:030e0ec4eac5 32 if (col >= COLUMNS) col = COLUMNS - 1;
bwang 42:030e0ec4eac5 33
bwang 42:030e0ec4eac5 34 *d = (float) dtab_n_w25[row][col] / 128.f;
bwang 42:030e0ec4eac5 35 *q = (float) qtab_n_w25[row][col] / 128.f;
bwang 42:030e0ec4eac5 36 }
bwang 44:3fd6a43b91f0 37 }
bwang 44:3fd6a43b91f0 38
bwang 44:3fd6a43b91f0 39 void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) {
bwang 45:cf8ad81fb0f0 40 float is = torque_percent * _tmax / _kt;
bwang 45:cf8ad81fb0f0 41 *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq));
bwang 45:cf8ad81fb0f0 42 *q = sqrtf(is * is - *d * *d);
bwang 42:030e0ec4eac5 43 }