Chun Feng Huang / ANTI_SLIP_FUZZY_CONTROL

A new type of anti-slip controller based on TS fuzzy models

ANTI_SLIP_FUZZY_CONTROL.cpp@0:bfcd2371f3dc, 2017-02-23 (annotated)

Committer:
benson516
Date:
Thu Feb 23 11:12:35 2017 +0000
Revision:
0:bfcd2371f3dc
Child:
1:773d8ae11c1a
Test succeed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:bfcd2371f3dc 1 #include "ANTI_SLIP_FUZZY_CONTROL.h"
benson516 0:bfcd2371f3dc 2
benson516 0:bfcd2371f3dc 3 //------------------------------------------//
benson516 0:bfcd2371f3dc 4 // The template for building a library
benson516 0:bfcd2371f3dc 5 // for control system apllication
benson516 0:bfcd2371f3dc 6 //------------------------------------------//
benson516 0:bfcd2371f3dc 7
benson516 0:bfcd2371f3dc 8 // The plant is a (p, n, q) system
benson516 0:bfcd2371f3dc 9 // Dimensions:
benson516 0:bfcd2371f3dc 10 //
benson516 0:bfcd2371f3dc 11 // Inputs, u | States, x | outputs, y
benson516 0:bfcd2371f3dc 12 // p --> n --> q
benson516 0:bfcd2371f3dc 13 //
benson516 0:bfcd2371f3dc 14
benson516 0:bfcd2371f3dc 15 //
benson516 0:bfcd2371f3dc 16 // The number of vertex systems is m_vertex.
benson516 0:bfcd2371f3dc 17 //
benson516 0:bfcd2371f3dc 18
benson516 0:bfcd2371f3dc 19 ANTI_SLIP_FUZZY_CONTROL::ANTI_SLIP_FUZZY_CONTROL(size_t num_state, size_t num_in, size_t num_out, size_t num_vertex, float samplingTime):
benson516 0:bfcd2371f3dc 20 n(num_state), p(num_in), q(num_out), m_vertex(num_vertex), Ts(samplingTime),
benson516 0:bfcd2371f3dc 21 E_out(num_out, vector<float>(num_state,0.0)),
benson516 0:bfcd2371f3dc 22 ver_K_matrix(num_vertex ,vector<vector<float> >(num_in, vector<float>( (num_state + num_out), 0.0)) ),
benson516 0:bfcd2371f3dc 23 SA_r(1.0, 0.0),
benson516 0:bfcd2371f3dc 24 SA_l(1.0, 0.0)
benson516 0:bfcd2371f3dc 25 {
benson516 0:bfcd2371f3dc 26 // Normally, q = p
benson516 0:bfcd2371f3dc 27 if (q > p)
benson516 0:bfcd2371f3dc 28 q = p;
benson516 0:bfcd2371f3dc 29 //
benson516 0:bfcd2371f3dc 30 zeros_n.assign(n, 0.0);
benson516 0:bfcd2371f3dc 31 zeros_p.assign(p, 0.0);
benson516 0:bfcd2371f3dc 32 zeros_q.assign(q, 0.0);
benson516 0:bfcd2371f3dc 33 zeros_nPq.assign((n+q), 0.0); // (n+q)
benson516 0:bfcd2371f3dc 34 zeros_m_vertex.assign(m_vertex, 0.0);
benson516 0:bfcd2371f3dc 35 //
benson516 0:bfcd2371f3dc 36 ones_p.assign(p, 1.0);
benson516 0:bfcd2371f3dc 37
benson516 0:bfcd2371f3dc 38 // States
benson516 0:bfcd2371f3dc 39 // Input signal ---
benson516 0:bfcd2371f3dc 40 states = zeros_n;
benson516 0:bfcd2371f3dc 41 command = zeros_q; // r, commands, q = p
benson516 0:bfcd2371f3dc 42 // Output signal ---
benson516 0:bfcd2371f3dc 43 sys_inputs = zeros_p;
benson516 0:bfcd2371f3dc 44
benson516 0:bfcd2371f3dc 45 // Internal states ---
benson516 0:bfcd2371f3dc 46 ver_u.assign(m_vertex, zeros_p);
benson516 0:bfcd2371f3dc 47 sys_outputs = zeros_q;
benson516 0:bfcd2371f3dc 48 // Integral state
benson516 0:bfcd2371f3dc 49 state_int = zeros_q;
benson516 0:bfcd2371f3dc 50 // Total states, [states; state_int]
benson516 0:bfcd2371f3dc 51 state_total = zeros_nPq;
benson516 0:bfcd2371f3dc 52
benson516 0:bfcd2371f3dc 53
benson516 0:bfcd2371f3dc 54 // The composition ratio of each vertex system
benson516 0:bfcd2371f3dc 55 ver_ratio = zeros_m_vertex; // ver_ratio \in R^m_vertex, its values are in [0, 1]
benson516 0:bfcd2371f3dc 56 ver_ratio[3] = 1.0; // Vertex of no-slip system
benson516 0:bfcd2371f3dc 57
benson516 0:bfcd2371f3dc 58 }
benson516 0:bfcd2371f3dc 59 // Assign Parameters
benson516 0:bfcd2371f3dc 60 void ANTI_SLIP_FUZZY_CONTROL::assign_E_out(float* E_out_in){
benson516 0:bfcd2371f3dc 61 // E_out_in is the pointer of a mutidimentional array with size q by n
benson516 0:bfcd2371f3dc 62 E_out.assign(q, zeros_n);
benson516 0:bfcd2371f3dc 63 //
benson516 0:bfcd2371f3dc 64 for (size_t i = 0; i < q; ++i){
benson516 0:bfcd2371f3dc 65 for (size_t j = 0; j < n; ++j){
benson516 0:bfcd2371f3dc 66 // E_out[i][j] = E_out_in[i][j];
benson516 0:bfcd2371f3dc 67 E_out[i][j] = *E_out_in;
benson516 0:bfcd2371f3dc 68 E_out_in++;
benson516 0:bfcd2371f3dc 69 }
benson516 0:bfcd2371f3dc 70 }
benson516 0:bfcd2371f3dc 71 }
benson516 0:bfcd2371f3dc 72 // 1st controller Parameters (no-slip plant, sys_dVs)
benson516 0:bfcd2371f3dc 73 void ANTI_SLIP_FUZZY_CONTROL::assign_ver_K_matrix(float* ver_K_matrix_in){
benson516 0:bfcd2371f3dc 74 // ver_K_matrix_in is the pointer of a list of mutidimentional array with size p by (n+q)
benson516 0:bfcd2371f3dc 75 ver_K_matrix.assign(m_vertex, vector<vector<float> >(p, zeros_nPq) );
benson516 0:bfcd2371f3dc 76 //
benson516 0:bfcd2371f3dc 77 for (size_t k = 0; k < m_vertex; ++k){
benson516 0:bfcd2371f3dc 78 // For each vertex
benson516 0:bfcd2371f3dc 79 for (size_t i = 0; i < p; ++i){
benson516 0:bfcd2371f3dc 80 for (size_t j = 0; j < (n+q); ++j){
benson516 0:bfcd2371f3dc 81 // (ver_K_matrix[k])[i][j] = ver_K_matrix_in[i][j];
benson516 0:bfcd2371f3dc 82 (ver_K_matrix[k])[i][j] = *ver_K_matrix_in;
benson516 0:bfcd2371f3dc 83 ver_K_matrix_in++;
benson516 0:bfcd2371f3dc 84 }
benson516 0:bfcd2371f3dc 85 }
benson516 0:bfcd2371f3dc 86 //
benson516 0:bfcd2371f3dc 87 }
benson516 0:bfcd2371f3dc 88 //
benson516 0:bfcd2371f3dc 89 }
benson516 0:bfcd2371f3dc 90
benson516 0:bfcd2371f3dc 91
benson516 0:bfcd2371f3dc 92 //
benson516 0:bfcd2371f3dc 93 void ANTI_SLIP_FUZZY_CONTROL::set_ver_ratio(float ratio_ft_right, float ratio_ft_left){
benson516 0:bfcd2371f3dc 94 float one_ratio_ft_right;
benson516 0:bfcd2371f3dc 95 float one_ratio_ft_left;
benson516 0:bfcd2371f3dc 96 // Input Saturation
benson516 0:bfcd2371f3dc 97 ratio_ft_right = SA_r.filter(ratio_ft_right);
benson516 0:bfcd2371f3dc 98 ratio_ft_left = SA_l.filter(ratio_ft_left);
benson516 0:bfcd2371f3dc 99 //
benson516 0:bfcd2371f3dc 100 one_ratio_ft_right = 1.0 - ratio_ft_right;
benson516 0:bfcd2371f3dc 101 one_ratio_ft_left = 1.0 - ratio_ft_left;
benson516 0:bfcd2371f3dc 102 //
benson516 0:bfcd2371f3dc 103 ver_ratio[0] = one_ratio_ft_right * one_ratio_ft_left;
benson516 0:bfcd2371f3dc 104 ver_ratio[1] = ratio_ft_right * one_ratio_ft_left;
benson516 0:bfcd2371f3dc 105 ver_ratio[2] = one_ratio_ft_right * ratio_ft_left;
benson516 0:bfcd2371f3dc 106 ver_ratio[3] = ratio_ft_right * ratio_ft_left;
benson516 0:bfcd2371f3dc 107 }
benson516 0:bfcd2371f3dc 108 //
benson516 0:bfcd2371f3dc 109 void ANTI_SLIP_FUZZY_CONTROL::fullStateFeedBack_calc(bool enable){
benson516 0:bfcd2371f3dc 110
benson516 0:bfcd2371f3dc 111 // Control law
benson516 0:bfcd2371f3dc 112 if (enable){
benson516 0:bfcd2371f3dc 113 // Get the state_total
benson516 0:bfcd2371f3dc 114 get_state_total();
benson516 0:bfcd2371f3dc 115
benson516 0:bfcd2371f3dc 116
benson516 0:bfcd2371f3dc 117 /*
benson516 0:bfcd2371f3dc 118 // Slower solution, record the outout of each vertex controller
benson516 0:bfcd2371f3dc 119 sys_inputs = zeros_p;
benson516 0:bfcd2371f3dc 120 //
benson516 0:bfcd2371f3dc 121 for (size_t k = 0; k < m_vertex; ++k){
benson516 0:bfcd2371f3dc 122 ver_u[k] = Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), -1.0 );
benson516 0:bfcd2371f3dc 123 Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( ver_u[k], ver_ratio[k]), false); // +=
benson516 0:bfcd2371f3dc 124 }
benson516 0:bfcd2371f3dc 125 */
benson516 0:bfcd2371f3dc 126
benson516 0:bfcd2371f3dc 127
benson516 0:bfcd2371f3dc 128
benson516 0:bfcd2371f3dc 129 // Faster solution, no recording the output of each vertex controller
benson516 0:bfcd2371f3dc 130 sys_inputs = zeros_p;
benson516 0:bfcd2371f3dc 131 //
benson516 0:bfcd2371f3dc 132
benson516 0:bfcd2371f3dc 133 for (size_t k = 0; k < m_vertex; ++k){
benson516 0:bfcd2371f3dc 134 // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
benson516 0:bfcd2371f3dc 135 Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
benson516 0:bfcd2371f3dc 136 }
benson516 0:bfcd2371f3dc 137
benson516 0:bfcd2371f3dc 138
benson516 0:bfcd2371f3dc 139
benson516 0:bfcd2371f3dc 140 /*
benson516 0:bfcd2371f3dc 141 for (size_t k = 3; k < 4; ++k){
benson516 0:bfcd2371f3dc 142 // sys_inputs -= ver_ratio[k]*(ver_K_matrix[k]*state_total);
benson516 0:bfcd2371f3dc 143 Get_VectorIncrement(sys_inputs, Get_VectorScalarMultiply( Mat_multiply_Vec(ver_K_matrix[k], state_total), ver_ratio[k] ), true); // -=
benson516 0:bfcd2371f3dc 144 }
benson516 0:bfcd2371f3dc 145 */
benson516 0:bfcd2371f3dc 146
benson516 0:bfcd2371f3dc 147
benson516 0:bfcd2371f3dc 148
benson516 0:bfcd2371f3dc 149 //
benson516 0:bfcd2371f3dc 150 }else{
benson516 0:bfcd2371f3dc 151 state_total = zeros_nPq;
benson516 0:bfcd2371f3dc 152 sys_inputs = zeros_p;
benson516 0:bfcd2371f3dc 153 }
benson516 0:bfcd2371f3dc 154
benson516 0:bfcd2371f3dc 155 // Integral action
benson516 0:bfcd2371f3dc 156 get_integral(enable);
benson516 0:bfcd2371f3dc 157 }
benson516 0:bfcd2371f3dc 158
benson516 0:bfcd2371f3dc 159 // Private functions
benson516 0:bfcd2371f3dc 160 // Calculate the sys_outputs
benson516 0:bfcd2371f3dc 161 void ANTI_SLIP_FUZZY_CONTROL::get_sys_outputs(void){ // Calculate the sys_outputs from states, by mutiplying E_out
benson516 0:bfcd2371f3dc 162 // sys_outputs = E_out*states
benson516 0:bfcd2371f3dc 163 // Mat_multiply_Vec(sys_outputs, E_out, states);
benson516 0:bfcd2371f3dc 164 sys_outputs = Mat_multiply_Vec(E_out, states);
benson516 0:bfcd2371f3dc 165 }
benson516 0:bfcd2371f3dc 166 // Calculate the Integral
benson516 0:bfcd2371f3dc 167 void ANTI_SLIP_FUZZY_CONTROL::get_integral(bool enable){ // Calculate the state_int
benson516 0:bfcd2371f3dc 168 //
benson516 0:bfcd2371f3dc 169 // Calculate the sys_outputs
benson516 0:bfcd2371f3dc 170 get_sys_outputs();
benson516 0:bfcd2371f3dc 171
benson516 0:bfcd2371f3dc 172 // Integral action
benson516 0:bfcd2371f3dc 173 // state_int += sys_outputs - command
benson516 0:bfcd2371f3dc 174 if (enable){
benson516 0:bfcd2371f3dc 175 Get_VectorIncrement(state_int, Get_VectorScalarMultiply(Get_VectorPlus(sys_outputs,command,true),Ts) ,false); // +=
benson516 0:bfcd2371f3dc 176 }else{
benson516 0:bfcd2371f3dc 177 state_int = zeros_q;
benson516 0:bfcd2371f3dc 178 }
benson516 0:bfcd2371f3dc 179 }
benson516 0:bfcd2371f3dc 180 // Concatenate the state and state_int
benson516 0:bfcd2371f3dc 181 void ANTI_SLIP_FUZZY_CONTROL::get_state_total(void){ // Total states, [states; state_int]
benson516 0:bfcd2371f3dc 182 //
benson516 0:bfcd2371f3dc 183 size_t idx = 0;
benson516 0:bfcd2371f3dc 184 // states
benson516 0:bfcd2371f3dc 185 for (size_t i = 0; i < n; ++i){
benson516 0:bfcd2371f3dc 186 state_total[idx] = states[i];
benson516 0:bfcd2371f3dc 187 idx++;
benson516 0:bfcd2371f3dc 188 }
benson516 0:bfcd2371f3dc 189 // state_int
benson516 0:bfcd2371f3dc 190 for (size_t i = 0; i < q; ++i){
benson516 0:bfcd2371f3dc 191 state_total[idx] = state_int[i];
benson516 0:bfcd2371f3dc 192 idx++;
benson516 0:bfcd2371f3dc 193 }
benson516 0:bfcd2371f3dc 194 }
benson516 0:bfcd2371f3dc 195
benson516 0:bfcd2371f3dc 196 // Utilities
benson516 0:bfcd2371f3dc 197 void ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:bfcd2371f3dc 198
benson516 0:bfcd2371f3dc 199 // Size check
benson516 0:bfcd2371f3dc 200 if (v_out.size() != m_left.size()){
benson516 0:bfcd2371f3dc 201 v_out.resize(m_left.size());
benson516 0:bfcd2371f3dc 202 }
benson516 0:bfcd2371f3dc 203
benson516 0:bfcd2371f3dc 204 /*
benson516 0:bfcd2371f3dc 205 // Iterators
benson516 0:bfcd2371f3dc 206 static vector<float>::iterator it_out;
benson516 0:bfcd2371f3dc 207 static vector<float>::iterator it_m_row;
benson516 0:bfcd2371f3dc 208 static vector<float>::iterator it_v;
benson516 0:bfcd2371f3dc 209
benson516 0:bfcd2371f3dc 210 //
benson516 0:bfcd2371f3dc 211 it_out = v_out.begin();
benson516 0:bfcd2371f3dc 212 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 213 *it_out = 0.0;
benson516 0:bfcd2371f3dc 214 it_m_row = vector<vector<float> >(m_left)[i].begin();
benson516 0:bfcd2371f3dc 215 it_v = vector<float>(v_right).begin();
benson516 0:bfcd2371f3dc 216 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:bfcd2371f3dc 217 // *it_out += m_left[i][j] * v_right[j];
benson516 0:bfcd2371f3dc 218 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:bfcd2371f3dc 219 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 220 }else{
benson516 0:bfcd2371f3dc 221 // (*it_out) += 0.0
benson516 0:bfcd2371f3dc 222 }
benson516 0:bfcd2371f3dc 223 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 224 //
benson516 0:bfcd2371f3dc 225 it_m_row++;
benson516 0:bfcd2371f3dc 226 it_v++;
benson516 0:bfcd2371f3dc 227 }
benson516 0:bfcd2371f3dc 228 it_out++;
benson516 0:bfcd2371f3dc 229 }
benson516 0:bfcd2371f3dc 230 */
benson516 0:bfcd2371f3dc 231
benson516 0:bfcd2371f3dc 232 // Indexing
benson516 0:bfcd2371f3dc 233 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 234 //
benson516 0:bfcd2371f3dc 235 v_out[i] = 0.0;
benson516 0:bfcd2371f3dc 236 //
benson516 0:bfcd2371f3dc 237 for (size_t j = 0; j < v_right.size(); ++j){
benson516 0:bfcd2371f3dc 238 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 0:bfcd2371f3dc 239 v_out[i] += m_left[i][j]*v_right[j];
benson516 0:bfcd2371f3dc 240 }
benson516 0:bfcd2371f3dc 241 }
benson516 0:bfcd2371f3dc 242
benson516 0:bfcd2371f3dc 243 }
benson516 0:bfcd2371f3dc 244 vector<float> ANTI_SLIP_FUZZY_CONTROL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 0:bfcd2371f3dc 245 vector<float> v_out;
benson516 0:bfcd2371f3dc 246 // Size check
benson516 0:bfcd2371f3dc 247 if (v_out.size() != m_left.size()){
benson516 0:bfcd2371f3dc 248 v_out.resize(m_left.size());
benson516 0:bfcd2371f3dc 249 }
benson516 0:bfcd2371f3dc 250
benson516 0:bfcd2371f3dc 251 /*
benson516 0:bfcd2371f3dc 252 // Iterators
benson516 0:bfcd2371f3dc 253 static vector<float>::iterator it_out;
benson516 0:bfcd2371f3dc 254 static vector<const float>::iterator it_m_row;
benson516 0:bfcd2371f3dc 255 static vector<const float>::iterator it_v;
benson516 0:bfcd2371f3dc 256 //
benson516 0:bfcd2371f3dc 257 it_out = v_out.begin();
benson516 0:bfcd2371f3dc 258 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 259 *it_out = 0.0;
benson516 0:bfcd2371f3dc 260 it_m_row = m_left[i].begin();
benson516 0:bfcd2371f3dc 261 it_v = v_right.begin();
benson516 0:bfcd2371f3dc 262 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 0:bfcd2371f3dc 263 // *it_out += m_left[i][j] * v_right[j];
benson516 0:bfcd2371f3dc 264 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 0:bfcd2371f3dc 265 (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 266 }else{
benson516 0:bfcd2371f3dc 267 // (*it_out) += 0.0
benson516 0:bfcd2371f3dc 268 }
benson516 0:bfcd2371f3dc 269 // (*it_out) += (*it_m_row) * (*it_v);
benson516 0:bfcd2371f3dc 270 //
benson516 0:bfcd2371f3dc 271 it_m_row++;
benson516 0:bfcd2371f3dc 272 it_v++;
benson516 0:bfcd2371f3dc 273 }
benson516 0:bfcd2371f3dc 274 it_out++;
benson516 0:bfcd2371f3dc 275 }
benson516 0:bfcd2371f3dc 276 */
benson516 0:bfcd2371f3dc 277
benson516 0:bfcd2371f3dc 278 // Indexing
benson516 0:bfcd2371f3dc 279 for (size_t i = 0; i < m_left.size(); ++i){
benson516 0:bfcd2371f3dc 280 //
benson516 0:bfcd2371f3dc 281 v_out[i] = 0.0;
benson516 0:bfcd2371f3dc 282 //
benson516 0:bfcd2371f3dc 283 for (size_t j = 0; j < v_right.size(); ++j){
benson516 0:bfcd2371f3dc 284 if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
benson516 0:bfcd2371f3dc 285 v_out[i] += m_left[i][j]*v_right[j];
benson516 0:bfcd2371f3dc 286 }
benson516 0:bfcd2371f3dc 287 }
benson516 0:bfcd2371f3dc 288
benson516 0:bfcd2371f3dc 289 return v_out;
benson516 0:bfcd2371f3dc 290 }
benson516 0:bfcd2371f3dc 291 vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 0:bfcd2371f3dc 292 {
benson516 0:bfcd2371f3dc 293 static vector<float> v_c;
benson516 0:bfcd2371f3dc 294 // Size check
benson516 0:bfcd2371f3dc 295 if (v_c.size() != v_a.size()){
benson516 0:bfcd2371f3dc 296 v_c.resize(v_a.size());
benson516 0:bfcd2371f3dc 297 }
benson516 0:bfcd2371f3dc 298 //
benson516 0:bfcd2371f3dc 299 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:bfcd2371f3dc 300 if (is_minus){
benson516 0:bfcd2371f3dc 301 v_c[i] = v_a[i] - v_b[i];
benson516 0:bfcd2371f3dc 302 }else{
benson516 0:bfcd2371f3dc 303 v_c[i] = v_a[i] + v_b[i];
benson516 0:bfcd2371f3dc 304 }
benson516 0:bfcd2371f3dc 305 }
benson516 0:bfcd2371f3dc 306 return v_c;
benson516 0:bfcd2371f3dc 307 }
benson516 0:bfcd2371f3dc 308 vector<float> ANTI_SLIP_FUZZY_CONTROL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 0:bfcd2371f3dc 309 {
benson516 0:bfcd2371f3dc 310 static vector<float> v_c;
benson516 0:bfcd2371f3dc 311 // Size check
benson516 0:bfcd2371f3dc 312 if (v_c.size() != v_a.size()){
benson516 0:bfcd2371f3dc 313 v_c.resize(v_a.size());
benson516 0:bfcd2371f3dc 314 }
benson516 0:bfcd2371f3dc 315 // for pure negative
benson516 0:bfcd2371f3dc 316 if (scale == -1.0){
benson516 0:bfcd2371f3dc 317 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:bfcd2371f3dc 318 v_c[i] = -v_a[i];
benson516 0:bfcd2371f3dc 319 }
benson516 0:bfcd2371f3dc 320 return v_c;
benson516 0:bfcd2371f3dc 321 }
benson516 0:bfcd2371f3dc 322 // else
benson516 0:bfcd2371f3dc 323 for (size_t i = 0; i < v_a.size(); ++i){
benson516 0:bfcd2371f3dc 324 v_c[i] = scale*v_a[i];
benson516 0:bfcd2371f3dc 325
benson516 0:bfcd2371f3dc 326 }
benson516 0:bfcd2371f3dc 327 return v_c;
benson516 0:bfcd2371f3dc 328 }
benson516 0:bfcd2371f3dc 329 // Increment
benson516 0:bfcd2371f3dc 330 void ANTI_SLIP_FUZZY_CONTROL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 0:bfcd2371f3dc 331 // Size check
benson516 0:bfcd2371f3dc 332 if (v_a.size() != v_b.size()){
benson516 0:bfcd2371f3dc 333 v_a.resize(v_b.size());
benson516 0:bfcd2371f3dc 334 }
benson516 0:bfcd2371f3dc 335 //
benson516 0:bfcd2371f3dc 336 if (is_minus){ // -=
benson516 0:bfcd2371f3dc 337 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:bfcd2371f3dc 338 v_a[i] -= v_b[i];
benson516 0:bfcd2371f3dc 339 }
benson516 0:bfcd2371f3dc 340 }else{ // +=
benson516 0:bfcd2371f3dc 341 for (size_t i = 0; i < v_b.size(); ++i){
benson516 0:bfcd2371f3dc 342 v_a[i] += v_b[i];
benson516 0:bfcd2371f3dc 343 }
benson516 0:bfcd2371f3dc 344 }
benson516 0:bfcd2371f3dc 345
benson516 0:bfcd2371f3dc 346 }