Prueba

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Robot.h Source File

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