Diff: main.cpp

Revision:

10:328cc5179ffb

Parent:

9:54710454ce60

Child:

11:644c13da326d

diff -r 54710454ce60 -r 328cc5179ffb main.cpp
--- a/main.cpp	Thu Feb 09 17:18:40 2017 +0800
+++ b/main.cpp	Thu Feb 09 17:59:27 2017 +0800
@@ -38,7 +38,7 @@
 //==Pin define==//
 DigitalOut myled(LED1);
 Serial pc(SERIAL_TX, SERIAL_RX);
-
+DigitalIn mybutton(USER_BUTTON);
 
 int ROM_Setting()
 {
@@ -194,24 +194,33 @@
   
 	//myled = 0; // LED is ON
    
-	//==Test Output_to_Dynamixel==//
 	float Ang_rad[MAX_AXIS_NUM]={0};
-	Ang_rad[4]=90*DEF_PI/180;
-	Ang_rad[5]=90*DEF_PI/180;
-	Ang_rad[6]=30*DEF_PI/180;
-		
-	//dxl_write_word(ID_AXIS5,MAX_TORQUE,55);
-	//dxl_write_word(ID_AXIS5,TORQUE_LIMIT,512);
-
-	rt=Output_to_Dynamixel(Ang_rad);
-	pc.printf("Output_to_Dynamixel rt=%d",rt);
-	
     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;
+	
+		rt=Output_to_Dynamixel(Ang_rad);
+		pc.printf("Output_to_Dynamixel rt=%d",rt);
+	
+		while(mybutton);
+
+		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);
+
 		//pc.printf("before=%d\n",rt);
     
 
-		//===Test move
+		//===Test move==//
         //dxl_write_word(3,GOAL_POSITION,400);
         //setPosition(3,2048,10);
          //pc.printf("after=%d\n",rt);
@@ -229,17 +238,17 @@
 		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;
+		//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");
+		//  pc.printf("X%d=%d,",i,pos);
+		//}
+		//pc.printf("\n");
 		
 
 		
@@ -272,7 +281,6 @@
 		//	0x07,0x145,0x167
 		//};
 
-
 		//syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length;
 	
 	}