Diff: RobotControl_7Axis.h

Revision:

2:71ed7da9ea36

Parent:

1:584f36ed8717

Child:

3:e380c7db9133

diff -r 584f36ed8717 -r 71ed7da9ea36 RobotControl_7Axis.h
--- a/RobotControl_7Axis.h	Sat Feb 11 00:22:11 2017 +0800
+++ b/RobotControl_7Axis.h	Sat Feb 11 13:06:49 2017 +0000
@@ -1,7 +1,8 @@
 #ifndef _ROBOT_CONTROL_7AXIS_HEADER
 #define _ROBOT_CONTROL_7AXIS_HEADER
 
-
+#include "Matrix.h"
+//#include "MatrixMath.h"
 //==========
 //==MAX AXIS
 //==========
@@ -67,7 +68,7 @@
 //================
 //==Unit transform
 //=================
-#define DEF_PI (3.1415F)
+#define DEF_PI (3.1415926F)
 #define DEF_RATIO_PUS_TO_DEG (0.0879F)			//360/4096
 #define DEF_RATIO_PUS_TO_RAD (0.0015F)		 //2pi/4096   0.00153398078788564122971808758949
 #define DEF_RATIO_DEG_TO_PUS (11.3778F)		//4096/360
@@ -161,6 +162,18 @@
 #define DEF_JOINT_MODE_MAX_PULSE 4095
 #define DEF_JOINT_MODE_MIN_PULSE 0
 
+//===================
+//==ROBOT link length
+//====================
+#define L0 0     	//可能要刪掉
+#define L1 100    	//upper arm
+#define L2 100   	//forearm
+#define L3 10     	//length of end effector
+#define X_BASE 0  	//基準點只能都先設0
+#define Y_BASE 0
+#define Z_BASE 0
+
+
 
 
 //Function
@@ -169,8 +182,10 @@
 int ROM_Setting();
 int Read_pos(float *pos,unsigned char unit);
 int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) ;
-
-
+Matrix R_z1x2y3(float alpha,float beta,float gamma);
+float norm(const Matrix& v);
+Matrix Rogridues(float theta,const Matrix& V_A);
+int IK_7DOF(const float l1,const float l2,const float l3,const float x_base,const float y_base,const float z_base,const float x_end,const float y_end,const float z_end,const float alpha,const float beta,const float gamma,const float Rednt_alpha,float* theta);
 
 
 #endif
\ No newline at end of file