Robot

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 26/06/2020.
00003 //
00004 
00005 #ifndef UNTITLED_ROBOT_H
00006 #define UNTITLED_ROBOT_H
00007 #include "mbed.h"
00008 
00009 #include "Vector.h"
00010 #include "Matrix.h"
00011 
00012 #include "Link.h"
00013 #include "Motor.h"
00014 
00015 #include "Trajectory.h"
00016 #include "End_Effector.h"
00017 #include <cmath>
00018 
00019 class Robot {
00020 
00021 public:
00022     Robot(float _ToolMatrix[][4],float _BaseMatrix[][4],const float DH_Table[], const float lenght[],float dyn_mat [][10],const float MotorParam[],const float ControlGain[],int RobotDOF,const Link::Links_Type link [], const float MotorConstraints[],PinName PinSensorA[], PinName PinSensorB[],const float SensorProperties[], PinName PinDriver[]);
00023     Robot(Matrix<float>& _ToolMatrix,Matrix<float>& _BaseMatrix,const float DH_Table[], const float lenght[],float dyn_mat [][10],const float MotorParam[],const float ControlGain[],int RobotDOF,const Link::Links_Type link [], const float MotorConstraints[],PinName PinSensorA[], PinName PinSensorB[],const float SensorProperties[], PinName PinDriver[]);
00024     ~Robot();
00025 
00026     Vector<float> GetTorque();
00027     int GetDOF();
00028     
00029     float ReadSensor(int num);
00030     float ReadVelocity(float prevValue,float sampling,int num);
00031     
00032     Vector<float> ReturnSaturationControl(Vector<float> value);
00033     Vector<float> ReturnComputeP(Vector<float> qref, Vector<float> qi);
00034     Vector<float> ReturnComputePD_FirstVersion(Vector<float> qref, Vector<float> qi, Vector<float> qdotref, Vector<float> qdoti);
00035     Vector<float> ReturnComputePD_SecondVersion(Vector<float> qref, Vector<float> qi, Vector<float> qdoti);
00036     Vector<float> ReturnComputePID(Vector<float> qref, Vector<float> qi, Vector<float> qdotref, Vector<float> qdoti);
00037     Vector<float> ReturnComputePDG(Vector<float> qref, Vector<float> qi, Vector<float> qdotref, Vector<float> qdoti, Vector<float> Tau);
00038     
00039     int SendData(Vector<float> data);
00040     
00041     Matrix <float> GetRotation(int num);
00042     Matrix <float> GetRotation(Matrix<float> mat);
00043     Vector <float> GetPosition(Matrix<float> vect);
00044     
00045     Link GetLink(int num);
00046     
00047     float ConstraintsVelocityMotor(int num);
00048     float ConstraintsAccelerationMotor(int num);
00049     float ConstraintsJerkMotor(int num);
00050     
00051     Vector<float> GetKP();
00052     Vector<float> GetKV();
00053     Vector<float> GetKI();
00054     Vector<float> GetEndPosition();
00055     Matrix<float> GetJacobian();
00056 
00057 
00058     void ForwardKinematics(const float thetaOffset[]);
00059     Vector<float> InverseKinematics(End_Effector* EFF_q0, int conf);
00060     void Jacobian(float q[]);
00061     void EulerNewton(const float q[],const float qd[],const float qdd[]);
00062     void GravityTorque(float q[]);
00063 
00064 
00065 private:
00066     Matrix <float> ToolMatrix;
00067     Matrix <float> BaseMatrix;
00068     Matrix <float> _Jacobian;
00069 
00070     Link *_Linker;
00071     Motor *_Motor;
00072     
00073     int _RobotDOF;
00074 };
00075 
00076 
00077 #endif //UNTITLED_ROBOT_H