hsu han-lin
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
Diff: main.cpp
- Revision:
- 11:644c13da326d
- Parent:
- 10:328cc5179ffb
- Child:
- 12:1eeea035bf87
diff -r 328cc5179ffb -r 644c13da326d main.cpp
--- a/main.cpp Thu Feb 09 17:59:27 2017 +0800
+++ b/main.cpp Fri Feb 10 22:56:11 2017 +0800
@@ -2,37 +2,173 @@
#include "dynamixel.h"
+#define DEBUG 1
+
+
+#if (DEBUG)
+ #define DBGMSG(x) pc.printf x;
+#else
+ #define DBGMSG(x)
+#endif
+
+
+
+#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
+};
+
+
-//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
+//==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
-#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
+//==morot max pulse in joint mode==//
+#define DEF_JOINT_MODE_MAX_PULSE 4095
+#define DEF_JOINT_MODE_MIN_PULSE 0
-#define DEF_PI (3.1415)
//==Pin define==//
@@ -40,22 +176,80 @@
Serial pc(SERIAL_TX, SERIAL_RX);
DigitalIn mybutton(USER_BUTTON);
-int ROM_Setting()
+int ROM_Setting() //new
{
-
- int i=0;
+ //==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
+ };
+
- for(i=ID_AXIS1;i<=ID_AXIS7;i++)
+ //==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]=
{
- //Set MAX_torgue
- dxl_write_word(i,MAX_TORQUE,55);// more safe
+ 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};
- //Set all to multi turn mode==//
- dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff);
- dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff);
+
+ 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;
+ }
+
+
- //Set all to multi turn mode==//
- dxl_write_word(i,MULTITURN_OFFSET,-1024);
+ //==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);
}
@@ -64,36 +258,36 @@
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++)
+ for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
{
- pc.printf("===AXIS_%d===\n",i);
+ pc.printf("===AXIS_%d===\n",gMapAxisNO[i]);
- //MAX_torgue
- max_torque = dxl_read_word(i, MAX_TORQUE);
+ //==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(i,CW_ANGLE_LIMIT_L);
+ //==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=%x\n",cw_angel_lim);
+ 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(i,CCW_ANGLE_LIMIT_L);
+ 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=%x\n",ccw_angle_lim);
+ 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(i,MULTITURN_OFFSET);
+ //==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);
@@ -104,7 +298,148 @@
return 0;
}
-int Output_to_Dynamixel(float *Ang_rad)
+
+
+
+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==//
@@ -173,53 +508,65 @@
return 0;
}
+*/
int main()
{
int rt =0;
-
+ unsigned char i=0;
+
//==================//
//==dxl_initialize==//
//===================//
- pc.printf("start\n",rt);
+ DBGMSG(("start\n"))
+ //pc.printf("start\n",rt);
rt=dxl_initialize( 1, 1);
- pc.printf("dxl_initialize rt=%d\n",rt);
+ 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)
{
//==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;
+ //Ang_rad[Index_AXIS5]=160*DEF_RATIO_DEG_TO_RAD;
+ //Ang_rad[Index_AXIS6]=100*DEF_RATIO_DEG_TO_RAD;
+ //Ang_rad[Index_AXIS7]=90*DEF_RATIO_DEG_TO_RAD;
+ //rt=Output_to_Dynamixel(Ang_rad,velocity);
+ //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
- rt=Output_to_Dynamixel(Ang_rad);
- pc.printf("Output_to_Dynamixel rt=%d",rt);
-
- while(mybutton);
+ //while(mybutton);
+
+ //
+ //Ang_rad[Index_AXIS5]=-180*DEF_RATIO_DEG_TO_RAD;
+ //Ang_rad[Index_AXIS6]=-70*DEF_RATIO_DEG_TO_RAD;
+ //Ang_rad[Index_AXIS7]=-90*DEF_RATIO_DEG_TO_RAD;
+ //rt=Output_to_Dynamixel(Ang_rad,velocity);
+ //pc.printf("Output_to_Dynamixel rt=%d\n",rt);
- 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);
+
+
- while(mybutton);
+ //==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);
- //pc.printf("before=%d\n",rt);
-
-
+ for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+ {
+ pc.printf("X%d=%.2f,",gMapAxisNO[i],pos_deg[i]);
+ }
+
//===Test move==//
//dxl_write_word(3,GOAL_POSITION,400);
//setPosition(3,2048,10);