Revision 7:9127ccc07448, committed 2017-04-09

Comitter:

stanley1228

Date:

Sun Apr 09 21:50:03 2017 +0800

Parent:

6:e6e7a2ba9f65

Child:

8:0adb0b96d630

Commit message:

1.change baudrate to 460800
2.comment readmotorinfo() now because when add this the system become more unstable
3. change velocity of axis7 to 500 but doesn't work
4. fMobusPoll ticker change to 5ms don't know how is the good

Changed in this revision

--- 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);
 	
 	}