Diff: main.cpp

Revision:

6:e6e7a2ba9f65

Parent:

5:77e9dc268338

Child:

7:9127ccc07448

--- a/main.cpp	Fri Mar 31 17:51:51 2017 +0800
+++ b/main.cpp	Sat Apr 08 21:29:21 2017 +0800
@@ -103,6 +103,7 @@
 
 /* ----------------------- Start implementation -----------------------------*/
 Ticker tMobusPoll;
+Ticker tReadMotorInfo;
 
 void fMobusPoll()
 {
@@ -120,7 +121,7 @@
 		}
 }
 
-unsigned int  Modbus_Initail(void)
+unsigned int Modbus_Initial(void)
 {
 	eMBErrorCode    eStatus;
 
@@ -184,7 +185,7 @@
 	//==================
 	//== Test IK  it need 120ms to calculate
 	//===================
-	unsigned short int Cur_Pus[MAX_AXIS_NUM]={0};
+	//unsigned short int Cur_Pus[MAX_AXIS_NUM]={0};
 	unsigned short int Tar_Pus[MAX_AXIS_NUM]={0};
 	//short int Ofset_Pus[MAX_AXIS_NUM]={0};
 	short int Tar_Point[3]={0};
@@ -213,10 +214,10 @@
 	}
 
 	//==Current position pulse==//
-	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
-	{
-		Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS);
-	}
+	//for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	//{
+	//	Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS);
+	//}
 	
 	//==offset postion pulse==// ©|¥¼¥[¤J³t«×³W¹º
 	//for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
@@ -231,16 +232,53 @@
 	return rt;
 }
 
+void ReadMotorInfo()
+{
+	static float pos_pus[MAX_AXIS_NUM]={0};
+	unsigned int rt=0;
+
+	rt=Read_pos(pos_pus,DEF_UNIT_PUS);
+	//DBGMSG(("Read_pos rt==%d\n",rt));
+	if(rt==0)
+	{
+		for(int i=0; i<MAX_AXIS_NUM;i++)
+			usRegHoldingBuf[DEF_INX_TARPOS1+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]));
+	//}
+	//for(i=ID_AXIS1;i<=ID_AXIS7;i++)
+	//{
+	//	pos = dxl_read_word(i, PRESENT_POS);
+	//	if(dxl_get_result()!=COMM_RXSUCCESS)
+	//		pos=-1;
+	//}
+}
+
+void ReadMotorInfo_Initial()
+{
+	tReadMotorInfo.attach_us(&ReadMotorInfo,5000);
+}
+
 
 int main( void )
 {
 	unsigned int rt=0;
-    Modbus_Initail();
+    Modbus_Initial();
 	
 	myled=1;//stanley
 	
 
 	rt=dxl_initialize( 1, 1);
+
+	ReadMotorInfo_Initial();
+
 	DBGMSG(("dxl_initialize rt=%d\n",rt))
 
 	while(1)