![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
Diff: main.cpp
- Revision:
- 12:1eeea035bf87
- Parent:
- 11:644c13da326d
- Child:
- 13:f0f52287352a
--- a/main.cpp Fri Feb 10 22:56:11 2017 +0800 +++ b/main.cpp Sat Feb 11 00:23:03 2017 +0800 @@ -1,5 +1,7 @@ #include "mbed.h" #include "dynamixel.h" +#include "RobotControl_7Axis.h" + #define DEBUG 1 @@ -13,526 +15,29 @@ -#define MAX_AXIS_NUM 7 -enum{ - Index_AXIS1=0, - Index_AXIS2, - Index_AXIS3, - Index_AXIS4, - Index_AXIS5, - Index_AXIS6, - Index_AXIS7 -}; - -enum{ - ID_AXIS1=1, - ID_AXIS2, - ID_AXIS3, - ID_AXIS4, - ID_AXIS5, - ID_AXIS6, - ID_AXIS7 -}; - -enum{ - NO_AXIS1=1, - NO_AXIS2, - NO_AXIS3, - NO_AXIS4, - NO_AXIS5, - NO_AXIS6, - NO_AXIS7 -}; - - -static const unsigned char gMapAxisNO[MAX_AXIS_NUM]= -{ - NO_AXIS1, - NO_AXIS2, - NO_AXIS3, - NO_AXIS4, - NO_AXIS5, - NO_AXIS6, - NO_AXIS7 -}; -static const unsigned char gMapAxisID[MAX_AXIS_NUM]= -{ - ID_AXIS1, - ID_AXIS2, - ID_AXIS3, - ID_AXIS4, - ID_AXIS5, - ID_AXIS6, - 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 - -enum{ - DEF_UNIT_RAD=1, - DEF_UNIT_DEG, - DEF_UNIT_PUS -}; - - -#define DEF_PI (3.1415F) -#define DEF_RATIO_PUS_TO_DEG (0.0879F) //360/4096 -#define DEF_RATIO_PUS_TO_RAD (0.0015F) //2pi/4096 0.00153398078788564122971808758949 -#define DEF_RATIO_DEG_TO_PUS (11.3778F) //4096/360 -#define DEF_RATIO_DEG_TO_RAD (DEF_PI/180) //pi/180 0.01745329251994329576923690768489 (0.0175F) -#define DEF_RATIO_RAD_TO_PUS (651.8986F) //4096/2pi 651.89864690440329530934789477382 - -//==robot to Motor offset==// robot pos=motor position -M2R_OFFSET -#define AXIS1_R2M_OFFSET_DEG 90 -#define AXIS2_R2M_OFFSET_DEG 90 -#define AXIS3_R2M_OFFSET_DEG 225 -#define AXIS4_R2M_OFFSET_DEG 90 -#define AXIS5_R2M_OFFSET_DEG 180 -#define AXIS6_R2M_OFFSET_DEG 90 -#define AXIS7_R2M_OFFSET_DEG 90 - -static const unsigned short int gr2m_offset_pulse[MAX_AXIS_NUM]= -{ - (unsigned short int)(AXIS1_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), - (unsigned short int)(AXIS2_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), - (unsigned short int)(AXIS3_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), - (unsigned short int)(AXIS4_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), - (unsigned short int)(AXIS5_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), - (unsigned short int)(AXIS6_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS), - (unsigned short int)(AXIS7_R2M_OFFSET_DEG*DEF_RATIO_DEG_TO_PUS) -}; - - -//==robot angle limit==// -#define AXIS1_ROBOT_LIM_DEG_L (-80) -#define AXIS1_ROBOT_LIM_DEG_H 180 -#define AXIS2_ROBOT_LIM_DEG_L (-18) -#define AXIS2_ROBOT_LIM_DEG_H 180 -#define AXIS3_ROBOT_LIM_DEG_L (-225) -#define AXIS3_ROBOT_LIM_DEG_H 105 -#define AXIS4_ROBOT_LIM_DEG_L 0 //need to test -#define AXIS4_ROBOT_LIM_DEG_H 114 //need to test -#define AXIS5_ROBOT_LIM_DEG_L (-180) -#define AXIS5_ROBOT_LIM_DEG_H 148 -#define AXIS6_ROBOT_LIM_DEG_L (-60) -#define AXIS6_ROBOT_LIM_DEG_H 90 -#define AXIS7_ROBOT_LIM_DEG_L (-30) -#define AXIS7_ROBOT_LIM_DEG_H 30 - -static const float grobot_lim_rad_L[MAX_AXIS_NUM]= -{ - AXIS1_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, - AXIS2_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, - AXIS3_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, - AXIS4_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, - AXIS5_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, - AXIS6_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD, - AXIS7_ROBOT_LIM_DEG_L*DEF_RATIO_DEG_TO_RAD -}; - -static const float grobot_lim_rad_H[MAX_AXIS_NUM]= -{ - AXIS1_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, - AXIS2_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, - AXIS3_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, - AXIS4_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, - AXIS5_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, - AXIS6_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD, - AXIS7_ROBOT_LIM_DEG_H*DEF_RATIO_DEG_TO_RAD -}; - - - - -//==robot angle limit==// -#define AXIS1_MAX_TORQUE 55 -#define AXIS2_MAX_TORQUE 55 -#define AXIS3_MAX_TORQUE 55 -#define AXIS4_MAX_TORQUE 55 -#define AXIS5_MAX_TORQUE 55 -#define AXIS6_MAX_TORQUE 55 -#define AXIS7_MAX_TORQUE 55 - -//==morot max pulse in joint mode==// -#define DEF_JOINT_MODE_MAX_PULSE 4095 -#define DEF_JOINT_MODE_MIN_PULSE 0 - - - //==Pin define==// DigitalOut myled(LED1); Serial pc(SERIAL_TX, SERIAL_RX); DigitalIn mybutton(USER_BUTTON); -int ROM_Setting() //new -{ - //==calculate Max torque to set in rom - const short int Max_torque[MAX_AXIS_NUM]= - { - AXIS1_MAX_TORQUE, - AXIS2_MAX_TORQUE, - AXIS3_MAX_TORQUE, - AXIS4_MAX_TORQUE, - AXIS5_MAX_TORQUE, - AXIS6_MAX_TORQUE, - AXIS7_MAX_TORQUE - }; - - - //==Calculate angle limit==// - const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]= - { - AXIS1_R2M_OFFSET_DEG, - AXIS2_R2M_OFFSET_DEG, - AXIS3_R2M_OFFSET_DEG, - AXIS4_R2M_OFFSET_DEG, - AXIS5_R2M_OFFSET_DEG, - AXIS6_R2M_OFFSET_DEG, - AXIS7_R2M_OFFSET_DEG - }; - - const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]= - { - AXIS1_ROBOT_LIM_DEG_L, - AXIS2_ROBOT_LIM_DEG_L, - AXIS3_ROBOT_LIM_DEG_L, - AXIS4_ROBOT_LIM_DEG_L, - AXIS5_ROBOT_LIM_DEG_L, - AXIS6_ROBOT_LIM_DEG_L, - AXIS7_ROBOT_LIM_DEG_L - }; - - const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]= - { - AXIS1_ROBOT_LIM_DEG_H, - AXIS2_ROBOT_LIM_DEG_H, - AXIS3_ROBOT_LIM_DEG_H, - AXIS4_ROBOT_LIM_DEG_H, - AXIS5_ROBOT_LIM_DEG_H, - AXIS6_ROBOT_LIM_DEG_H, - AXIS7_ROBOT_LIM_DEG_H - }; - - unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0}; - unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0}; - - - int i=0; - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS; - Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS; - } - - - - //==writing to ROM==// - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - //==Set MAX_torgue==// - dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);// more safe - - //==Set angel limit==// - dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]); - dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]); - - //==Set MULTITURN_OFFSET to 0==// - dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0); - } - - - //==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=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - pc.printf("===AXIS_%d===\n",gMapAxisNO[i]); - - //==MAX_torgue==// - max_torque = dxl_read_word(gMapAxisID[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(gMapAxisID[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=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG); - - ccw_angle_lim=dxl_read_word(gMapAxisID[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=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG); - - - //==multi turn offset==// - multi_turn_offset=dxl_read_word(gMapAxisID[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 Read_pos(float *pos,unsigned char unit) -{ - int i=0; - short int pulse=0; - int rt=0; - - if(unit==DEF_UNIT_RAD) - { - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); - if(dxl_get_result()!=COMM_RXSUCCESS) - { - rt=-gMapAxisNO[i]; - pos[i]=0xffff; - } - else - { - pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset - pos[i]=pulse*DEF_RATIO_PUS_TO_RAD; - } - } - - } - else if(unit==DEF_UNIT_DEG) - { - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); - if(dxl_get_result()!=COMM_RXSUCCESS) - { - rt=-gMapAxisNO[i]; - pos[i]=0xffff; - } - else - { - pulse-=gr2m_offset_pulse[i]; - pos[i]=pulse*DEF_RATIO_PUS_TO_DEG; - } - } - } - else if(unit==DEF_UNIT_PUS) - { - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); - if(dxl_get_result()!=COMM_RXSUCCESS) - { - rt=-gMapAxisNO[i]; - pos[i]=0xffff; - } - else - { - pulse-=gr2m_offset_pulse[i]; - pos[i]=pulse; - } - } - - } - else - { - //non offset value - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS); - if(dxl_get_result()!=COMM_RXSUCCESS) - { - rt=-gMapAxisNO[i]; - pos[i]=0xffff; - } - } - } - - return rt; -} - - - - -int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) //new -{ - unsigned char i=0; - - //===================================================================// - //==limit axis if not zero ,the return value is the overlimit axis==// - //===================================================================// - //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - //{ - // if( (Ang_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) ) - // { - // DBGMSG(("AXIS%d over limit Ang_rad=%f,grobot_lim_rad_L=%f,grobot_lim_rad_H=%f\n",gMapAxisNO[i],Ang_rad[i],grobot_lim_rad_L[i],grobot_lim_rad_H[i])) - // return -gMapAxisNO[i]; - // } - //} - - //===================================================// - //==trnasformat to pulse and offset to motor domain==// - //====================================================// - short int Ang_pulse[MAX_AXIS_NUM]={0}; - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS); - Ang_pulse[i]+=gr2m_offset_pulse[i]; - - if( Ang_pulse[i] > DEF_JOINT_MODE_MAX_PULSE )// 0~4095 - { - DBGMSG(("AXIS%d over range of mortor Ang_pulse=%d,JOINT_MODE_MIN_PULSE=%d,JOINT_MODE_MAX_PULSE=%d\n",gMapAxisNO[i],Ang_pulse[i],DEF_JOINT_MODE_MIN_PULSE,DEF_JOINT_MODE_MAX_PULSE)) - return -gMapAxisNO[i]; - } - } - - //================================// - //==output to motor by syncpage===// - //===============================// - unsigned short int SyncPage1[21]= - { - ID_AXIS1,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity - ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2], - ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3], - ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4], - ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5], - ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6], - ID_AXIS7,(unsigned short int)Ang_pulse[Index_AXIS7],velocity[Index_AXIS7], - }; - - -#if (DEBUG) - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]); -#endif - - syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length); - - return 0; -} - - -/* -int Output_to_Dynamixel(float *Ang_rad) //old -{ - //========================// - //==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; unsigned char i=0; - //==================// - //==dxl_initialize==// - //===================// - DBGMSG(("start\n")) - //pc.printf("start\n",rt); + //================== + //==dxl_initialize + //=================== rt=dxl_initialize( 1, 1); DBGMSG(("dxl_initialize rt=%d\n",rt)) - //pc.printf("dxl_initialize rt=%d\n",rt); - - + //=========================// //==ROM parameter setting==// //=========================// //rt=ROM_Setting(); //while(1); - //myled = 0; // LED is ON - + float Ang_rad[MAX_AXIS_NUM]={0}; unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10}; while(1) @@ -559,76 +64,45 @@ //==Read robot pos by pos_deg==// float pos_deg[MAX_AXIS_NUM]={0}; - rt=Read_pos(pos_deg,DEF_UNIT_RAD); - pc.printf("Read_pos rt==%d\n",rt); + rt=Read_pos(pos_deg,DEF_UNIT_DEG); + DBGMSG(("Read_pos rt==%d\n",rt)); for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { - pc.printf("X%d=%.2f,",gMapAxisNO[i],pos_deg[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"); - - - - - - //====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; + } } + + +//===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"); \ No newline at end of file