Revision 3:e380c7db9133, committed 2017-03-31

Comitter:

stanley1228

Date:

Fri Mar 31 16:31:30 2017 +0800

Parent:

2:71ed7da9ea36

Commit message:

1.Add output_to_dynamixel_pulse
2.grobot_lim_pus_H
3.grobot_lim_pus_L

Changed in this revision

--- a/RobotControl_7Axis.cpp	Sat Feb 11 13:06:49 2017 +0000
+++ b/RobotControl_7Axis.cpp	Fri Mar 31 16:31:30 2017 +0800
@@ -292,7 +292,61 @@
 	return 0;
 }
 
+int Output_to_Dynamixel_pulse(const unsigned short int *Ang_pulse,const unsigned short int *velocity) 
+{
+	unsigned char i=0;
 
+	//===================================================================//
+	//==limit axis  if not zero ,the return value is the overlimit axis==//
+	//===================================================================//
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		if( (Ang_pulse[i] > grobot_lim_pus_H[i]) || (Ang_pulse[i] < grobot_lim_pus_L[i]) )
+		{
+			DBGMSG(("AXIS%d over limit Ang_pus=%d,grobot_lim_pus_L=%d,grobot_lim_pus_H=%d\n",gMapAxisNO[i],Ang_pulse[i],grobot_lim_pus_L[i],grobot_lim_pus_H[i]))
+			return -gMapAxisNO[i];
+		}
+	}
+
+	//====================================================//
+	//==trnasformat to pulse and offset to motor domain===//
+	//====================================================//
+	unsigned short int Ang_pulse_with_offset[MAX_AXIS_NUM]={0};
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		Ang_pulse_with_offset[i]=Ang_pulse[i]+gr2m_offset_pulse[i];
+
+		if( Ang_pulse_with_offset[i] > DEF_JOINT_MODE_MAX_PULSE )//  0~4095
+		{
+			DBGMSG(("AXIS%d over range of mortor  Ang_pulse=%d,JOINT_MODE_MIN_PULSE=%d,JOINT_MODE_MAX_PULSE=%d\n",gMapAxisNO[i],Ang_pulse_with_offset[i],DEF_JOINT_MODE_MIN_PULSE,DEF_JOINT_MODE_MAX_PULSE))
+			return -gMapAxisNO[i];
+		}
+	}
+
+	//================================//
+	//==output to motor by syncpage===//
+	//===============================//
+	unsigned short int SyncPage1[21]=
+	{ 
+		ID_AXIS1,Ang_pulse_with_offset[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity
+		ID_AXIS2,Ang_pulse_with_offset[Index_AXIS2],velocity[Index_AXIS2], 
+		ID_AXIS3,Ang_pulse_with_offset[Index_AXIS3],velocity[Index_AXIS3], 
+		ID_AXIS4,Ang_pulse_with_offset[Index_AXIS4],velocity[Index_AXIS4], 
+		ID_AXIS5,Ang_pulse_with_offset[Index_AXIS5],velocity[Index_AXIS5], 
+		ID_AXIS6,Ang_pulse_with_offset[Index_AXIS6],velocity[Index_AXIS6], 
+		ID_AXIS7,Ang_pulse_with_offset[Index_AXIS7],velocity[Index_AXIS7], 
+	};
+
+	
+#if (DEBUG)
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+		pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]);
+#endif
+
+	syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
+  
+	return 0;
+}
 
 
 //歐拉 Z1X2Y3 Intrinsic Rotaions相對於當前坐標系的的旋轉

--- a/RobotControl_7Axis.h	Sat Feb 11 13:06:49 2017 +0000
+++ b/RobotControl_7Axis.h	Fri Mar 31 16:31:30 2017 +0800
@@ -156,6 +156,28 @@
 	AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD,
 	AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD
 };
+
+static const float grobot_lim_pus_L[MAX_AXIS_NUM]=
+{
+	AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS,
+	AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS,
+	AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS,
+	AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS,
+	AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS,
+	AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS,
+	AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_PUS
+};
+
+static const float grobot_lim_pus_H[MAX_AXIS_NUM]=
+{
+	AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS,
+	AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS,
+	AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS,
+	AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS,
+	AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS,
+	AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS,
+	AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_PUS
+};
 //=================================
 //==morot max pulse in joint mode
 //=================================
@@ -182,6 +204,7 @@
 int ROM_Setting();
 int Read_pos(float *pos,unsigned char unit);
 int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) ;
+int Output_to_Dynamixel_pulse(const unsigned short int *Ang_pus,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);