Prueba
Embed:
(wiki syntax)
Show/hide line numbers
Robot.h
00001 // 00002 // Created by m007 on 03/04/2020. 00003 // 00004 00005 #ifndef ROBOT_H 00006 #define ROBOT_H 00007 #include <stdio.h> 00008 #include <math.h> 00009 #include "Vector.h" 00010 #include "Controller.h" 00011 #include "Sensor.h" 00012 #define DOF 4 00013 00014 /*MOVE C*/ 00015 enum Links {REVOLUTION, LINEAR}; 00016 enum Trajectory_Types {TRAPEZOIDAL, SPLIN5, SPLIN7}; 00017 00018 struct Matrix_4N{ 00019 float m[4][4]; 00020 }; 00021 struct DH{ 00022 enum Links Type; 00023 float theta; 00024 float alpha; 00025 float d; 00026 float a; 00027 struct Matrix_4N A0; 00028 }; 00029 struct Motor{ 00030 float Degrees_Kinematics; 00031 struct Vector_3N pcg; 00032 float Contrains_v; 00033 float Contrains_a; 00034 }; 00035 struct Eslabon{ 00036 float Lenght; 00037 float Inertia_tensor[3][3]; 00038 float Cent_Gravity [3]; 00039 float Mass; 00040 float Tau; 00041 struct Vector_3N w; 00042 struct Vector_3N wd; 00043 struct Vector_3N vd; 00044 struct Vector_3N a; 00045 struct Vector_3N f; 00046 struct Vector_3N M; 00047 struct DH _DH; 00048 }; 00049 00050 struct Position{ 00051 float x; 00052 float y; 00053 float z; 00054 }; 00055 struct Orientation{ 00056 float roll; 00057 float pitch; 00058 float yaw; 00059 }; 00060 struct End_Effector{ 00061 struct Orientation _Orientation; 00062 struct Position _Position; 00063 }; 00064 struct Trajectory_Path{ 00065 float q [DOF]; 00066 float qd [DOF]; 00067 float qdd [DOF]; 00068 }; 00069 struct Trajectory{ 00070 struct Trajectory_Path *traj; 00071 float Tsampling; 00072 enum Trajectory_Types _TrajectoryType; 00073 float duration; 00074 }; 00075 00076 00077 struct Robot{ 00078 //matriz herramienta 00079 float ToolMatrix[4][4]; 00080 struct Matrix_4N BaseMatrix; 00081 struct Trajectory _Trajectory; 00082 struct Motor _Motor[DOF]; 00083 struct Eslabon _Eslabon[DOF]; 00084 }; 00085 00086 00087 struct Matrix_3N{ 00088 float m[3][3]; 00089 }; 00090 00091 00092 void ForwardKinematics(struct Robot* _Robot,float thetaOffset[DOF]); 00093 void InverseKinematics(struct Robot* Robot1,struct End_Effector* EFF_q,float q[3]); 00094 void Jacobian(struct Robot* Robot1,float q[DOF]); 00095 void init(struct Robot* _Robot, float _ToolMatrix[4][4], float _BaseMatrix[4][4]); 00096 void EulerNewton(struct Robot* Robot1,float q[DOF], float qd[DOF],float qdd[DOF]); 00097 void GravityTorque(struct Robot* _Robot,float q[DOF]); 00098 00099 struct Matrix_4N RotX(float thetaX); 00100 struct Matrix_4N RotY(float thetaY); 00101 struct Matrix_4N RotZ(float thetaZ); 00102 struct Matrix_4N Trans(float dx, float dy, float dz); 00103 struct Matrix_3N multiply_3N(float mat1[][3], float mat2[][3]); 00104 struct Matrix_4N multiply_4N(float mat1[][4], float mat2[][4]); 00105 struct Matrix_3N GetRotation(struct Robot* _Robot, int number); 00106 struct Vector_3N GetPosition(float mat1[][4]); 00107 struct Matrix_3N Inv_3N(float mat1[][3]); 00108 00109 00110 int ConvertTime(float time, float stime); 00111 int FindMinOrMax(const float array[], int size, _Bool minormax); 00112 00113 void Trapezoid(struct Robot* _Robot, struct End_Effector* EFF_q0, struct End_Effector* EFF_qf,float stime); 00114 void Spline_5th (struct Robot* _Robot, struct End_Effector* EFF_q0, struct End_Effector* EFF_qf,float stime,float time); 00115 void Spline_7th (struct Robot* _Robot, struct End_Effector* EFF_q0, struct End_Effector* EFF_qf,float stime,float time); 00116 00117 #endif //ROBOT_H
Generated on Thu Jul 28 2022 11:10:45 by
1.7.2