File content as of revision 11:644c13da326d:

#include "mbed.h"
#include "dynamixel.h"


#define DEBUG 1


#if (DEBUG)
	#define DBGMSG(x)  pc.printf x;
#else
    #define DBGMSG(x)
#endif



#define MAX_AXIS_NUM  7
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
};
//#define AXIS1_PULSE_LIM_L (-910)
//#define AXIS1_PULSE_LIM_H 2048
//#define AXIS2_PULSE_LIM_L (-205)
//#define AXIS2_PULSE_LIM_H 2048
//#define AXIS3_PULSE_LIM_L (-2840)
//#define AXIS3_PULSE_LIM_H 1190
//#define AXIS4_PULSE_LIM_L 0//need to test
//#define AXIS4_PULSE_LIM_H 0//need to test
//#define AXIS5_PULSE_LIM_L (-2048)
//#define AXIS5_PULSE_LIM_H 1680
//#define AXIS6_PULSE_LIM_L (-680)
//#define AXIS6_PULSE_LIM_H 1024
//#define AXIS7_PULSE_LIM_L (-420)
//#define AXIS7_PULSE_LIM_H 420

enum{
	DEF_UNIT_RAD=1,
	DEF_UNIT_DEG,
	DEF_UNIT_PUS
};


#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

//==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

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)
};


//==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

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
};




//==robot angle 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

//==morot max pulse in joint mode==//
#define DEF_JOINT_MODE_MAX_PULSE 4095
#define DEF_JOINT_MODE_MIN_PULSE 0



//==Pin define==//
DigitalOut myled(LED1);
Serial pc(SERIAL_TX, SERIAL_RX);
DigitalIn mybutton(USER_BUTTON);

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)  //new
{
	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;
}


/*
int Output_to_Dynamixel(float *Ang_rad) //old
{
  //========================//
  //==trnasformat to pulse==//
  //========================//
  short int Ang_pulse[MAX_AXIS_NUM]={0};
  short int i=0;
  for(i=0;i<MAX_AXIS_NUM;i++)
  {
    Ang_pulse[i]=(int)(Ang_rad[i]*57.3*11.378); //radian => degree => pulse
  }

  //===================================================================//
  //==limit axis  if not zero ,the return value is the overlimit axis==//
  //===================================================================//
	static int Axis_Pulse_lim_H[MAX_AXIS_NUM]=
	{    
	AXIS1_PULSE_LIM_H,
	AXIS2_PULSE_LIM_H,
	AXIS3_PULSE_LIM_H,
	AXIS4_PULSE_LIM_H,
	AXIS5_PULSE_LIM_H,
	AXIS6_PULSE_LIM_H,
	AXIS7_PULSE_LIM_H};
    
	static int Axis_Pulse_lim_L[MAX_AXIS_NUM]=
	{    
	AXIS1_PULSE_LIM_L,
	AXIS2_PULSE_LIM_L,
	AXIS3_PULSE_LIM_L,
	AXIS4_PULSE_LIM_L,
	AXIS5_PULSE_LIM_L,
	AXIS6_PULSE_LIM_L,
	AXIS7_PULSE_LIM_L};  

	for(i=0;i<MAX_AXIS_NUM;i++)
	{
		if( (Ang_pulse[i] > Axis_Pulse_lim_H[i]) || (Ang_pulse[i] < Axis_Pulse_lim_L[i]) )
		{
			pc.printf("over limit Ang_pulse[%d]=%d,Axis_Pulse_lim_H[%d]=%d,Axis_Pulse_lim_L[%d]=%d\n",i,Ang_pulse[i],i,Axis_Pulse_lim_H[i],i,Axis_Pulse_lim_L[i]);
			return i+1;
		}
	}

	//================================//
	//==output to motor by syncpage===//
	//===============================//
	short int velocity[MAX_AXIS_NUM]={10,10,10,10,30,30,30};
  
	short int SyncPage1[21]=
	{ 
		ID_AXIS1,Ang_pulse[0],velocity[0], //ID,goal,velocity
		ID_AXIS2,Ang_pulse[1],velocity[1], 
		ID_AXIS3,Ang_pulse[2],velocity[2], 
		ID_AXIS4,Ang_pulse[3],velocity[3], 
		ID_AXIS5,Ang_pulse[4],velocity[4], 
		ID_AXIS6,Ang_pulse[5],velocity[5], 
		ID_AXIS7,Ang_pulse[6],velocity[6], 
	};
   
	for(i=0;i<MAX_AXIS_NUM;i++)
		pc.printf("Ang_pulse[%d]=%d velocity[%d]=%d\n",i,Ang_pulse[i],i,velocity[i]);



	syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
  
	return 0;
}
*/

