Prueba

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Robot.cpp Source File

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