Diff: main.cpp

Revision:

8:37f5a7219fe6

Parent:

7:15f0494813f7

Child:

9:54710454ce60

diff -r 15f0494813f7 -r 37f5a7219fe6 main.cpp
--- a/main.cpp	Wed Feb 08 12:27:32 2017 +0800
+++ b/main.cpp	Wed Feb 08 15:50:53 2017 +0800
@@ -15,7 +15,7 @@
   ID_AXIS7
 };
 
-#define MAX_AXIS_NUM     7
+#define MAX_AXIS_NUM  ID_AXIS7
 
 #define AXIS1_PULSE_LIM_L (-910)
 #define AXIS1_PULSE_LIM_H 2048
@@ -37,6 +37,70 @@
 Serial pc(SERIAL_TX, SERIAL_RX);
 
 
+int ROM_Setting()
+{
+	
+	int i=0;
+
+	for(i=ID_AXIS1;i<=ID_AXIS7;i++)
+	{
+		//Set MAX_torgue
+		dxl_write_word(i,MAX_TORQUE,55);//  more safe
+	
+		//Set all to multi turn mode==//
+		dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff);
+		dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff);  
+
+		//Set all to multi turn mode==//
+		dxl_write_word(i,MULTITURN_OFFSET,-1024);
+	}
+	
+
+	//==read and check ==//
+	int	txrx_result=0;
+	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++)
+	{
+		pc.printf("===AXIS_%d===\n",i);
+
+		//MAX_torgue
+		max_torque = dxl_read_word(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);
+		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);
+
+		ccw_angle_lim=dxl_read_word(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);
+		
+
+		//multi turn offset==//
+		multi_turn_offset=dxl_read_word(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);
+		else	
+			pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset);
+	}
+
+	return 0;
+}
+
 int main()
 {
    int rt =0;
@@ -46,15 +110,19 @@
     rt=dxl_initialize( 1, 1);
     pc.printf("dxl_initialize rt=%d\n",rt);
 
-    myled = 0; // LED is ON
-    
+	short int pos=0;
+	
+	//==ROM parameter setting==//
+	//rt=ROM_Setting();
+	
+  
+	myled = 0; // LED is ON
     /**************************
     *** Check communication ***
     **************************/
     //dxl_write_byte(3,ADDRESS_LED,0);
     //dxl_ping(3);
-    
-	short int pos=0; //16bit
+   
     while(1)
     {
 		//pc.printf("before=%d\n",rt);
@@ -106,7 +174,7 @@
 			ID_AXIS7,100,5,
 		};
 
-		wait(2);    
+		wait(5);    
 		//short int SyncPage1[21]=//Test use
 		//{ 
 		//	0x00,0x010,0x150, // 3 Dynamixels are move to position 512