File content as of revision 10:328cc5179ffb:

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




//stanley robot test define//
enum{
  ID_AXIS1=1,
  ID_AXIS2,
  ID_AXIS3,
  ID_AXIS4,
  ID_AXIS5,
  ID_AXIS6,
  ID_AXIS7
};

#define MAX_AXIS_NUM  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

#define DEF_PI (3.1415)


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

int ROM_Setting()
{
	
	int i=0;

	for(i=ID_AXIS1;i<=ID_AXIS7;i++)
	{
		//Set MAX_torgue
		dxl_write_word(i,MAX_TORQUE,55);//  more safe
	
		//Set all to multi turn mode==//
		dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff);
		dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff);  

		//Set all to multi turn mode==//
		dxl_write_word(i,MULTITURN_OFFSET,-1024);
	}
	

	//==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=ID_AXIS1;i<=ID_AXIS7;i++)
	{
		pc.printf("===AXIS_%d===\n",i);

		//MAX_torgue
		max_torque = dxl_read_word(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(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=%x\n",cw_angel_lim);

		ccw_angle_lim=dxl_read_word(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=%x\n",ccw_angle_lim);
		

		//multi turn offset==//
		multi_turn_offset=dxl_read_word(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 Output_to_Dynamixel(float *Ang_rad)
{
  //========================//
  //==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;
   
	//==================//
	//==dxl_initialize==//
	//===================//
    pc.printf("start\n",rt);
    rt=dxl_initialize( 1, 1);
    pc.printf("dxl_initialize rt=%d\n",rt);

	
	//=========================//
	//==ROM parameter setting==//
	//=========================//
	//rt=ROM_Setting();
	
  
	//myled = 0; // LED is ON
   
	float Ang_rad[MAX_AXIS_NUM]={0};
    while(1)
    {
		//==Test Output_to_Dynamixel==//
		
		Ang_rad[4]=90*DEF_PI/180;
		Ang_rad[5]=90*DEF_PI/180;
		Ang_rad[6]=30*DEF_PI/180;
	
		rt=Output_to_Dynamixel(Ang_rad);
		pc.printf("Output_to_Dynamixel rt=%d",rt);
	
		while(mybutton);

		Ang_rad[4]=0*DEF_PI/180;
		Ang_rad[5]=0*DEF_PI/180;
		Ang_rad[6]=0*DEF_PI/180;
	
		rt=Output_to_Dynamixel(Ang_rad);
		pc.printf("Output_to_Dynamixel rt=%d",rt);

		while(mybutton);

		//pc.printf("before=%d\n",rt);
    

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