hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

Diff: main.cpp

Revision:
13:f0f52287352a
Parent:
12:1eeea035bf87
Child:
14:235c64640200
--- 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);