hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

Files at this revision

API Documentation at this revision

Revision 17:2b67b218da4d, committed 2017-04-17

Comitter:
stanley1228
Date:
Mon Apr 17 13:51:42 2017 +0800
Parent:
16:f956f24960e7
Commit message:
1.??????position

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Fri Mar 31 16:33:17 2017 +0800
+++ b/main.cpp	Mon Apr 17 13:51:42 2017 +0800
@@ -78,37 +78,47 @@
 	unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
     while(1)
     {
-		//==Test Output_to_Dynamixel==//
-		Ang_rad[Index_AXIS5]=140*DEF_RATIO_DEG_TO_RAD;
-		Ang_rad[Index_AXIS6]=50*DEF_RATIO_DEG_TO_RAD;
-		Ang_rad[Index_AXIS7]=10*DEF_RATIO_DEG_TO_RAD;
-		rt=Output_to_Dynamixel(Ang_rad,velocity);
-		pc.printf("Output_to_Dynamixel rt=%d\n",rt);
+		////==Test Output_to_Dynamixel==//
+		//Ang_rad[Index_AXIS5]=140*DEF_RATIO_DEG_TO_RAD;
+		//Ang_rad[Index_AXIS6]=50*DEF_RATIO_DEG_TO_RAD;
+		//Ang_rad[Index_AXIS7]=10*DEF_RATIO_DEG_TO_RAD;
+		//rt=Output_to_Dynamixel(Ang_rad,velocity);
+		//pc.printf("Output_to_Dynamixel rt=%d\n",rt);
 	
-		while(mybutton);
+		//while(mybutton);
 
-		//
-		Ang_rad[Index_AXIS5]=0*DEF_RATIO_DEG_TO_RAD;
-		Ang_rad[Index_AXIS6]=-40*DEF_RATIO_DEG_TO_RAD;
-		Ang_rad[Index_AXIS7]=-10*DEF_RATIO_DEG_TO_RAD;
-		rt=Output_to_Dynamixel(Ang_rad,velocity);
-		pc.printf("Output_to_Dynamixel rt=%d\n",rt);
+		////
+		//Ang_rad[Index_AXIS5]=0*DEF_RATIO_DEG_TO_RAD;
+		//Ang_rad[Index_AXIS6]=-40*DEF_RATIO_DEG_TO_RAD;
+		//Ang_rad[Index_AXIS7]=-10*DEF_RATIO_DEG_TO_RAD;
+		//rt=Output_to_Dynamixel(Ang_rad,velocity);
+		//pc.printf("Output_to_Dynamixel rt=%d\n",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_DEG);
-//		DBGMSG(("Read_pos rt==%d\n",rt));
-//
-//		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
-//		{
-//			DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
-//		}
-	
-	
+		float pos_deg[MAX_AXIS_NUM]={0};
+		rt=Read_pos(pos_deg,DEF_UNIT_DEG);
+		DBGMSG(("Read_pos rt==%d\n",rt));
+
+		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+		{
+			DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
+		}
+
+		wait_ms(500);
+		/*unsigned short int pos=0;
+		for(i=ID_AXIS1;i<=ID_AXIS7;i++)
+		{
+		  pos = dxl_read_word(i, PRESENT_POS);
+		  if((rt=dxl_get_result())!=COMM_RXSUCCESS)
+			DBGMSG(("X%d dxl_get_result=%d\n",i,rt))
+		  else
+			DBGMSG(("X%d=%d\n",i,pos))
+		}*/
+		
 	}
 }