hsu han-lin
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