Diff: main.cpp

Revision:

4:8fc956bd0e78

Parent:

1:249f89a36223

Child:

5:77e9dc268338

--- a/main.cpp	Fri Mar 31 03:55:32 2017 +0000
+++ b/main.cpp	Fri Mar 31 16:51:55 2017 +0800
@@ -1,7 +1,10 @@
+//1.IK ¯ÊAlpha beta gamma
+
 #include "mb.h"
 #include "mbport.h"
 #include "mbed.h" //stanley
 #include "dynamixel.h"
+#include "RobotControl_7Axis.h"
 
 #define DEBUG 1
 
@@ -21,7 +24,7 @@
 #define REG_INPUT_START (3001)  //not use now stanley
 #define REG_INPUT_NREGS 15		//not use now stanley
 #define REG_HOLDING_START (4001)
-#define REG_HOLDING_NREGS 28
+#define REG_HOLDING_NREGS 29
 
 #define SLAVE_ID 0x0A
 #define DEF_MAX_AXIS 7
@@ -70,15 +73,26 @@
 	DEF_INX_VELVAL4,
 	DEF_INX_VELVAL5,
 	DEF_INX_VELVAL6,
-	DEF_INX_VELVAL7
+	DEF_INX_VELVAL7,
+
+	DEF_INX_ERR_STATUS,
+	DEF_INX_STATE
+};
+
+enum eState
+{
+	DONE=0,
+	BUSY
+	
 };
 
 enum eOPMode
 {
-    JOG,
+    JOG=0,
     P2P,
     SEWING,
-    LINE
+    LINE,
+	IDLE
 };
 /* ----------------------- Static variables ---------------------------------*/
 static USHORT   usRegInputStart = REG_INPUT_START; //not use now stanley
@@ -106,31 +120,20 @@
 		}
 }
 
-
-int main( void )
+unsigned int  Modbus_Initail(void)
 {
-    eMBErrorCode    eStatus;
+	eMBErrorCode    eStatus;
 
     eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE );
 
     /* Enable the Modbus Protocol Stack. */
 	eMBEnable(  );
-    //eStatus = eMBEnable(  );
     
-	
-
-    // Initialise some registers
- //   usRegInputBuf[1] = 0x1234;
- //   usRegInputBuf[2] = 0x5678;
- //   usRegInputBuf[3] = 0x9abc;  
-	//usRegInputBuf[4] = 0x1000;
- //   usRegInputBuf[5] = 0x1001;  
- //   usRegInputBuf[6] = 0x1002;  
-
+	//trial initial value
 	usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
 	usRegHoldingBuf[DEF_INX_TARGET_POSY]=1000;
 	usRegHoldingBuf[DEF_INX_TARGET_POSZ]=2000;
-	usRegHoldingBuf[DEF_INX_OPMODE]=JOG;
+	usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
 	
 	float fSpeedRatio=0.345;
 	USHORT* usp=(USHORT*)&fSpeedRatio;
@@ -144,11 +147,122 @@
 		usRegHoldingBuf[DEF_INX_VELVAL1+i]=2000+i;
 	}
 	
+	
+	tMobusPoll.attach_us(&fMobusPoll,5000);
 
-	tMobusPoll.attach_us(&fMobusPoll,10000);
+	return 0;
+}
+
+unsigned int Jog_Motion(void)
+{
+	unsigned int rt=0;
+
+	float Ang_rad[MAX_AXIS_NUM]={0};
+	unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
+
+	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]))
+		DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
+	}
+
+	
+	
+
+	//== Output_to_Dynamixel==//
+	rt=Output_to_Dynamixel(Ang_rad,velocity);
+
+	return rt;
+}
+
+
+
+unsigned int P2P_Motion(void)
+{
+	//==================
+	//== Test IK  it need 120ms to calculate
+	//===================
+	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};
+	
+	//==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);
+	if(rt)
+	{
+		return rt;
+	}
+
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS;
+	}
+
+	//==Current position pulse==//
+	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++)
+	//{
+	//	Ofset_Pus[i]=Tar_Pus[i]-Cur_Pus[i];
+	//}
+
+	//== Output_to_Dynamixel==//
+	unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
+	rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity); 
+	
+	return rt;
+}
+
+
+int main( void )
+{
+	unsigned int rt=0;
+    Modbus_Initail();
+	
 	myled=1;//stanley
 	
-	while(1);
+
+	rt=dxl_initialize( 1, 1);
+	DBGMSG(("dxl_initialize rt=%d\n",rt))
+
+	while(1)
+	{
+
+		switch(usRegHoldingBuf[DEF_INX_OPMODE])
+		{
+
+			case JOG:
+				//usRegHoldingBuf[DEF_INX_STATE]=BUSY;
+				Jog_Motion();
+				//usRegHoldingBuf[DEF_INX_STATE]=DONE;
+				usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
+				
+			break;
+			case IDLE:
+			case P2P:
+				P2P_Motion();
+				usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
+			case SEWING:
+			case LINE:
+			
+			
+			
+			break;
+
+
+    
+		}
+		wait_ms(100);
+	
+	}
    
 }