Diff: main.cpp

Revision:

8:0adb0b96d630

Parent:

7:9127ccc07448

diff -r 9127ccc07448 -r 0adb0b96d630 main.cpp
--- a/main.cpp	Sun Apr 09 21:50:03 2017 +0800
+++ b/main.cpp	Mon Apr 10 23:08:29 2017 +0800
@@ -59,23 +59,23 @@
 	DEF_INX_TARPOS6,
 	DEF_INX_TARPOS7,
 
-	DEF_INX_POSVAL1=13,
-	DEF_INX_POSVAL2,
-	DEF_INX_POSVAL3,
-	DEF_INX_POSVAL4,
-	DEF_INX_POSVAL5,
-	DEF_INX_POSVAL6,
-	DEF_INX_POSVAL7,
+	DEF_INX_TARVEL1=13,
+    DEF_INX_TARVEL2,
+    DEF_INX_TARVEL3,
+    DEF_INX_TARVEL4,
+    DEF_INX_TARVEL5,
+    DEF_INX_TARVEL6,
+    DEF_INX_TARVEL7,
 
-	DEF_INX_VELVAL1=20,
-	DEF_INX_VELVAL2,
-	DEF_INX_VELVAL3,
-	DEF_INX_VELVAL4,
-	DEF_INX_VELVAL5,
-	DEF_INX_VELVAL6,
-	DEF_INX_VELVAL7,
+    DEF_INX_POSVAL1 = 20,
+    DEF_INX_POSVAL2,
+    DEF_INX_POSVAL3,
+    DEF_INX_POSVAL4,
+    DEF_INX_POSVAL5,
+    DEF_INX_POSVAL6,
+    DEF_INX_POSVAL7,
 
-	DEF_INX_ERR_STATUS,
+	DEF_INX_ERR_STATE,
 	DEF_INX_STATE
 };
 
@@ -147,7 +147,7 @@
 	{
 		usRegHoldingBuf[DEF_INX_TARPOS1+i]=500+i;
 		usRegHoldingBuf[DEF_INX_POSVAL1+i]=1000+i;
-		usRegHoldingBuf[DEF_INX_VELVAL1+i]=2000+i;
+		usRegHoldingBuf[DEF_INX_TARVEL1+i]=20;
 	}
 	
 	
@@ -166,8 +166,9 @@
 	for(int i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
 	{
 		Ang_rad[i]=usRegHoldingBuf[DEF_INX_TARPOS1+i]*DEF_RATIO_PUS_TO_RAD;
-
-		//DBGMSG(("HB[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
+		velocity[i]=usRegHoldingBuf[DEF_INX_TARVEL1+i];
+		DBGMSG(("p[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
+		DBGMSG(("v[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARVEL1+i]))
 		//DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
 	}
 
@@ -236,24 +237,43 @@
 
 void ReadMotorInfo()
 {
-	static float pos_pus[MAX_AXIS_NUM]={0};
-	unsigned int rt=0;
+	//static float pos_pus[MAX_AXIS_NUM]={0};
+	//unsigned int rt=0;
 
 
 	led_D7=1;
-	rt=Read_pos(pos_pus,DEF_UNIT_PUS);
-	led_D7=0;
+	//rt=Read_pos(pos_pus,DEF_UNIT_PUS);
+	
 	//DBGMSG(("Read_pos rt==%d\n",rt));
-	if(rt==0)
+	static USHORT pulse=0;
+
+	for(int i=Index_AXIS1;i<MAX_AXIS_NUM-1;i++)
 	{
-		for(int i=0; i<MAX_AXIS_NUM;i++)
-			usRegHoldingBuf[DEF_INX_POSVAL1+i]=(unsigned int)pos_pus[i];	
-	
+
+		DBGMSG(("gMapAxisID[%d]=%d\n",i,gMapAxisID[i]))
+		pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
+		if(dxl_get_result()==COMM_RXSUCCESS)
+		{
+			pulse-=gr2m_offset_pulse[i]; 
+			usRegHoldingBuf[DEF_INX_POSVAL1+i]=pulse;	
+			DBGMSG(("pulse%d=%d\n",i,pulse))
+		}
+
+		
 	}
-	else
-	{
-		usRegHoldingBuf[DEF_INX_ERR_STATUS]=rt;
-	}
+	led_D7=0;
+
+
+	//if(rt==0)
+	//{
+	//	for(int i=0; i<MAX_AXIS_NUM;i++)
+	//		usRegHoldingBuf[DEF_INX_POSVAL1+i]=(unsigned int)pos_pus[i];	
+	//
+	//}
+	//else
+	//{
+	//	usRegHoldingBuf[DEF_INX_ERR_STATUS]=rt;
+	//}
 	//for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
 	//{
 	//	DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
@@ -316,7 +336,7 @@
     
 		}
 
-		//ReadMotorInfo();
+		ReadMotorInfo();
 		wait_ms(20);
 	
 	}