Diff: main.cpp

Revision:

14:235c64640200

Parent:

13:f0f52287352a

Child:

17:2b67b218da4d

--- a/main.cpp	Sat Feb 11 13:07:35 2017 +0000
+++ b/main.cpp	Thu Feb 16 11:38:26 2017 +0000
@@ -22,29 +22,44 @@
 Serial pc(SERIAL_TX, SERIAL_RX);
 DigitalIn mybutton(USER_BUTTON);
 
+
 int main()
 {
 	int rt =0;
 	unsigned char i=0;
-
+	
+	//==================
+	//== Receive from PC by uart
+	//===================
+	//char buffer[128];
+//        
+//    pc.gets(buffer, 10);
+//    for(i=0;i<10;i++)
+//    {
+//    	 DBGMSG(("buffer[%d]=%d,",i,buffer[i]))		
+//    }
+   
+    
+	
+//	while(1);
 	//==================
 	//== Test IK  it need 120ms to calculate
 	//===================
 	
-	DBGMSG(("start"))
-	float theta[7]={0};
+	//DBGMSG(("start"))
+//	float theta[7]={0};
+//	
+//	myled=1;
+//	rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta);
+//	myled=0;
+//
+//
+//	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+//	{
+//		DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i]))
+//	}
+//	DBGMSG(("\n"))
 	
-	myled=1;
-	rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta);
-	myled=0;
-
-
-	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
-	{
-		DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i]))
-	}
-	DBGMSG(("\n"))
-	while(1);
 
 	//==================
 	//==dxl_initialize
@@ -64,40 +79,41 @@
     while(1)
     {
 		//==Test Output_to_Dynamixel==//
-		//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);
+		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]=-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[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]));
+//		}
 	
 	
 	}
 }
 
 
+
 //===Test move==//
 //dxl_write_word(3,GOAL_POSITION,400);
 //setPosition(3,2048,10);