File content as of revision 4:53ef39fbf9d9:

#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");
		wait_ms(500);    
    }
}