File content as of revision 12:1eeea035bf87:

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



#define DEBUG 1


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



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

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

	//==================
	//==dxl_initialize
	//===================
    rt=dxl_initialize( 1, 1);
	DBGMSG(("dxl_initialize rt=%d\n",rt))
    
	//=========================//
	//==ROM parameter setting==//
	//=========================//
	//rt=ROM_Setting();
	//while(1);
  
	
	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_DEG);
		DBGMSG(("Read_pos rt==%d\n",rt));

		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		{
			DBGMSG(("X%d=%.2f,",getMapAxisNO(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");