File content as of revision 7:15f0494813f7:

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

#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


DigitalOut myled(LED1);
Serial pc(SERIAL_TX, SERIAL_RX);


int main()
{
   int rt =0;
   
    pc.printf("start\n",rt);
  
    rt=dxl_initialize( 1, 1);
    pc.printf("dxl_initialize rt=%d\n",rt);

    myled = 0; // LED is ON
    
    /**************************
    *** Check communication ***
    **************************/
    //dxl_write_byte(3,ADDRESS_LED,0);
    //dxl_ping(3);
    
	short int pos=0; //16bit
    while(1)
    {
		//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===
		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(2);    
		//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;
	
	}
}