int main()
{
	int rt =0;
	unsigned char i=0;

	//==================//
	//==dxl_initialize==//
	//===================//
	DBGMSG(("start\n"))
    //pc.printf("start\n",rt);
    rt=dxl_initialize( 1, 1);
	DBGMSG(("dxl_initialize rt=%d\n",rt))
    //pc.printf("dxl_initialize rt=%d\n",rt);

	
	//=========================//
	//==ROM parameter setting==//
	//=========================//
	//rt=ROM_Setting();
	//while(1);
  
	//myled = 0; // LED is ON
   
	float Ang_rad[MAX_AXIS_NUM]={0};
	unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
    while(1)
    {
		//==Test Output_to_Dynamixel==//
		//Ang_rad[Index_AXIS5]=160*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS6]=100*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS7]=90*DEF_RATIO_DEG_TO_RAD;
		//rt=Output_to_Dynamixel(Ang_rad,velocity);
		//pc.printf("Output_to_Dynamixel rt=%d\n",rt);
	
		//while(mybutton);

		//
		//Ang_rad[Index_AXIS5]=-180*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS6]=-70*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS7]=-90*DEF_RATIO_DEG_TO_RAD;
		//rt=Output_to_Dynamixel(Ang_rad,velocity);
		//pc.printf("Output_to_Dynamixel rt=%d\n",rt);

		//while(mybutton);



		//==Read robot pos by pos_deg==//
		float pos_deg[MAX_AXIS_NUM]={0};
		rt=Read_pos(pos_deg,DEF_UNIT_RAD);
		pc.printf("Read_pos rt==%d\n",rt);

		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		{
			pc.printf("X%d=%.2f,",gMapAxisNO[i],pos_deg[i]);
		}
		
		//===Test move==//
        //dxl_write_word(3,GOAL_POSITION,400);
        //setPosition(3,2048,10);
         //pc.printf("after=%d\n",rt);
        //myled = 1; // LED is ON
        //wait(4);
        //dxl_write_word(3,GOAL_POSITION,-400);
        //setPosition(3,-2048,10);
        //myled = 0; // LED is ON
        //wait(4);
		

		//===Test read pos====//
		/*pos=dxl_read_word(3,PRESENT_POS);
		pc.printf("pos=%d\n",pos);
		wait_ms(200);    */

		//====Test read all pos===
		//static short int pos=0;
		//int i=0;
		//for(i=ID_AXIS1;i<=ID_AXIS7;i++)
		//{
		//  pos = dxl_read_word(i, PRESENT_POS);
		//  if(dxl_get_result()!=COMM_RXSUCCESS)
		//	pos=-1;

		//  pc.printf("X%d=%d,",i,pos);
		//}
		//pc.printf("\n");
		

		


		//====Test syncWrite_u16base===
		/*static short int pos3=-100;
		pos3=(pos3==-100)? 100 :-100;

		short int SyncPage1[21]=
		{ 
			ID_AXIS1,100,5,
			ID_AXIS2,100,5,
			ID_AXIS3,100,5,
			ID_AXIS4,100,5,
			ID_AXIS5,100,5,
			ID_AXIS6,100,5,
			ID_AXIS7,100,5,
		};

		wait(0.5);    */
		//short int SyncPage1[21]=//Test use
		//{ 
		//	0x00,0x010,0x150, // 3 Dynamixels are move to position 512
		//	0x01,0x020,0x360,
		//	0x02,0x030,0x170,
		//	0x03,0x220,0x380,
		//	0x05,0x123,0x121,
		//	0x06,0x234,0x143,
		//	0x07,0x145,0x167
		//};

		//syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length;
	
	}
}