File content as of revision 17:2b67b218da4d:

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

DigitalOut Test(D4,PullDown);
Serial pc(SERIAL_TX, SERIAL_RX);
DigitalIn mybutton(USER_BUTTON);


int main()
{
	int rt =0;
	unsigned char i=0;
	
	//==================
	//== Receive from PC by uart
	//===================
	//char buffer[128];
//        
//    pc.gets(buffer, 10);
//    for(i=0;i<10;i++)
//    {
//    	 DBGMSG(("buffer[%d]=%d,",i,buffer[i]))		
//    }
   
    
	
//	while(1);
	//==================
	//== Test IK  it need 120ms to calculate
	//===================
	
	//DBGMSG(("start"))
//	float theta[7]={0};
//	
//	myled=1;
//	rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta);
//	myled=0;
//
//
//	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
//	{
//		DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i]))
//	}
//	DBGMSG(("\n"))
	

	//==================
	//==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]=140*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS6]=50*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS7]=10*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]=0*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS6]=-40*DEF_RATIO_DEG_TO_RAD;
		//Ang_rad[Index_AXIS7]=-10*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]));
		}

		wait_ms(500);
		/*unsigned short int pos=0;
		for(i=ID_AXIS1;i<=ID_AXIS7;i++)
		{
		  pos = dxl_read_word(i, PRESENT_POS);
		  if((rt=dxl_get_result())!=COMM_RXSUCCESS)
			DBGMSG(("X%d dxl_get_result=%d\n",i,rt))
		  else
			DBGMSG(("X%d=%d\n",i,pos))
		}*/
		
	}
}



//===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");