Diff: main.cpp

Revision:

7:9127ccc07448

Parent:

6:e6e7a2ba9f65

Child:

8:0adb0b96d630

diff -r e6e7a2ba9f65 -r 9127ccc07448 main.cpp
--- a/main.cpp	Sat Apr 08 21:29:21 2017 +0800
+++ b/main.cpp	Sun Apr 09 21:50:03 2017 +0800
@@ -16,7 +16,7 @@
 
 
 DigitalOut myled(LED1); //stanley
-
+DigitalOut led_D7(D7,PullUp); //stanley
 
 Serial pc(USBTX, USBRX);  //stanley
 
@@ -109,23 +109,25 @@
 {
 	//(void)eMBPoll(  ); origianl
 		//
+		//led_D7=1;
 		eMBPoll();
-		
+		//led_D7=0;
+
         /* Here we simply count the number of poll cycles. */
         //usRegHoldingBuf[DEF_INX_TARGET_POSX]++;
 		 
-		if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley
-		{
-			myled=!myled;
-			//usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
-		}
+		//if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley
+		//{
+		//	myled=!myled;
+		//	//usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
+		//}
 }
 
 unsigned int Modbus_Initial(void)
 {
 	eMBErrorCode    eStatus;
 
-    eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE );
+    eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 460800, MB_PAR_NONE );
 
     /* Enable the Modbus Protocol Stack. */
 	eMBEnable(  );
@@ -165,8 +167,8 @@
 	{
 		Ang_rad[i]=usRegHoldingBuf[DEF_INX_TARPOS1+i]*DEF_RATIO_PUS_TO_RAD;
 
-		DBGMSG(("HB[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
-		DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
+		//DBGMSG(("HB[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
+		//DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
 	}
 
 	
@@ -226,7 +228,7 @@
 	//}
 
 	//== Output_to_Dynamixel==//
-	unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
+	unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,500};
 	rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity); 
 	
 	return rt;
@@ -237,12 +239,15 @@
 	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;
 	//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];	
+			usRegHoldingBuf[DEF_INX_POSVAL1+i]=(unsigned int)pos_pus[i];	
 	
 	}
 	else
@@ -263,7 +268,7 @@
 
 void ReadMotorInfo_Initial()
 {
-	tReadMotorInfo.attach_us(&ReadMotorInfo,5000);
+	tReadMotorInfo.attach_us(&ReadMotorInfo,40000);  //Read_pos may cost 10ms
 }
 
 
@@ -277,7 +282,7 @@
 
 	rt=dxl_initialize( 1, 1);
 
-	ReadMotorInfo_Initial();
+	//ReadMotorInfo_Initial();
 
 	DBGMSG(("dxl_initialize rt=%d\n",rt))
 
@@ -310,7 +315,9 @@
 
     
 		}
-		wait_ms(100);
+
+		//ReadMotorInfo();
+		wait_ms(20);
 	
 	}