Diff: main.cpp

Revision:

17:2b67b218da4d

Parent:

14:235c64640200

diff -r f956f24960e7 -r 2b67b218da4d main.cpp
--- 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))
+		}*/
+		
 	}
 }