Diff: RobotControl_7Axis.cpp

Revision:

1:584f36ed8717

Parent:

0:c62673155cbf

Child:

2:71ed7da9ea36

diff -r c62673155cbf -r 584f36ed8717 RobotControl_7Axis.cpp
--- a/RobotControl_7Axis.cpp	Fri Feb 10 15:26:47 2017 +0000
+++ b/RobotControl_7Axis.cpp	Sat Feb 11 00:22:11 2017 +0800
@@ -0,0 +1,288 @@
+#include "mbed.h"
+#include "dynamixel.h"
+#include "RobotControl_7Axis.h"
+
+extern Serial pc;
+
+
+#if (DEBUG)
+	#define DBGMSG(x)  pc.printf x;
+#else
+    #define DBGMSG(x)
+#endif
+
+
+
+unsigned char getMapAxisNO(unsigned char index)
+{
+	return gMapAxisNO[index];
+
+}
+
+unsigned char getMapAxisID(unsigned char index)
+{
+	return gMapAxisID[index];
+}
+
+
+int ROM_Setting() //new
+{
+	//==calculate Max torque to set in rom 
+	const short int Max_torque[MAX_AXIS_NUM]=
+	{
+		AXIS1_MAX_TORQUE,
+		AXIS2_MAX_TORQUE,
+		AXIS3_MAX_TORQUE,
+		AXIS4_MAX_TORQUE,
+		AXIS5_MAX_TORQUE,
+		AXIS6_MAX_TORQUE,
+		AXIS7_MAX_TORQUE
+	};
+
+
+	//==Calculate angle limit==//
+	const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]=
+	{
+		AXIS1_R2M_OFFSET_DEG,
+		AXIS2_R2M_OFFSET_DEG,
+		AXIS3_R2M_OFFSET_DEG,
+		AXIS4_R2M_OFFSET_DEG,
+		AXIS5_R2M_OFFSET_DEG,
+		AXIS6_R2M_OFFSET_DEG,
+		AXIS7_R2M_OFFSET_DEG
+	};
+
+	const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]=
+	{
+		AXIS1_ROBOT_LIM_DEG_L,
+		AXIS2_ROBOT_LIM_DEG_L,
+		AXIS3_ROBOT_LIM_DEG_L,
+		AXIS4_ROBOT_LIM_DEG_L,
+		AXIS5_ROBOT_LIM_DEG_L,
+		AXIS6_ROBOT_LIM_DEG_L,
+		AXIS7_ROBOT_LIM_DEG_L
+	};
+
+	const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]=
+	{
+		AXIS1_ROBOT_LIM_DEG_H,
+		AXIS2_ROBOT_LIM_DEG_H,
+		AXIS3_ROBOT_LIM_DEG_H,
+		AXIS4_ROBOT_LIM_DEG_H,
+		AXIS5_ROBOT_LIM_DEG_H,
+		AXIS6_ROBOT_LIM_DEG_H,
+		AXIS7_ROBOT_LIM_DEG_H
+	};
+
+	unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0};
+	unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0};
+	
+
+	int i=0;
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
+		Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
+	}
+
+
+
+	//==writing to ROM==//
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		//==Set MAX_torgue==//
+		dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);//  more safe
+	
+		//==Set angel limit==//
+		dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]);
+		dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]);  
+
+		//==Set MULTITURN_OFFSET to 0==//
+		dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0);
+	}
+	
+
+	//==read and check ==//
+	int	txrx_result=0;
+	short int max_torque=0;
+	short int cw_angel_lim=0,ccw_angle_lim=0;
+	short int multi_turn_offset=0;
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		pc.printf("===AXIS_%d===\n",gMapAxisNO[i]);
+
+		//==MAX_torgue==//
+		max_torque = dxl_read_word(gMapAxisID[i], MAX_TORQUE);
+		txrx_result = dxl_get_result();
+		if(txrx_result!=COMM_RXSUCCESS)
+			pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result);
+		else
+			pc.printf("MAX_TORQUE=%d\n",max_torque);
+	
+		//==CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==//
+		cw_angel_lim=dxl_read_word(gMapAxisID[i],CW_ANGLE_LIMIT_L);
+		txrx_result = dxl_get_result();
+		if(txrx_result!=COMM_RXSUCCESS)
+			pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result);
+		else	
+			pc.printf("CW_ANGLE_LIMIT=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG);
+
+		ccw_angle_lim=dxl_read_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L);
+		txrx_result = dxl_get_result();
+		if(txrx_result!=COMM_RXSUCCESS)
+			pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result);
+		else	
+			pc.printf("CCW_ANGLE_LIMIT=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG);
+		
+
+		//==multi turn offset==//
+		multi_turn_offset=dxl_read_word(gMapAxisID[i],MULTITURN_OFFSET);
+		txrx_result = dxl_get_result();
+		if(txrx_result!=COMM_RXSUCCESS)
+			pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result);
+		else	
+			pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset);
+	}
+
+	return 0;
+}
+
+
+
+
+int Read_pos(float *pos,unsigned char unit)
+{
+	int i=0;
+	short int pulse=0;
+	int rt=0;
+
+	if(unit==DEF_UNIT_RAD)
+	{
+		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+		{
+			pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
+			if(dxl_get_result()!=COMM_RXSUCCESS)
+			{
+				rt=-gMapAxisNO[i];
+				pos[i]=0xffff;
+			}
+			else
+			{
+				pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset
+				pos[i]=pulse*DEF_RATIO_PUS_TO_RAD;
+			}
+		}
+		
+	}
+	else if(unit==DEF_UNIT_DEG)
+	{
+		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+		{
+			pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
+			if(dxl_get_result()!=COMM_RXSUCCESS)
+			{
+				rt=-gMapAxisNO[i];	
+				pos[i]=0xffff;
+			}
+			else
+			{
+				pulse-=gr2m_offset_pulse[i]; 
+				pos[i]=pulse*DEF_RATIO_PUS_TO_DEG;
+			}
+		}
+	}
+	else if(unit==DEF_UNIT_PUS)
+	{
+		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+		{
+			pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
+			if(dxl_get_result()!=COMM_RXSUCCESS)
+			{
+				rt=-gMapAxisNO[i];
+				pos[i]=0xffff;
+			}
+			else
+			{
+				pulse-=gr2m_offset_pulse[i]; 
+				pos[i]=pulse;
+			}
+		}
+		
+	}
+	else
+	{
+		//non offset value
+		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+		{
+			pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS);
+			if(dxl_get_result()!=COMM_RXSUCCESS)
+			{
+				rt=-gMapAxisNO[i];
+				pos[i]=0xffff;
+			}
+		}
+	}
+	
+	return rt;
+}
+
+
+
+
+int Output_to_Dynamixel(const float *Ang_rad,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_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) )
+		{
+			DBGMSG(("AXIS%d over limit Ang_rad=%f,grobot_lim_rad_L=%f,grobot_lim_rad_H=%f\n",gMapAxisNO[i],Ang_rad[i],grobot_lim_rad_L[i],grobot_lim_rad_H[i]))
+			return -gMapAxisNO[i];
+		}
+	}
+
+	//===================================================//
+	//==trnasformat to pulse and offset to motor domain==//
+	//====================================================//
+	short int Ang_pulse[MAX_AXIS_NUM]={0};
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS);
+		Ang_pulse[i]+=gr2m_offset_pulse[i];
+
+		if( Ang_pulse[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[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,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity
+		ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2], 
+		ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3], 
+		ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4], 
+		ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5], 
+		ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6], 
+		ID_AXIS7,(unsigned short int)Ang_pulse[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;
+}
+