Diff: RobotControl_7Axis.h

Revision:

1:584f36ed8717

Parent:

0:c62673155cbf

Child:

2:71ed7da9ea36

--- a/RobotControl_7Axis.h	Fri Feb 10 15:26:47 2017 +0000
+++ b/RobotControl_7Axis.h	Sat Feb 11 00:22:11 2017 +0800
@@ -0,0 +1,176 @@
+#ifndef _ROBOT_CONTROL_7AXIS_HEADER
+#define _ROBOT_CONTROL_7AXIS_HEADER
+
+
+//==========
+//==MAX AXIS
+//==========
+#define MAX_AXIS_NUM  7
+
+
+//===========================
+//==Axis index,ID,NO mapping
+//==========================
+enum{
+	Index_AXIS1=0,
+	Index_AXIS2,
+	Index_AXIS3,
+	Index_AXIS4,
+	Index_AXIS5,
+	Index_AXIS6,
+	Index_AXIS7
+};
+
+enum{
+	ID_AXIS1=1,
+	ID_AXIS2,
+	ID_AXIS3,
+	ID_AXIS4,
+	ID_AXIS5,
+	ID_AXIS6,
+	ID_AXIS7
+};
+
+enum{
+	NO_AXIS1=1,
+	NO_AXIS2,
+	NO_AXIS3,
+	NO_AXIS4,
+	NO_AXIS5,
+	NO_AXIS6,
+	NO_AXIS7
+};
+
+
+static const unsigned char gMapAxisNO[MAX_AXIS_NUM]=
+{
+	NO_AXIS1,
+	NO_AXIS2,
+	NO_AXIS3,
+	NO_AXIS4,
+	NO_AXIS5,
+	NO_AXIS6,
+	NO_AXIS7
+};
+static const unsigned char gMapAxisID[MAX_AXIS_NUM]=
+{
+	ID_AXIS1,
+	ID_AXIS2,
+	ID_AXIS3,
+	ID_AXIS4,
+	ID_AXIS5,
+	ID_AXIS6,
+	ID_AXIS7
+};
+
+
+//================
+//==Unit transform
+//=================
+#define DEF_PI (3.1415F)
+#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
+#define DEF_RATIO_DEG_TO_RAD (DEF_PI/180)		//pi/180	0.01745329251994329576923690768489 (0.0175F)
+#define DEF_RATIO_RAD_TO_PUS (651.8986F)	//4096/2pi	651.89864690440329530934789477382
+
+
+//for read pos unit select
+enum{
+	DEF_UNIT_RAD=1,
+	DEF_UNIT_DEG,
+	DEF_UNIT_PUS
+};
+
+//=====================================
+//==robot hard ware dependent parameter
+//=====================================
+
+//==robot to Motor offset==//  //robot pos=motor position -M2R_OFFSET
+
+#define AXIS1_R2M_OFFSET_DEG 90
+#define AXIS2_R2M_OFFSET_DEG 90
+#define AXIS3_R2M_OFFSET_DEG 225
+#define AXIS4_R2M_OFFSET_DEG 90
+#define AXIS5_R2M_OFFSET_DEG 180
+#define AXIS6_R2M_OFFSET_DEG 90
+#define AXIS7_R2M_OFFSET_DEG 90
+
+//==robot angle limit==//
+#define AXIS1_ROBOT_LIM_DEG_L (-80)
+#define AXIS1_ROBOT_LIM_DEG_H 180
+#define AXIS2_ROBOT_LIM_DEG_L (-18)
+#define AXIS2_ROBOT_LIM_DEG_H 180
+#define AXIS3_ROBOT_LIM_DEG_L (-225)
+#define AXIS3_ROBOT_LIM_DEG_H 105
+#define AXIS4_ROBOT_LIM_DEG_L 0	//need to test
+#define AXIS4_ROBOT_LIM_DEG_H 114	//need to test
+#define AXIS5_ROBOT_LIM_DEG_L (-180)
+#define AXIS5_ROBOT_LIM_DEG_H 148
+#define AXIS6_ROBOT_LIM_DEG_L (-60)
+#define AXIS6_ROBOT_LIM_DEG_H 90
+#define AXIS7_ROBOT_LIM_DEG_L (-30)
+#define AXIS7_ROBOT_LIM_DEG_H 30
+
+
+//==robot TORQUE limit==//
+#define AXIS1_MAX_TORQUE 55
+#define AXIS2_MAX_TORQUE 55
+#define AXIS3_MAX_TORQUE 55
+#define AXIS4_MAX_TORQUE 55
+#define AXIS5_MAX_TORQUE 55
+#define AXIS6_MAX_TORQUE 55
+#define AXIS7_MAX_TORQUE 55
+
+static const unsigned short int gr2m_offset_pulse[MAX_AXIS_NUM]=
+{
+	(unsigned short int)(AXIS1_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
+	(unsigned short int)(AXIS2_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
+	(unsigned short int)(AXIS3_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
+	(unsigned short int)(AXIS4_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
+	(unsigned short int)(AXIS5_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
+	(unsigned short int)(AXIS6_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS),
+	(unsigned short int)(AXIS7_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS)
+};
+
+
+static const float grobot_lim_rad_L[MAX_AXIS_NUM]=
+{
+	AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
+	AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
+	AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
+	AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
+	AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
+	AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD,
+	AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD
+};
+
+static const float grobot_lim_rad_H[MAX_AXIS_NUM]=
+{
+	AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
+	AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
+	AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
+	AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
+	AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
+	AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
+	AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD
+};
+//=================================
+//==morot max pulse in joint mode
+//=================================
+#define DEF_JOINT_MODE_MAX_PULSE 4095
+#define DEF_JOINT_MODE_MIN_PULSE 0
+
+
+
+//Function
+unsigned char getMapAxisNO(unsigned char index); //index 0~ (MAX_AXIS_NUM-1)
+unsigned char getMapAxisID(unsigned char index);
+int ROM_Setting();
+int Read_pos(float *pos,unsigned char unit);
+int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) ;
+
+
+
+
+#endif
\ No newline at end of file