Prueba
Embed:
(wiki syntax)
Show/hide line numbers
Robot.cpp
00001 #include "Robot.h" 00002 00003 00004 Links Link[5] ={REVOLUTION,REVOLUTION,REVOLUTION,REVOLUTION};//REVOLUTION 00005 00006 float dyn_mat [4][10]={ 00007 // Ix Iy Iz Ixy Ixz Iyz mx my mz mass 00008 {0.0056F, 0.0150F, 0.0150F, 1, 3, 2, -0.8200F, 0, 0, 2}, 00009 {0.0028F, 0.0056F, 0.0056F, 0, 0, 0, -0.1950F, 0, 0, 1}, 00010 {0, 0, 0.0003F, 0, 0, 0, 0, 0, 0.0013F, 0.1000F}, 00011 {0, 0, 0.0001F, 0, 0, 0, 0, 0, -0.0019F, 0.0500F} 00012 }; 00013 float ToolMatrix[4][4]={ 00014 {1, 2, 3, 10}, 00015 {4, 5, 6, 11}, 00016 {7, 8, 9, 12}, 00017 {0, 0, 0, 1} 00018 }; 00019 float BaseMatrix[4][4]={ 00020 {1, 2, 3, 10}, 00021 {4, 5, 6, 11}, 00022 {7, 8, 9, 12}, 00023 {0, 0, 0, 1} 00024 }; 00025 00026 const float DH_Table[DOF*4]={ 00027 /* theta alpha d a */ 00028 0, 1.5708F, 0, 0, 00029 0, 0, 0, 0.3000F, 00030 0, 0, 0, 0.3260F, 00031 0.1000F, 1.5708F, 0, 0 00032 }; 00033 00034 const float ControllerGain[DOF*3]={ 00035 /* KP KD KI*/ 00036 /*1*/ 0, 0, 0, 00037 /*2*/ 0, 0, 0, 00038 /*3*/ 0, 0, 0, 00039 /*4*/ 0, 0, 0 00040 }; 00041 00042 00043 00044 void init(struct Robot* _Robot,float _ToolMatrix[4][4], float _BaseMatrix[4][4]){ 00045 int VariablesInicialization=1; 00046 // initDynamicMatrix(); 00047 //_Robot->ForwardKinematics_x = ForwardKinematics; 00048 //_Robot->InverseKinematics_x = InverseKinematics; 00049 00050 for (int i=0; i<DOF;i++){ 00051 // _Robot->_Motor[DOF].Degrees=0; 00052 _Robot->_Eslabon[i]._DH.Type = Link[i]; 00053 _Robot->_Eslabon[i]._DH.theta = DH_Table[((i*4)+0)]; 00054 _Robot->_Eslabon[i]._DH.alpha = DH_Table[((i*4)+1)]; 00055 _Robot->_Eslabon[i]._DH.d = DH_Table[((i*4)+2)]; 00056 _Robot->_Eslabon[i]._DH.a = DH_Table[((i*4)+3)]; 00057 00058 _Robot->_Eslabon[i].Inertia_tensor[0][0] = dyn_mat[i][1]+dyn_mat[i][2]; 00059 _Robot->_Eslabon[i].Inertia_tensor[0][1] = dyn_mat[i][3]; 00060 _Robot->_Eslabon[i].Inertia_tensor[0][2] = dyn_mat[i][4]; 00061 00062 _Robot->_Eslabon[i].Inertia_tensor[1][0] = dyn_mat[i][3]; 00063 _Robot->_Eslabon[i].Inertia_tensor[1][1] = dyn_mat[i][0]+dyn_mat[i][2]; 00064 _Robot->_Eslabon[i].Inertia_tensor[1][2] = dyn_mat[i][5]; 00065 00066 _Robot->_Eslabon[i].Inertia_tensor[2][0] = dyn_mat[i][4]; 00067 _Robot->_Eslabon[i].Inertia_tensor[2][1] = dyn_mat[i][5]; 00068 _Robot->_Eslabon[i].Inertia_tensor[2][2] = dyn_mat[i][1]+dyn_mat[i][0]; 00069 00070 _Robot->_Eslabon[i].Cent_Gravity[0] = dyn_mat[i][6]; 00071 _Robot->_Eslabon[i].Cent_Gravity[1] = dyn_mat[i][7]; 00072 _Robot->_Eslabon[i].Cent_Gravity[2] = dyn_mat[i][8]; 00073 00074 _Robot->_Eslabon[i].Mass = dyn_mat[i][9]; 00075 /**/ 00076 for(int x=0;x<4;x++) { 00077 for (int y = 0; y < 4; y++) { 00078 if(VariablesInicialization) { 00079 _Robot->ToolMatrix[x][y] = _ToolMatrix[x][y]; 00080 _Robot->BaseMatrix.m[x][y] = BaseMatrix[x][y]; 00081 } 00082 _Robot->_Eslabon[i]._DH.A0.m[x][y]=0; 00083 } 00084 } 00085 VariablesInicialization=0; 00086 } 00087 } 00088 void ForwardKinematics(struct Robot* _Robot,float thetaOffset[DOF]){ 00089 float STheta,CTheta,SAlpha,CAlpha,theta,d; 00090 for (int i=0; i<DOF;i++) { 00091 if (_Robot->_Eslabon[i]._DH.Type == REVOLUTION) { 00092 theta = thetaOffset[i]; 00093 d = _Robot->_Eslabon[i]._DH.d; 00094 }else{ 00095 d = thetaOffset[i]; 00096 theta = _Robot->_Eslabon[i]._DH.theta; 00097 } 00098 STheta = sinf(theta); 00099 CTheta = cosf(theta); 00100 SAlpha = sinf( _Robot->_Eslabon[i]._DH.alpha); 00101 CAlpha = cosf( _Robot->_Eslabon[i]._DH.alpha); 00102 if( (CAlpha>-0.000001F)&&(CAlpha<0.000001F)){ 00103 CAlpha = 0; 00104 } 00105 if( (SAlpha>-0.000001F)&&(SAlpha<0.000001F)){ 00106 SAlpha = 0; 00107 } 00108 if( (STheta>-0.000001F)&&(STheta<0.000001F)){ 00109 STheta = 0; 00110 } 00111 if( (CTheta>-0.000001F)&&(CTheta<0.000001F)){ 00112 CTheta = 0; 00113 } 00114 for (int x = 0; x < 4; x++) { 00115 for (int y = 0; y < 4; y++) { 00116 if (x == 0 && y == 0) { 00117 _Robot->_Eslabon[i]._DH.A0.m [x][y] = CTheta; 00118 } else if (x == 0 && y == 1) { 00119 _Robot->_Eslabon[i]._DH.A0.m [x][y] = -1 * CAlpha * STheta; 00120 } else if (x == 0 && y == 2) { 00121 _Robot->_Eslabon[i]._DH.A0.m [x][y] = SAlpha * STheta; 00122 } else if (x == 0 && y == 3) { 00123 _Robot->_Eslabon[i]._DH.A0.m [x][y] = _Robot->_Eslabon[i]._DH.a * CTheta; 00124 } else if (x == 1 && y == 0) { 00125 _Robot->_Eslabon[i]._DH.A0.m [x][y] = STheta; 00126 } else if (x == 1 && y == 1) { 00127 _Robot->_Eslabon[i]._DH.A0.m [x][y] = CAlpha * CTheta; 00128 } else if (x == 1 && y == 2) { 00129 _Robot->_Eslabon[i]._DH.A0.m [x][y] = -1 * SAlpha * CTheta; 00130 } else if (x == 1 && y == 3) { 00131 _Robot->_Eslabon[i]._DH.A0.m [x][y] = _Robot->_Eslabon[i]._DH.a * STheta; 00132 } else if (x == 2 && y == 1) { 00133 _Robot->_Eslabon[i]._DH.A0.m [x][y] = SAlpha; 00134 } else if (x == 2 && y == 2) { 00135 _Robot->_Eslabon[i]._DH.A0.m [x][y] = CAlpha; 00136 } else if (x == 2 && y == 3) { 00137 _Robot->_Eslabon[i]._DH.A0.m [x][y] = d; 00138 } else if (x == 3 && y == 3) { 00139 _Robot->_Eslabon[i]._DH.A0.m [x][y] = 1; 00140 } 00141 else{ 00142 _Robot->_Eslabon[i]._DH.A0.m [x][y]=0; 00143 } 00144 if( (_Robot->_Eslabon[i]._DH.A0.m [x][y]>-0.000001F)&&(_Robot->_Eslabon[i]._DH.A0.m [x][y])<0.000001F){ 00145 _Robot->_Eslabon[i]._DH.A0.m [x][y]=0; 00146 } 00147 00148 } 00149 } 00150 } 00151 } 00152 void InverseKinematics(struct Robot* Robot1, struct End_Effector* EFF_q0,float q[3]){ 00153 /*USER SPECIFICATIONS*/ 00154 float Xf = EFF_q0->_Position.x; 00155 float Yf = EFF_q0->_Position.y; 00156 float fi = EFF_q0->_Orientation.pitch; 00157 float theta = EFF_q0->_Orientation.roll; 00158 00159 float L1 = Robot1->_Eslabon[0].Lenght; 00160 float L2 = Robot1->_Eslabon[1].Lenght; 00161 float L3 = Robot1->_Eslabon[2].Lenght; 00162 float L4 = Robot1->_Eslabon[3].Lenght; 00163 00164 //Wrist points 00165 float Cfi = cosf(fi); 00166 float Sfi = sinf(fi); 00167 if( (Cfi>-0.000001F)&&(Cfi<0.000001F)){ 00168 Cfi = 0; 00169 } 00170 if( (Sfi>-0.000001F)&&(Sfi<0.000001F)){ 00171 Sfi = 0; 00172 } 00173 float Xm = Xf - L4 * Cfi; 00174 float Ym = Yf - L4 * Sfi; 00175 00176 //Calc articular coordinates 00177 //q4 00178 q[3] = theta; 00179 00180 //q2 00181 float C2 = ((powf(Xm,2.0F)) + (powf(Ym,2.0F)) - (powf(L1,2.0F)) - (powf(L2,2.0F))) / (2*L1*L2); 00182 float S2 = sqrtf(1-powf(C2,2.0F)); 00183 float q21 = atan2f(S2,C2); 00184 float q22 = atan2f(-S2,C2); 00185 q[1] = q21; 00186 00187 //q1 00188 float alpha = atan2f((L2*sinf(q[1])),(L1+L2*cosf(q[1]))); 00189 float beta_1 = atan2f(Ym, Xm); 00190 float beta_2 = atan2f(Ym,-Xm); 00191 00192 //Elbow up 00193 float q1_1 = beta_1-alpha; 00194 //Elbow down 00195 float q1_2 = beta_2-alpha; 00196 00197 q[0]= q1_1; 00198 00199 //q3 00200 q[2] = fi-q[0]-q[1]; 00201 } 00202 void Jacobian(struct Robot* Robot1,float q[DOF]){ 00203 ForwardKinematics(Robot1,q); 00204 struct Matrix_4N A0 = Robot1->BaseMatrix; 00205 for(int i=0; i<DOF; i++){ 00206 A0 = multiply_4N(A0.m,Robot1->_Eslabon[i]._DH.A0.m); 00207 } 00208 struct Vector_3N p0_dof; 00209 p0_dof.vector[0] = A0.m[0][3]; 00210 p0_dof.vector[1] = A0.m[1][3]; 00211 p0_dof.vector[2] = A0.m[2][3]; 00212 00213 struct Vector_3N p[DOF]; 00214 struct Vector_3N pA0; 00215 pA0.vector[0] = Robot1->BaseMatrix.m[0][3]; 00216 pA0.vector[1] = Robot1->BaseMatrix.m[1][3]; 00217 pA0.vector[2] = Robot1->BaseMatrix.m[2][3]; 00218 00219 p[0]=SubtractionVectors(p0_dof.vector,pA0.vector); 00220 00221 struct Vector_3N z[DOF]; 00222 struct Vector_3N z0; 00223 z0.vector[0] = Robot1->BaseMatrix.m[0][2]; 00224 z0.vector[1] = Robot1->BaseMatrix.m[1][2]; 00225 z0.vector[2] = Robot1->BaseMatrix.m[2][2]; 00226 00227 z[0]=z0; 00228 00229 00230 struct Matrix_4N A01 = multiply_4N(BaseMatrix,Robot1->_Eslabon[0]._DH.A0.m); 00231 struct Vector_3N Jv[DOF-1]; 00232 struct Vector_3N Jw[DOF-1]; 00233 00234 if(Robot1->_Eslabon[0]._DH.Type == REVOLUTION){ 00235 Jv[0] = crossProduct(z[0].vector,p[0].vector); 00236 Jw[0] = z[0]; 00237 }else{ 00238 Jv[0] = z[0]; 00239 Jw[0] = zeros(3); 00240 } 00241 for(int i=1;i<DOF;i++){ 00242 pA0.vector[0] = A01.m[0][3]; 00243 pA0.vector[1] = A01.m[1][3]; 00244 pA0.vector[2] = A01.m[2][3]; 00245 00246 z0.vector[0] = A01.m[0][2]; 00247 z0.vector[1] = A01.m[1][2]; 00248 z0.vector[2] = A01.m[2][2]; 00249 00250 p[i] = SubtractionVectors(p0_dof.vector,pA0.vector); 00251 z[i] = z0; 00252 00253 if(Robot1->_Eslabon[i]._DH.Type == REVOLUTION){ 00254 Jv[i] = crossProduct(z[i].vector,p[i].vector); 00255 Jw[i] = z[i]; 00256 }else{ 00257 Jv[i] = z[i]; 00258 Jw[i] = zeros(3); 00259 } 00260 00261 A01 = multiply_4N(A01.m,Robot1->_Eslabon[i]._DH.A0.m); 00262 } 00263 } 00264 void EulerNewton(struct Robot* Robot1,float q[DOF], float qd[DOF],float qdd[DOF]){ 00265 00266 struct Matrix_3N Rtot; 00267 00268 //Get inverse rotation 00269 ForwardKinematics(Robot1,q); 00270 struct Matrix_3N R3_post_inv = Inv_3N(GetRotation(Robot1,0).m); 00271 00272 Rtot = GetRotation(Robot1,0); 00273 00274 //Vector join Si-1 and Si 00275 struct Vector_3N R_pi; 00276 R_pi.vector[0] = Robot1->_Eslabon[0]._DH.a; 00277 R_pi.vector[1] = Robot1->_Eslabon[0]._DH.d*sinf(Robot1->_Eslabon[0]._DH.alpha); 00278 R_pi.vector[2] = Robot1->_Eslabon[0]._DH.d*cosf(Robot1->_Eslabon[0]._DH.alpha); 00279 00280 //Center of gravity 00281 struct Vector_3N s; 00282 s.vector[0] = Robot1->_Eslabon[0].Cent_Gravity[0]/ Robot1->_Eslabon[0].Mass; 00283 s.vector[1] = Robot1->_Eslabon[0].Cent_Gravity[1]/ Robot1->_Eslabon[0].Mass; 00284 s.vector[2] = Robot1->_Eslabon[0].Cent_Gravity[2]/ Robot1->_Eslabon[0].Mass; 00285 00286 // 00287 float z0[3] = {0,0,1}; 00288 float g[3] = {0,9.81F,0}; 00289 00290 struct Vector_3N w,wd,vd,a; 00291 if (Robot1->_Eslabon[0]._DH.Type == REVOLUTION){ 00292 w = ProdMatrVect(R3_post_inv.m,z0,qd[0]); 00293 00294 00295 wd = ProdMatrVect(R3_post_inv.m,z0,qdd[0]); 00296 00297 00298 00299 vd = AddVectors(((AddVectors((crossProduct(wd.vector,R_pi.vector)).vector, 00300 crossProduct(w.vector, crossProduct(w.vector,R_pi.vector).vector).vector)).vector), 00301 (ProdMatrVect(R3_post_inv.m,g,1).vector)); 00302 00303 00304 }else{ 00305 w=zeros(3); 00306 wd=zeros(3); 00307 vd = ProdMatrVect(R3_post_inv.m,(AddVectors(ProdVectConst(z0,qdd[0]).vector,g).vector),1); 00308 } 00309 a = AddVectors(AddVectors((crossProduct(wd.vector,s.vector)).vector, 00310 crossProduct(w.vector,crossProduct(w.vector,s.vector).vector).vector).vector,vd.vector); 00311 00312 00313 copyVector(&(Robot1->_Eslabon[0].w),w); 00314 copyVector(&(Robot1->_Eslabon[0].wd),wd); 00315 copyVector(&(Robot1->_Eslabon[0].vd),vd); 00316 copyVector(&(Robot1->_Eslabon[0].a),a); 00317 00318 for (int i=1; i<DOF; i++){ 00319 //Get inverse rotation 00320 Rtot = multiply_3N(Rtot.m,(GetRotation(Robot1,i).m)); 00321 R3_post_inv = Inv_3N(GetRotation(Robot1,i).m); 00322 00323 //Vector join Si-1 and Si 00324 R_pi.vector[0] = Robot1->_Eslabon[i]._DH.a; 00325 R_pi.vector[1] = Robot1->_Eslabon[i]._DH.d*sinf(Robot1->_Eslabon[i]._DH.alpha); 00326 R_pi.vector[2] = Robot1->_Eslabon[i]._DH.d*cosf(Robot1->_Eslabon[i]._DH.alpha); 00327 00328 //Center of gravity 00329 s.vector[0] = Robot1->_Eslabon[i].Cent_Gravity[0]/ Robot1->_Eslabon[i].Mass; 00330 s.vector[1] = Robot1->_Eslabon[i].Cent_Gravity[1]/ Robot1->_Eslabon[i].Mass; 00331 s.vector[2] = Robot1->_Eslabon[i].Cent_Gravity[2]/ Robot1->_Eslabon[i].Mass; 00332 00333 struct Vector_3N z03qd = ProdVectConst(z0,qd[i]); 00334 struct Vector_3N z03qdd = ProdVectConst(z0,qdd[i]); 00335 00336 if (Robot1->_Eslabon[i]._DH.Type == REVOLUTION){ 00337 w = ProdMatrVect(R3_post_inv.m,AddVectors(Robot1->_Eslabon[i-1].w.vector,z03qd.vector).vector,1); 00338 wd = ProdMatrVect( R3_post_inv.m, 00339 ( AddVectors((AddVectors(Robot1->_Eslabon[i-1].wd.vector,z03qdd.vector).vector), 00340 crossProduct(Robot1->_Eslabon[i-1].w.vector,z03qd.vector).vector) ).vector, 1 ); 00341 00342 00343 vd = AddVectors((AddVectors((crossProduct( wd.vector, R_pi.vector).vector), 00344 (crossProduct( w.vector,(crossProduct( w.vector,R_pi.vector)).vector)).vector)).vector , 00345 (ProdMatrVect( R3_post_inv.m,Robot1->_Eslabon[i-1].vd.vector,1).vector) ); 00346 }else{ 00347 w = ProdMatrVect(R3_post_inv.m,Robot1->_Eslabon[i-1].w.vector ,1); 00348 wd = ProdMatrVect(R3_post_inv.m,Robot1->_Eslabon[i-1].wd.vector ,1); 00349 vd = AddVectors(AddVectors(AddVectors((ProdMatrVect(R3_post_inv.m,(AddVectors(z03qdd.vector,Robot1->_Eslabon[i-1].vd.vector)).vector,1)).vector, 00350 crossProduct(w.vector,R_pi.vector).vector).vector, 00351 (crossProduct( w.vector,(crossProduct( w.vector,R_pi.vector)).vector)).vector).vector, 00352 ProdVectConst((crossProduct( w.vector , ProdMatrVect(R3_post_inv.m,z0,1).vector).vector),2*qd[i]).vector); 00353 00354 } 00355 00356 a = AddVectors(AddVectors((crossProduct(wd.vector,s.vector)).vector, 00357 crossProduct(w.vector,crossProduct(w.vector,s.vector).vector).vector).vector,vd.vector); 00358 00359 copyVector(&(Robot1->_Eslabon[i].w),w); 00360 copyVector(&(Robot1->_Eslabon[i].wd),wd); 00361 copyVector(&(Robot1->_Eslabon[i].vd),vd); 00362 copyVector(&(Robot1->_Eslabon[i].a),a); 00363 } 00364 00365 //Vector join Si-1 and Si 00366 R_pi.vector[0] = Robot1->_Eslabon[DOF-1]._DH.a; 00367 R_pi.vector[1] = Robot1->_Eslabon[DOF-1]._DH.d*sinf(Robot1->_Eslabon[DOF-1]._DH.alpha); 00368 R_pi.vector[2] = Robot1->_Eslabon[DOF-1]._DH.d*cosf(Robot1->_Eslabon[DOF-1]._DH.alpha); 00369 00370 //Center of gravity 00371 s.vector[0] = Robot1->_Eslabon[DOF-1].Cent_Gravity[0]/ Robot1->_Eslabon[DOF-1].Mass; 00372 s.vector[1] = Robot1->_Eslabon[DOF-1].Cent_Gravity[1]/ Robot1->_Eslabon[DOF-1].Mass; 00373 s.vector[2] = Robot1->_Eslabon[DOF-1].Cent_Gravity[2]/ Robot1->_Eslabon[DOF-1].Mass; 00374 00375 00376 // Fuerza ejercida sobre i en el cdm 00377 struct Vector_3N f_cdm = ProdVectConst(Robot1->_Eslabon[DOF-1].a.vector, -1*(Robot1->_Eslabon[DOF-1].Mass)); 00378 00379 00380 struct Vector_3N M_cdm = SubtractionVectors(ProdMatrVect(Robot1->_Eslabon[DOF-1].Inertia_tensor,Robot1->_Eslabon[DOF-1].wd.vector,-1).vector, 00381 crossProduct(Robot1->_Eslabon[DOF-1].w.vector,(ProdMatrVect(Robot1->_Eslabon[DOF-1].Inertia_tensor,Robot1->_Eslabon[DOF-1].w.vector,1)).vector).vector );//MIRAR 00382 00383 00384 struct Vector_3N f = ProdVectConst(f_cdm.vector,-1); 00385 00386 00387 struct Vector_3N M = SubtractionVectors(ProdVectConst((crossProduct(AddVectors(R_pi.vector,s.vector).vector,f_cdm.vector)).vector,-1).vector,M_cdm.vector); 00388 float Tau; 00389 if (Robot1->_Eslabon[DOF-1]._DH.Type == REVOLUTION){ 00390 Tau = ProdVectors(ProdVectMatr(M.vector,R3_post_inv.m,1).vector,z0); 00391 }else{ 00392 Tau = ProdVectors(ProdVectMatr(f.vector,R3_post_inv.m,1).vector,z0); 00393 } 00394 Robot1->_Eslabon[DOF-1].Tau=Tau; 00395 copyVector(&(Robot1->_Eslabon[DOF-1].f),f); 00396 copyVector(&(Robot1->_Eslabon[DOF-1].M),M); 00397 00398 for (int i=DOF-2; i>=0; i--){ 00399 struct Matrix_3N R3_post = GetRotation(Robot1,i+1); 00400 struct Matrix_3N R3_inv = Inv_3N(GetRotation(Robot1,i+1).m); 00401 Rtot = multiply_3N(Rtot.m, R3_inv.m); 00402 struct Matrix_3N Rtot_inv = Inv_3N(Rtot.m); 00403 00404 struct Matrix_3N R3_inv_ant = Inv_3N(GetRotation(Robot1,i).m); 00405 00406 //Vector join Si-1 and Si 00407 R_pi.vector[0] = Robot1->_Eslabon[i]._DH.a; 00408 R_pi.vector[1] = Robot1->_Eslabon[i]._DH.d*sinf(Robot1->_Eslabon[i]._DH.alpha); 00409 R_pi.vector[2] = Robot1->_Eslabon[i]._DH.d*cosf(Robot1->_Eslabon[i]._DH.alpha); 00410 struct Vector_3N R_pi_post = ProdMatrVect(R3_inv.m,R_pi.vector,1); 00411 //Center of gravity 00412 s.vector[0] = Robot1->_Eslabon[i].Cent_Gravity[0]/ Robot1->_Eslabon[i].Mass; 00413 s.vector[1] = Robot1->_Eslabon[i].Cent_Gravity[1]/ Robot1->_Eslabon[i].Mass; 00414 s.vector[2] = Robot1->_Eslabon[i].Cent_Gravity[2]/ Robot1->_Eslabon[i].Mass; 00415 00416 00417 00418 f_cdm = ProdVectConst(Robot1->_Eslabon[i].a.vector, -1*(Robot1->_Eslabon[i].Mass)); 00419 00420 M_cdm = SubtractionVectors(ProdMatrVect(Robot1->_Eslabon[i].Inertia_tensor,Robot1->_Eslabon[i].wd.vector,-1).vector, 00421 crossProduct(Robot1->_Eslabon[i].w.vector,(ProdMatrVect(Robot1->_Eslabon[i].Inertia_tensor,Robot1->_Eslabon[i].w.vector,1)).vector).vector );//MIRAR 00422 00423 00424 f = SubtractionVectors(ProdMatrVect(R3_post.m,Robot1->_Eslabon[i+1].f.vector,1).vector,f_cdm.vector); 00425 M = SubtractionVectors((SubtractionVectors((ProdMatrVect(R3_post.m,AddVectors(Robot1->_Eslabon[i+1].M.vector,crossProduct(R_pi_post.vector,Robot1->_Eslabon[i+1].f.vector).vector 00426 ).vector,1)).vector, crossProduct(AddVectors(R_pi.vector,s.vector).vector,f_cdm.vector).vector).vector), M_cdm.vector); 00427 00428 if (Robot1->_Eslabon[i]._DH.Type == REVOLUTION){ 00429 Tau = ProdVectors(ProdVectMatr(M.vector,R3_inv_ant.m,1).vector,z0); 00430 }else{ 00431 Tau = ProdVectors(ProdVectMatr(f.vector,R3_inv_ant.m,1).vector,z0); 00432 } 00433 00434 Robot1->_Eslabon[i].Tau=Tau; 00435 copyVector(&(Robot1->_Eslabon[i].f),f); 00436 copyVector(&(Robot1->_Eslabon[i].M),M); 00437 } 00438 00439 00440 } 00441 void GravityTorque(struct Robot* _Robot, float q[DOF]) { //HACER: INCLUIR GRAVEDAD 00442 float g[3] = {0, 9.81F, 0}; 00443 ForwardKinematics(_Robot, q); 00444 struct Matrix_4N A0 = _Robot->_Eslabon[0]._DH.A0; 00445 struct Vector_4N pcg; 00446 00447 struct Vector_3N position[DOF]; 00448 position[0] = zeros(3); 00449 position[1] = GetPosition(A0.m); 00450 00451 struct Vector_3N z0 [DOF]; 00452 z0[0].vector[0] = 0; 00453 z0[0].vector[1] = 0; 00454 z0[0].vector[2] = 1; 00455 00456 struct Vector_4N m; 00457 for (int i = 0; i < DOF; i++) { 00458 if (i > 0) { 00459 z0[i].vector[0] = A0.m[0][2]; 00460 z0[i].vector[1] = A0.m[1][2]; 00461 z0[i].vector[2] = A0.m[2][2]; 00462 00463 A0 = multiply_4N(A0.m, _Robot->_Eslabon[i]._DH.A0.m); 00464 if (i<DOF-1) { 00465 position[i + 1] = GetPosition(A0.m); 00466 } 00467 } 00468 m.vector[0] = _Robot->_Eslabon[i].Cent_Gravity[0] / _Robot->_Eslabon[i].Mass; 00469 m.vector[1] = _Robot->_Eslabon[i].Cent_Gravity[1] / _Robot->_Eslabon[i].Mass; 00470 m.vector[2] = _Robot->_Eslabon[i].Cent_Gravity[2] / _Robot->_Eslabon[i].Mass; 00471 m.vector[3] = 1; 00472 00473 00474 pcg = ProdMatrVect_4N(A0.m, m.vector, 1); 00475 _Robot->_Motor[i].pcg.vector[0] = pcg.vector[0]; 00476 _Robot->_Motor[i].pcg.vector[1] = pcg.vector[1]; 00477 _Robot->_Motor[i].pcg.vector[2] = pcg.vector[2]; 00478 } 00479 struct Vector_3N M; 00480 struct Vector_3N mg; 00481 float Tau; 00482 for (int i =0; i < DOF+5; i++) { 00483 mg = ProdVectConst(g, _Robot->_Eslabon[i].Mass); 00484 M = crossProduct(mg.vector, SubtractionVectors(_Robot->_Motor[i].pcg.vector,position[i].vector).vector); 00485 for (int j = 0; j < DOF+5; j++) { 00486 if(j>i) { 00487 mg = ProdVectConst(g, _Robot->_Eslabon[j].Mass); 00488 M = AddVectors(M.vector, SubtractionVectors(crossProduct(mg.vector, _Robot->_Motor[j].pcg.vector).vector, 00489 position[i].vector).vector); 00490 } 00491 } 00492 Tau = ProdVectors(M.vector,z0[i].vector); 00493 //HACER: COPIAR VECTORES 00494 M = zeros(3); 00495 } 00496 00497 } 00498 00499 00500 struct Matrix_3N GetRotation(struct Robot* _Robot, int number){ 00501 struct Matrix_3N a; 00502 for(int x=0;x<3;x++){ 00503 for(int y=0; y<3; y++){ 00504 a.m[x][y]=_Robot->_Eslabon[number]._DH.A0.m[x][y]; 00505 } 00506 } 00507 return a; 00508 } 00509 struct Matrix_4N RotX(float thetaX){ 00510 struct Matrix_4N a; 00511 float STheta = sinf(thetaX); 00512 float CTheta = cosf( thetaX); 00513 for(int x=0;x<4;x++){ 00514 for(int y=0; y<4; y++){ 00515 if ((x == 0 && y == 0) || (x == 3 && y == 3) ) { 00516 a.m[x][y]=1; 00517 } else if (x == 1 && y == 1) { 00518 a.m[x][y]=CTheta; 00519 } else if (x == 2 && y == 1) { 00520 a.m[x][y]=STheta; 00521 } else if (x == 1 && y == 2) { 00522 a.m[x][y]=-STheta; 00523 } else if (x == 2 && y == 2) { 00524 a.m[x][y]=CTheta; 00525 }else{ 00526 a.m[x][y]=0; 00527 } 00528 if( (a.m [x][y]>-0.000001F)&&(a.m [x][y])<0.000001F){ 00529 a.m [x][y]=0; 00530 } 00531 } 00532 } 00533 return a; 00534 } 00535 struct Matrix_4N RotY(float thetaY){ 00536 struct Matrix_4N a; 00537 float STheta = sinf(thetaY); 00538 float CTheta = cosf( thetaY); 00539 for(int x=0;x<4;x++){ 00540 for(int y=0; y<4; y++){ 00541 if ((x == 1 && y == 1) || (x == 3 && y == 3) ) { 00542 a.m[x][y]=1; 00543 } else if (x == 0 && y == 0) { 00544 a.m[x][y]=CTheta; 00545 } else if (x == 0 && y == 2) { 00546 a.m[x][y]=STheta; 00547 } else if (x == 2 && y == 0) { 00548 a.m[x][y]=-STheta; 00549 } else if (x == 2 && y == 2) { 00550 a.m[x][y]=CTheta; 00551 }else{ 00552 a.m[x][y]=0; 00553 } 00554 if( (a.m [x][y]>-0.000001F)&&(a.m [x][y])<0.000001F){ 00555 a.m [x][y]=0; 00556 } 00557 } 00558 } 00559 return a; 00560 } 00561 struct Matrix_4N RotZ(float thetaZ){ 00562 struct Matrix_4N a; 00563 float STheta = sinf(thetaZ); 00564 float CTheta = cosf( thetaZ); 00565 for(int x=0;x<4;x++){ 00566 for(int y=0; y<4; y++){ 00567 if ((x == 2 && y == 2) || (x == 3 && y == 3) ) { 00568 a.m[x][y]=1; 00569 } else if (x == 0 && y == 0) { 00570 a.m[x][y]=CTheta; 00571 } else if (x == 0 && y == 1) { 00572 a.m[x][y]=-STheta; 00573 } else if (x == 1 && y == 0) { 00574 a.m[x][y]=STheta; 00575 } else if (x == 1 && y == 1) { 00576 a.m[x][y]=CTheta; 00577 }else{ 00578 a.m[x][y]=0; 00579 } 00580 if( (a.m [x][y]>-0.000001F)&&(a.m [x][y])<0.000001F){ 00581 a.m [x][y]=0; 00582 } 00583 } 00584 } 00585 return a; 00586 } 00587 struct Matrix_4N Trans(float dx, float dy, float dz){ 00588 struct Matrix_4N a; 00589 for(int x=0;x<4;x++){ 00590 for(int y=0; y<4; y++){ 00591 if ( (x == 0 && y == 0) || (x == 1 && y == 1) || (x == 2 && y == 2) || (x == 3 && y == 3) ) { 00592 a.m[x][y]=1; 00593 } else if (x == 0 && y == 3) { 00594 a.m[x][y]=dx; 00595 } else if (x == 1 && y == 3) { 00596 a.m[x][y]=dy; 00597 } else if (x == 2 && y == 3) { 00598 a.m[x][y]=dz; 00599 }else{ 00600 a.m[x][y]=0; 00601 } 00602 } 00603 } 00604 return a; 00605 } 00606 struct Matrix_3N multiply_3N(float mat1[][3], float mat2[][3]){ 00607 struct Matrix_3N a; 00608 for (int i = 0; i < 3; i++) { 00609 for (int j = 0; j < 3; j++) { 00610 a.m[i][j] = 0; 00611 for (int k = 0; k < 3; k++) { 00612 a.m[i][j] += mat1[i][k]*mat2[k][j]; 00613 } 00614 } 00615 } 00616 return a; 00617 } 00618 struct Matrix_4N multiply_4N(float mat1[][4], float mat2[][4]){ 00619 struct Matrix_4N a; 00620 for (int i = 0; i < 4; i++) { 00621 for (int j = 0; j < 4; j++) { 00622 a.m[i][j] = 0; 00623 for (int k = 0; k < 4; k++) { 00624 a.m[i][j] += mat1[i][k]*mat2[k][j]; 00625 } 00626 } 00627 } 00628 return a; 00629 } 00630 struct Matrix_3N Inv_3N(float mat1[][3]){ 00631 float determinant=0; 00632 struct Matrix_3N a; 00633 for(int i = 0; i < 3; i++) { 00634 determinant = determinant + (mat1[0][i] * (mat1[1][(i + 1) % 3] * mat1[2][(i + 2) % 3] - 00635 mat1[1][(i + 2) % 3] * mat1[2][(i + 1) % 3])); 00636 } 00637 00638 if(determinant>0.01F) { 00639 for (int i = 0; i < 3; i++) { 00640 for (int j = 0; j < 3; j++) { 00641 a.m[i][j] = (((mat1[(j + 1) % 3][(i + 1) % 3] * mat1[(j + 2) % 3][(i + 2) % 3]) - 00642 (mat1[(j + 1) % 3][(i + 2) % 3] * mat1[(j + 2) % 3][(i + 1) % 3])) / determinant); 00643 00644 if( (a.m[i][j]>-0.000001F)&&(a.m[i][j])<0.000001F) { 00645 a.m[i][j] = 0; 00646 } 00647 } 00648 } 00649 }else{ 00650 for (int i = 0; i < 3; i++) { 00651 for (int j = 0; j < 3; j++) { 00652 a.m[i][j] = 0; 00653 } 00654 } 00655 } 00656 return a; 00657 } 00658 struct Vector_3N GetPosition(float mat1[][4]){ 00659 struct Vector_3N a; 00660 for(int i=0; i<3;i++) { 00661 a.vector[i] = mat1[i][3]; 00662 } 00663 return a; 00664 } 00665 00666 void Trapezoid(struct Robot* _Robot, struct End_Effector* EFF_q0, struct End_Effector* EFF_qf,float stime){ 00667 float s[DOF], sign[DOF], t1[DOF], t2[DOF], t3[DOF], qi[DOF], qf[DOF]; 00668 int time1, time2, time3, t=0, max; 00669 00670 _Robot->_Trajectory._TrajectoryType = TRAPEZOIDAL; 00671 00672 //Compute the coordenates in q-space 00673 InverseKinematics(_Robot, EFF_q0, qi); 00674 InverseKinematics(_Robot, EFF_qf, qf); 00675 00676 //Compute the instants of time when the constant velocity and 00677 //deceleration start, and the time when the movement ends 00678 for(int i=0;i<DOF;i++){ 00679 s[i] = qf[i] - qi[i]; 00680 if(s[i] < 0.0F){ 00681 sign[i] = -1.0F; 00682 } 00683 else if(s[i] > 0.0F){ 00684 sign[i] = 1.0F; 00685 } 00686 else{ 00687 sign[i] = 0.0F; 00688 } 00689 00690 if (fabsf(s[i]) > (powf(_Robot->_Motor[i].Contrains_v,2.0F) / _Robot->_Motor[i].Contrains_a)){ 00691 t1[i] = _Robot->_Motor[i].Contrains_v / _Robot->_Motor[i].Contrains_a; 00692 t2[i] = fabsf(s[i]) / _Robot->_Motor[i].Contrains_v; 00693 t3[i] = fabsf(s[i]) / _Robot->_Motor[i].Contrains_v + _Robot->_Motor[i].Contrains_v / _Robot->_Motor[i].Contrains_a; 00694 } 00695 else{ 00696 t1[i] = sqrtf(fabsf(s[i]) / _Robot->_Motor[i].Contrains_a); 00697 t3[i] = 2.0F*t1[i]; 00698 t2[i] = 0.0F; 00699 } 00700 } 00701 00702 //Convert the slowest t by phase in amount of sampling time 00703 max = FindMinOrMax(t1, DOF, 1); 00704 time1 = ConvertTime(t1[max], stime); 00705 max = FindMinOrMax(t2, DOF, 1); 00706 time2 = ConvertTime(t2[max], stime); 00707 max = FindMinOrMax(t3, DOF, 1); 00708 time3 = ConvertTime(t3[max], stime); 00709 00710 //Memory allocation to store the data 00711 _Robot->_Trajectory.traj = (struct Trajectory_Path*)malloc(sizeof(struct Trajectory_Path) * time3); 00712 00713 //q, qvel and qacel initialisation 00714 for(int i=0;i<DOF;i++){ 00715 _Robot->_Trajectory.traj[0].q[i] = qi[i]; 00716 _Robot->_Trajectory.traj[0].qd[i] = 0.0F; 00717 _Robot->_Trajectory.traj[0].qdd[i] = 0.0F; 00718 } 00719 00720 _Robot->_Trajectory.duration = (float) time3; 00721 _Robot->_Trajectory.Tsampling = stime; 00722 00723 if(_Robot->_Trajectory.traj != NULL){ 00724 if(time3 > 5000){ 00725 printf("too many trajectory instants\n"); 00726 } 00727 //Sampling in units of st (sampling time) 00728 for(t=0;t<time3;t++){ 00729 for(int i=0;i<DOF;i++){ 00730 //Calculation of the velocity and position for phase 1 (acceleration) 00731 if (t < time1){ 00732 _Robot->_Trajectory.traj[t].qdd[i] = sign[i]*_Robot->_Motor[i].Contrains_a; 00733 _Robot->_Trajectory.traj[t].qd[i] += _Robot->_Trajectory.traj[t].qdd[i]*stime; 00734 _Robot->_Trajectory.traj[t].q[i] += _Robot->_Trajectory.traj[t].qd[i]*stime + 00735 0.5F*_Robot->_Trajectory.traj[t].qdd[i]*stime*stime; 00736 } 00737 //Calculation of the velocity and position for phase 2 (constant velocity) 00738 else if ((t >= time1) && (t < time2)){ 00739 _Robot->_Trajectory.traj[t].qdd[i] = 0.0F; 00740 _Robot->_Trajectory.traj[t].qd[i] = sign[i]*_Robot->_Motor[i].Contrains_v; 00741 _Robot->_Trajectory.traj[t].q[i] += _Robot->_Trajectory.traj[t].qd[i]*stime; 00742 } 00743 //Calculation of the velocity and position for phase 3 (deceleration) 00744 else if ((t >= time2) && (t < time3)){ 00745 _Robot->_Trajectory.traj[t].qdd[i] = -sign[i]*_Robot->_Motor[i].Contrains_a; 00746 _Robot->_Trajectory.traj[t].qd[i] += _Robot->_Trajectory.traj[t].qdd[i]*stime; 00747 _Robot->_Trajectory.traj[t].q[i] += _Robot->_Trajectory.traj[t].qd[i]*stime + 00748 0.5F*_Robot->_Trajectory.traj[t].qdd[i]*stime*stime; 00749 }else{ 00750 _Robot->_Trajectory.traj[t].qdd[i] = 0.0F; 00751 _Robot->_Trajectory.traj[t].qd[i] = 0.0F; 00752 _Robot->_Trajectory.traj[t].q[i] = qf[i]; 00753 } 00754 } 00755 } 00756 }else{ 00757 printf("Error al reservar memoria dinamica GT/n"); 00758 } 00759 } 00760 void Spline_5th (struct Robot* _Robot, struct End_Effector* EFF_q0, struct End_Effector* EFF_qf,float stime,float time){ 00761 float a[DOF], b[DOF], c[DOF], d[DOF], e[DOF], f[DOF], qi[DOF], qf[DOF]; 00762 float cal_temp0, cal_temp1, cal_temp2; 00763 int cycle; 00764 00765 _Robot->_Trajectory._TrajectoryType = SPLIN5; 00766 00767 //Compute the coordenates in q-space 00768 InverseKinematics(_Robot, EFF_q0, qi); 00769 InverseKinematics(_Robot, EFF_qf, qf); 00770 00771 00772 printf("q1 = %f q2 = %f q3 = %f q4 = %f\n", qi[0], qi[1], qi[2], qi[3]); 00773 printf("q1 = %f q2 = %f q3 = %f q4 = %f\n", qf[0], qf[1], qf[2], qf[3]); 00774 00775 //Previous calculations 00776 cal_temp0 = powf(time,3.0F); 00777 cal_temp1 = time*(45.0F - 14.0F*powf(time,4.0F)); 00778 cal_temp2 = cal_temp1*cal_temp0; 00779 00780 //Convert the slowest t by phase in amount of sampling time 00781 cycle = ConvertTime(time, stime); 00782 //Memory allocation to store the data 00783 _Robot->_Trajectory.traj = (struct Trajectory_Path*)malloc(sizeof(struct Trajectory_Path) * cycle); 00784 00785 _Robot->_Trajectory.duration = (float) cycle; 00786 _Robot->_Trajectory.Tsampling = stime; 00787 00788 if(_Robot->_Trajectory.traj != NULL){ 00789 for(int i=0;i<DOF;i++){ 00790 f[i] = 3.0F * _Robot->_Motor[i].Contrains_v / cal_temp2 -6.0F*qi[i] / cal_temp1 00791 -6.0F * _Robot->_Motor[i].Contrains_v*time / cal_temp1 + 6.0F*qf[i] / cal_temp1; 00792 e[i] = 3.0F * _Robot->_Motor[i].Contrains_v/ (6.0F*cal_temp0) - f[i]*time*15.0F/2.0F; 00793 d[i] = -6.0F*e[i]*time + 20.0F*f[i]*time*time/6.0F; 00794 c[i] = 0.0F; //if initial acceleration = 0 00795 b[i] = _Robot->_Motor[i].Contrains_v; 00796 a[i] = qi[i]; 00797 } 00798 00799 //Sampling in units of st (sampling time) 00800 for(int t=0;t<cycle;t++){ 00801 for(int i=0;i<DOF;i++){ 00802 00803 00804 _Robot->_Trajectory.traj[t].q[i] = a[i] + b[i]*((float)t)*stime + c[i]*powf(((float)t)*stime,2.0F) + 00805 d[i]*powf(((float)t)*stime,3.0F) + e[i]*powf(((float)t)*stime,4.0F) + f[i]*powf(((float)t)*stime,5.0F); 00806 _Robot->_Trajectory.traj[t].qd[i] = b[i] + 2.0F*c[i]*((float)t)*stime + 3.0F*d[i]*powf(((float)t)*stime,2.0F) + 00807 4.0F*e[i]*powf(((float)t)*stime,3.0F) + 5.0F*f[i]*powf(((float)t)*stime,4.0F); 00808 _Robot->_Trajectory.traj[t].qdd[i] = 2.0F*c[i] + 6.0F*d[i]*((float)t)*stime + 12.0F*e[i]*powf(((float)t)*stime,2.0F) + 00809 20*f[i]*powf(((float)t)*stime,3.0F); 00810 } 00811 } 00812 }else{ 00813 printf("Error al reservar memoria dinamica GI5/n"); 00814 } 00815 } 00816 void Spline_7th(struct Robot* _Robot, struct End_Effector* EFF_q0, struct End_Effector* EFF_qf,float stime,float time){ 00817 float a[DOF], b[DOF], c[DOF], d[DOF], e[DOF], f[DOF], g[DOF], h[DOF], qi[DOF], qf[DOF]; 00818 float cal_temp0, cal_temp1, cal_temp2; 00819 int cycle; 00820 00821 _Robot->_Trajectory._TrajectoryType = SPLIN7; 00822 00823 //Compute the coordenates in q-space 00824 InverseKinematics(_Robot, EFF_q0, qi); 00825 InverseKinematics(_Robot, EFF_qf, qf); 00826 00827 //Previous calculations 00828 cal_temp0 = powf(time,5.0F); 00829 cal_temp1 = powf(time,4.0F); 00830 cal_temp2 = powf(time,3.0F); 00831 00832 //Convert the slowest t by phase in amount of sampling time 00833 cycle = ConvertTime(time, stime); 00834 //Memory allocation to store the data 00835 _Robot->_Trajectory.traj = (struct Trajectory_Path*)malloc(sizeof(struct Trajectory_Path) * cycle); 00836 00837 _Robot->_Trajectory.duration = (float) cycle; 00838 _Robot->_Trajectory.Tsampling = stime; 00839 00840 if(_Robot->_Trajectory.traj != NULL){ 00841 for(int i=0;i<DOF;i++){ 00842 h[i] = -(460.0F/747.0F)*(qi[i]-qf[i])/cal_temp0 -(11.0F/249.0F)*_Robot->_Motor[i].Contrains_v/cal_temp1 00843 -(32.0F/249)*_Robot->_Motor[i].Contrains_a/cal_temp2; 00844 g[i] = _Robot->_Motor[i].Contrains_v*2.0F/(23.0F*cal_temp0) + _Robot->_Motor[i].Contrains_a / 00845 (23.0F*cal_temp1) + h[i]*time*(112.0F/23.0F); 00846 f[i] = _Robot->_Motor[i].Contrains_a/(10.0F*cal_temp2) - g[i]*time/2.0F - h[i]*time*time*84.0F/5.0F; 00847 e[i] = -5.0F*f[i]*time/2.0F - 5.0F*g[i]*time*time - h[i]*cal_temp2*35.0F/4.0F; 00848 d[i] = 0.0F; 00849 c[i] = _Robot->_Motor[i].Contrains_a/2.0F; 00850 b[i] = _Robot->_Motor[i].Contrains_v; 00851 a[i] = qi[i]; 00852 } 00853 00854 //Sampling in units of st (sampling time) 00855 for(int t=0;t<cycle;t++){ 00856 for(int i=0;i<DOF;i++){ 00857 _Robot->_Trajectory.traj[t].q[i] = a[i] + b[i]*((float)t)*stime + c[i]*powf(((float)t)*stime,2.0F) + d[i]*powf(((float)t)*stime,3.0F) + 00858 e[i]*powf(((float)t)*stime,4.0F) + f[i]*powf(((float)t)*stime,5.0F) + g[i]*powf(((float)t)*stime,6.0F) 00859 + h[i]*powf(((float)t)*stime,7.0F); 00860 _Robot->_Trajectory.traj[t].qd[i] = b[i] + 2.0F*c[i]*((float)t)*stime + 3.0F*d[i]*powf(((float)t)*stime,2.0F) + 00861 4.0F*e[i]*powf(((float)t)*stime,3.0F) + 5.0F*f[i]*powf(((float)t)*stime,4.0F) + 00862 6.0F*g[i]*powf(((float)t)*stime,5.0F) + 7.0F*h[i]*powf(((float)t)*stime,6.0F); 00863 _Robot->_Trajectory.traj[t].qdd[i] = 2.0F*c[i] + 6.0F*d[i]*((float)t)*stime + 12.0F*e[i]*powf(((float)t)*stime,2.0F) + 00864 20.0F*f[i]*powf(((float)t)*stime,3.0F) + 30.0F*g[i]*powf(((float)t)*stime,4.0F) + 00865 42.0F*h[i]*powf(((float)t)*stime,5.0F); 00866 } 00867 } 00868 }else 00869 { 00870 printf("Error al reservar memoria dinamica GI7/n"); 00871 } 00872 } 00873 00874 int ConvertTime(float time, float stime){ 00875 float ciclos_temp; 00876 int ciclos; 00877 ciclos_temp = time / stime; //num de T de muestreo 00878 ciclos = (int) ciclos_temp; 00879 if(ciclos_temp == (float) ciclos){ 00880 return ciclos; 00881 }else{ 00882 return ciclos +1; // o +1 en caso de numero fraccionario 00883 } 00884 } 00885 int FindMinOrMax(const float array[], int size, _Bool minormax){ 00886 float max, min; 00887 int max_index=0, min_index=0; 00888 00889 if(minormax){ // Busca el max 00890 max = array[0]; 00891 00892 for (int i=0; i<size; i++){ 00893 if (array[i] > max){ 00894 max=array[i]; 00895 max_index = i; 00896 } 00897 } 00898 return max_index; 00899 } 00900 else{ // Busca el min 00901 min = array[0]; 00902 00903 for (int i=0; i<size; i++){ 00904 if (array[i] < min){ 00905 min=array[i]; 00906 min_index = i; 00907 } 00908 } 00909 return min_index; 00910 } 00911 } 00912
Generated on Thu Jul 28 2022 11:10:45 by
1.7.2