Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Embed:
(wiki syntax)
Show/hide line numbers
DQMapper.cpp
00001 #include "math.h" 00002 00003 #include "DQMapper.h" 00004 #include "prefs.h" 00005 00006 #include "tables.h" 00007 #include "ndtab.h" 00008 #include "nqtab.h" 00009 #include "pdtab.h" 00010 #include "pqtab.h" 00011 #include "zdtab.h" 00012 #include "zqtab.h" 00013 00014 void LutMapper::map(float torque_percent, float w, float *d, float *q) { 00015 w *= _POLE_PAIRS; 00016 bool neg = false; 00017 00018 if (w < 0.f) { 00019 w = -w; 00020 torque_percent = -torque_percent; 00021 neg = true; 00022 } 00023 if (fabs(torque_percent) < 0.02f) { 00024 int index = (int) (w / _W_STEP); 00025 if (index >= ROWS) index = ROWS - 1; 00026 00027 *d = (float) zdtab[index] / 128.f; 00028 *q = (float) zqtab[index] / 128.f; 00029 } else if (torque_percent >= 0.02f) { 00030 int row = (int) (w / _W_STEP); 00031 int col = (int) ((torque_percent - 0.02f) * COLUMNS); 00032 00033 if (row >= ROWS) row = ROWS - 1; 00034 if (col >= COLUMNS) col = COLUMNS - 1; 00035 00036 *d = (float) pdtab[row][col] / 128.f; 00037 *q = (float) pqtab[row][col] / 128.f; 00038 } else { 00039 int row = (int) (w / _W_STEP); 00040 int col = (int) ((-torque_percent - 0.02f) * COLUMNS); 00041 00042 if (row >= ROWS) row = ROWS - 1; 00043 if (col >= COLUMNS) col = COLUMNS - 1; 00044 00045 *d = (float) ndtab[row][col] / 128.f; 00046 *q = (float) nqtab[row][col] / 128.f; 00047 } 00048 00049 if (neg) { 00050 *q = -*q; 00051 } 00052 } 00053 00054 float InterpolatingLutMapper::lookup(short table[][COLUMNS], int row, int col) { 00055 if (row >= ROWS) row = ROWS - 1; 00056 if (col >= COLUMNS) col = COLUMNS - 1; 00057 return (float)table[row][col] / 128.f; 00058 } 00059 00060 float InterpolatingLutMapper::lookup(short *table, int index) { 00061 if (index >= ROWS) index = ROWS - 1; 00062 return (float)table[index] / 128.f; 00063 } 00064 00065 float InterpolatingLutMapper::interp(float a, float b, float eps) { 00066 float delta = eps * (b - a); 00067 return a + delta; 00068 } 00069 00070 float InterpolatingLutMapper::interp(float a, float b, float c, float eps_row, float eps_col) { 00071 float delta_row = eps_row * (b - a); 00072 float delta_col = eps_col * (c - a); 00073 return a + delta_row + delta_col; 00074 } 00075 00076 void InterpolatingLutMapper::map(float torque_percent, float w, float *d, float *q) { 00077 w *= _POLE_PAIRS; 00078 bool neg = false; 00079 00080 if (w < 0.f) { 00081 w = -w; 00082 torque_percent = -torque_percent; 00083 neg = true; 00084 } 00085 if (fabs(torque_percent) < 0.02f) { 00086 int index = (int)(w / _W_STEP); 00087 float eps = w / _W_STEP - index; 00088 00089 *d = interp(lookup(zdtab, index), lookup(zdtab, index + 1), eps); 00090 *q = interp(lookup(zqtab, index), lookup(zqtab, index + 1), eps); 00091 } 00092 else if (torque_percent >= 0.02f) { 00093 int row = (int)(w / _W_STEP); 00094 int col = (int)((torque_percent - 0.02f) * COLUMNS); 00095 00096 float row_eps = w / _W_STEP - row; 00097 float col_eps = (torque_percent - 0.02f) * COLUMNS - col; 00098 00099 *d = interp(lookup(pdtab, row, col), lookup(pdtab, row + 1, col), lookup(pdtab, row, col + 1), row_eps, col_eps); 00100 *q = interp(lookup(pqtab, row, col), lookup(pqtab, row + 1, col), lookup(pqtab, row, col + 1), row_eps, col_eps); 00101 } 00102 else { 00103 int row = (int)(w / _W_STEP); 00104 int col = (int)((-torque_percent - 0.02f) * COLUMNS); 00105 00106 float row_eps = w / _W_STEP - row; 00107 float col_eps = (-torque_percent - 0.02f) * COLUMNS - col; 00108 00109 *d = interp(lookup(ndtab, row, col), lookup(ndtab, row + 1, col), lookup(ndtab, row, col + 1), row_eps, col_eps); 00110 *q = interp(lookup(nqtab, row, col), lookup(nqtab, row + 1, col), lookup(nqtab, row, col + 1), row_eps, col_eps); 00111 } 00112 00113 if (neg) { 00114 *q = -*q; 00115 } 00116 } 00117 00118 void LinearNoFWMapper::map(float torque_percent, float w, float *d, float *q) { 00119 float is = torque_percent * _tmax / _kt; 00120 *d = (-_lambda + sqrtf(_lambda * _lambda + 8 * (_Ld - _Lq) * (_Ld - _Lq) * is * is)) / (4.f * (_Ld - _Lq)); 00121 *q = sqrtf(is * is - *d * *d); 00122 } 00123 00124 void AngleMapper::map(float torque_percent, float w, float *d, float *q) { 00125 *q = _is * torque_percent * sinf(_theta); 00126 *d = _is * torque_percent * cosf(_theta); 00127 } 00128 00129 void DirectMapper::map(float torque_percent, float w, float *d, float *q) { 00130 *d = _id * torque_percent; 00131 *q = _iq * torque_percent; 00132 } 00133 00134 void SwapMapper::map(float torque_percent, float w, float *d, float *q) { 00135 if (torque_percent < 0.5f) { 00136 *d = 0.0f; 00137 *q = _iq; 00138 } else { 00139 *d = _id; 00140 *q = 0.0f; 00141 } 00142 } 00143 00144 void AutoMapper::map(float torque_percent, float w, float *d, float *q) { 00145 if (torque_percent > 0.99f) { 00146 _theta += (_phase_high - _phase_low) / _steps; 00147 torque_percent = 0.0f; 00148 } 00149 00150 if (_theta >= _phase_high) { 00151 *q = 0.0f; 00152 *d = 0.0f; 00153 return; 00154 } 00155 00156 *q = _is * torque_percent * sinf(_theta); 00157 *d = _is * torque_percent * cosf(_theta); 00158 }
Generated on Tue Jul 12 2022 17:58:39 by 1.7.2