Diff: main.cpp

Revision:

5:77e9dc268338

Parent:

4:8fc956bd0e78

Child:

6:e6e7a2ba9f65

--- a/main.cpp	Fri Mar 31 16:51:55 2017 +0800
+++ b/main.cpp	Fri Mar 31 17:51:51 2017 +0800
@@ -111,12 +111,12 @@
 		eMBPoll();
 		
         /* Here we simply count the number of poll cycles. */
-        usRegHoldingBuf[DEF_INX_TARGET_POSX]++;
+        //usRegHoldingBuf[DEF_INX_TARGET_POSX]++;
 		 
 		if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley
 		{
 			myled=!myled;
-			usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
+			//usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
 		}
 }
 
@@ -187,11 +187,20 @@
 	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};
+	Tar_Point[0]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSX];
+	Tar_Point[1]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSY];
+	Tar_Point[2]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSZ];
+
+	DBGMSG(("Tar_Point[%d]=%d\n",0,Tar_Point[0]))
+	DBGMSG(("Tar_Point[%d]=%d\n",1,Tar_Point[1]))
+	DBGMSG(("Tar_Point[%d]=%d\n",2,Tar_Point[2]))
+
 	//==Target position pulse==//
 	float theta[MAX_AXIS_NUM]={0.0};
 	unsigned int rt=0,i=0;
 	rt = IK_7DOF(L1,L2,L3,0,0,0,usRegHoldingBuf[DEF_INX_TARGET_POSX]*DEF_RATIO_PUS_TO_RAD,usRegHoldingBuf[DEF_INX_TARGET_POSY]*DEF_RATIO_PUS_TO_RAD,usRegHoldingBuf[DEF_INX_TARGET_POSZ]*DEF_RATIO_PUS_TO_RAD,0,0,0,(float)-DEF_PI*0.5F,theta);
+	DBGMSG(("rt=%d\n",rt))
 	if(rt)
 	{
 		return rt;
@@ -200,6 +209,7 @@
 	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
 	{
 		Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS;
+		DBGMSG(("Tar_Pus[%d]=%d\n",i,Tar_Pus[i]))
 	}
 
 	//==Current position pulse==//
@@ -247,9 +257,11 @@
 				
 			break;
 			case IDLE:
+			break;
 			case P2P:
 				P2P_Motion();
 				usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
+			break;
 			case SEWING:
 			case LINE: