Diff: main.cpp

Revision:

13:f0f52287352a

Parent:

12:1eeea035bf87

Child:

14:235c64640200

diff -r 1eeea035bf87 -r f0f52287352a main.cpp
--- a/main.cpp	Sat Feb 11 00:23:03 2017 +0800
+++ b/main.cpp	Sat Feb 11 13:07:35 2017 +0000
@@ -17,6 +17,8 @@
 
 //==Pin define==//
 DigitalOut myled(LED1);
+
+DigitalOut Test(D4,PullDown);
 Serial pc(SERIAL_TX, SERIAL_RX);
 DigitalIn mybutton(USER_BUTTON);
 
@@ -26,6 +28,25 @@
 	unsigned char i=0;
 
 	//==================
+	//== Test IK  it need 120ms to calculate
+	//===================
+	
+	DBGMSG(("start"))
+	float theta[7]={0};
+	
+	myled=1;
+	rt = IK_7DOF(L1,L2,L3,0,0,0,60,0,0,0,0,0,(float)-DEF_PI*0.5F,theta);
+	myled=0;
+
+
+	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
+	{
+		DBGMSG(("X%d=%f,",getMapAxisNO(i),theta[i]))
+	}
+	DBGMSG(("\n"))
+	while(1);
+
+	//==================
 	//==dxl_initialize
 	//===================
     rt=dxl_initialize( 1, 1);