姿态插补，三减函数加减速

Matlab2CPP/Matlab2CPP/Debug/Matlab2CPP.log

@@ -1,3 +1,2 @@
-  pch.cpp
-  Matlab2CPP.cpp
+  Matlab2CPP.cpp
   Matlab2CPP.vcxproj -> C:\Users\Administrator\Desktop\MyRobot\Matlab2CPP\Debug\Matlab2CPP.exe

Matlab2CPP/Matlab2CPP/Matlab2CPP.cpp

@@ -6,15 +6,24 @@
 #include <Windows.h>
 #include "Kinematic.h"
 
-using namespace std;
+long nErr;
+long nPort;
+AmsAddr Addr;
+PAmsAddr pAddr;
 
+using namespace std;
+	//pAddr = &Addr;
+	//nPort = AdsPortOpen();
+	//nErr = AdsGetLocalAddress(pAddr);
+	//pAddr->port = 851;
 int main()
 {
 	Kinematic kine;//new一个对象
 	Trajplanning traj;
 
+	double Ratio[6] = { 49.99, 64.56, 99.69, 101.81, 160.68, 121.0 };//抓手->底座
 
-	double angle[6] = { 10,0,30,40,0,60 };
+	double angle[6] = { 0,0,0,0,90,0 };
 	for (int i = 0; i < 6; i++) angle[i] *= (PI / 180.0);
 
 	Theta Angle_Now(angle, angle + 6);
@@ -28,34 +37,35 @@ int main()
 
 	Array T1(T);
 	T1[0][3] = 0.2;
-	T1[1][3] = 0.2;
+	T1[1][3] = -0.2;
 	T1[2][3] = -0.3;
 
-	Array Pos = traj.MoveLine(T, T1, 0.2, 0.1, 0.001);
-
+	//Array Pos = traj.MoveLine(T, T1, 0.2, 0.1, 0.001);
+	//Array Pos2Angle(size(Pos[0]),vector<double>(6,0));
+	//for (int i = 0; i < size(Pos[0]); i++) 
+	//{
+	//	T1[0][3] = Pos[0][i];
+	//	T1[1][3] = Pos[1][i];
+	//	T1[2][3] = Pos[2][i];
+	//	Pos2Angle[i] = kine.Ikine_Step(T1, Angle_Last);
+	//}
+
 	LARGE_INTEGER t1, t2, tc;
 	QueryPerformanceFrequency(&tc);
 	QueryPerformanceCounter(&t1);
 
-	//traj.MoveLine(T, T1, 0.2, 0.1, 0.001);
+	traj.MoveLine(T, T1, 0.2, 0.1, 0.001);
 	//Array Angleout(size(Pos[0]),vector<double>(6,0));
 
-	//	T1[0][3] = Pos[0][i];
-	//	T1[1][3] = Pos[1][i];
-	//	T1[2][3] = Pos[2][i];
-		angleout = kine.Ikine_Step(T, Angle_Last);
-	//	Angleout[i] = angleout;
-
+	QueryPerformanceCounter(&t2);
+	printf("\n  Total Time:%f ms\n", \
+		(t2.QuadPart - t1.QuadPart)*1000.0 / tc.QuadPart);
 
 
-	QueryPerformanceCounter(&t2);
 	//for (int i = 0; i < size(Pos[0]); i++)
 	//{
 	//	cout << Pos[0][i] << ",  " << Pos[1][i] << ",  " << Pos[2][i] << endl;
 	//}
-	printf("\n  Total Time:%f ms\n", \
-		(t2.QuadPart - t1.QuadPart)*1000.0 / tc.QuadPart);
-
 
 	for (int i = 0; i < 4; i++)
 	{
@@ -86,4 +96,13 @@ int main()
 
 	//double B = 0.3501;
 	//double A = 0.3497;
-	//double D = atan2(B, A)*180/PI;
+	//double D = atan2(B, A)*180/PI;
+
+
+
+
+//ULONG     lHdlVar;
+//char   szVar[] = { "GVL.ExtPos[0]" };
+//nErr = AdsSyncReadWriteReq(pAddr, ADSIGRP_SYM_HNDBYNAME, 0x0, sizeof(lHdlVar), &lHdlVar, sizeof(szVar), szVar);
+//nErr = AdsSyncWriteReq(pAddr, ADSIGRP_SYM_VALBYHND, lHdlVar, 8, ExtPos);
+//nErr = AdsSyncWriteReq(pAddr, ADSIGRP_SYM_RELEASEHND, 0, sizeof(lHdlVar), &lHdlVar);

Matlab2CPP/Matlab2CPP/Matlab2CPP.vcxproj

@@ -75,6 +75,8 @@
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
     <LinkIncremental>true</LinkIncremental>
+    <IncludePath>C:\TwinCAT\AdsApi\TcAdsDll\Include;$(IncludePath)</IncludePath>
+    <LibraryPath>C:\TwinCAT\AdsApi\TcAdsDll\Lib;$(LibraryPath)</LibraryPath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
     <LinkIncremental>false</LinkIncremental>
@@ -106,10 +108,13 @@
       <PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
       <ConformanceMode>true</ConformanceMode>
       <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+      <AdditionalIncludeDirectories>C:\TwinCAT\AdsApi\TcAdsDll\Include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>
       <GenerateDebugInformation>true</GenerateDebugInformation>
+      <AdditionalDependencies>TcAdsDll.lib;%(AdditionalDependencies)</AdditionalDependencies>
+      <AdditionalLibraryDirectories>C:\TwinCAT\AdsApi\TcAdsDll\Lib;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
     </Link>
   </ItemDefinitionGroup>
   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
@@ -166,6 +171,9 @@
     </ClCompile>
     <ClCompile Include="Trajplanning.cpp" />
   </ItemGroup>
+  <ItemGroup>
+    <Library Include="TcAdsDll.lib" />
+  </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">
   </ImportGroup>

Matlab2CPP/Matlab2CPP/Matlab2CPP.vcxproj.filters

@@ -39,4 +39,7 @@
       <Filter>源文件</Filter>
     </ClCompile>
   </ItemGroup>
+  <ItemGroup>
+    <Library Include="TcAdsDll.lib" />
+  </ItemGroup>
 </Project>

Matlab2CPP/Matlab2CPP/TcAdsAPI.h

@@ -0,0 +1,244 @@
+////////////////////////////////////////////////////////////////////////////////
+//        File: TcAdsAPI.h
+// Description: Prototypes and Definitions for non C++ Applications
+//      Author: RamonB
+//     Created: Wed Nov 6 10:00:00 1996
+//
+//
+// BECKHOFF-Industrieelektronik-GmbH
+//
+// Modifications:	
+// KlausBue 11/1999
+// Register Callback for Router notifications
+//
+// ChristophC 16/07/2001
+// Double definition of router callback function removed 
+// 
+////////////////////////////////////////////////////////////////////////////////
+#ifndef __ADSAPI_H__
+#define __ADSAPI_H__
+
+#define ADSAPIERR_NOERROR			0x0000
+
+////////////////////////////////////////////////////////////////////////////////
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+__declspec( dllexport ) 
+long __stdcall AdsGetDllVersion( void );
+
+__declspec( dllexport )
+long	__stdcall AdsPortOpen( void ); 
+
+__declspec( dllexport )
+long	__stdcall AdsPortClose( void );
+
+__declspec( dllexport )
+long	__stdcall AdsGetLocalAddress( AmsAddr*	pAddr );
+
+__declspec( dllexport )
+long __stdcall AdsSyncWriteReq(	AmsAddr*	pServerAddr,	// Ams address of ADS server
+							 unsigned long	indexGroup,		//	index group in ADS server interface
+							 unsigned long	indexOffset,	// index offset in ADS server interface
+							 unsigned long	length,			// count of bytes to write
+							 void*				pData				// pointer to the client buffer
+							);
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadReq( AmsAddr*	pAddr,						// Ams address of ADS server           
+							 unsigned long		indexGroup,		//	index group in ADS server interface
+							 unsigned long		indexOffset,	// index offset in ADS server interface
+							 unsigned long		length,			// count of bytes to read
+							 void*				pData				// pointer to the client buffer
+							);
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadReqEx( AmsAddr*	pAddr,						// Ams address of ADS server           
+							 unsigned long		indexGroup,		//	index group in ADS server interface
+							 unsigned long		indexOffset,	// index offset in ADS server interface
+							 unsigned long		length,			// count of bytes to read
+							 void*				pData,			// pointer to the client buffer
+							 unsigned long*	pcbReturn		// count of bytes read
+							);
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadWriteReq( AmsAddr* pAddr,					// Ams address of ADS server
+							unsigned long indexGroup,			//	index group in ADS server interface
+							unsigned long indexOffset,			// index offset in ADS server interface
+							unsigned long cbReadLength,		// count of bytes to read
+							void* pReadData,						// pointer to the client buffer
+							unsigned long cbWriteLength, 		// count of bytes to write
+							void* pWriteData						// pointer to the client buffer
+							);											
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadWriteReqEx( AmsAddr* pAddr,					// Ams address of ADS server
+							unsigned long indexGroup,			//	index group in ADS server interface
+							unsigned long indexOffset,			// index offset in ADS server interface
+							unsigned long cbReadLength,		// count of bytes to read
+							void* pReadData,						// pointer to the client buffer
+							unsigned long cbWriteLength, 		// count of bytes to write
+							void* pWriteData,						// pointer to the client buffer
+							unsigned long*	pcbReturn			// count of bytes read
+							);											
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadDeviceInfoReq( AmsAddr*		pAddr,	// Ams address of ADS server 
+										 char*			pDevName,// fixed length string (16 Byte)
+										 AdsVersion*	pVersion	// client buffer to store server version
+										 );
+
+__declspec( dllexport )
+long __stdcall AdsSyncWriteControlReq( AmsAddr*		  pAddr,	   // Ams address of ADS server           
+									  unsigned short adsState, // index group in ADS server interface 
+									  unsigned short deviceState,// index offset in ADS server interface
+									  unsigned long length,	// count of bytes to write
+									  void*			  pData		// pointer to the client buffer        
+									  );
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadStateReq( AmsAddr*	pAddr,				// Ams address of ADS server           
+								  unsigned short*	pAdsState,	// pointer to client buffer
+								  unsigned short*	pDeviceState// pointer to the client buffer        
+								  );							          
+
+
+__declspec( dllexport )
+long __stdcall AdsSyncAddDeviceNotificationReq(	AmsAddr*	pAddr, 	// Ams address of ADS server           
+													unsigned long indexGroup, //	index group in ADS server interface
+													unsigned long indexOffset,//	index offset in ADS server interface
+													AdsNotificationAttrib* pNoteAttrib,	// attributes of notification request
+													PAdsNotificationFuncEx pNoteFunc,		// address of notification callback
+													unsigned long hUser,						// user handle
+													unsigned long *pNotification			// pointer to notification handle (return value)
+													);
+__declspec( dllexport )
+long __stdcall AdsSyncDelDeviceNotificationReq(	AmsAddr*	pAddr,// Ams address of ADS server            
+													unsigned long hNotification // notification handle
+													);
+
+__declspec( dllexport )
+long __stdcall AdsSyncSetTimeout(	long nMs ); // Set timeout in ms
+
+
+__declspec( dllexport )
+long __stdcall AdsGetLastError( void ); 
+
+
+/// register callback
+__declspec( dllexport )
+long __stdcall AdsAmsRegisterRouterNotification (PAmsRouterNotificationFuncEx pNoteFunc );
+
+/// unregister callback
+__declspec( dllexport )
+long __stdcall AdsAmsUnRegisterRouterNotification ();
+
+
+__declspec( dllexport )
+long  __stdcall AdsSyncGetTimeout(long *pnMs ); // client buffer to store timeout
+
+__declspec( dllexport )
+long __stdcall AdsAmsPortEnabled(BOOL *pbEnabled);
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// new Ads functions for multithreading applications
+__declspec( dllexport )
+long __stdcall AdsPortOpenEx( ); 
+
+__declspec( dllexport )
+long	__stdcall AdsPortCloseEx( long port );
+
+__declspec( dllexport )
+long	__stdcall AdsGetLocalAddressEx(long port, AmsAddr*	pAddr );
+
+__declspec( dllexport )
+long __stdcall AdsSyncWriteReqEx( long	port,				// Ams port of ADS client
+							 AmsAddr*			pServerAddr,	// Ams address of ADS server
+							 unsigned long		indexGroup,		//	index group in ADS server interface
+							 unsigned long		indexOffset,	// index offset in ADS server interface
+							 unsigned long		length,			// count of bytes to write
+							 void*				pData				// pointer to the client buffer
+							 );
+
+__declspec( dllexport )
+long __stdcall AdsSyncReadReqEx2( long	port,				// Ams port of ADS client
+							 AmsAddr*			pServerAddr,	// Ams address of ADS server
+							 unsigned long		indexGroup,		//	index group in ADS server interface
+							 unsigned long		indexOffset,	// index offset in ADS server interface
+							 unsigned long		length,			// count of bytes to read
+							 void*				pData,				// pointer to the client buffer
+							 unsigned long*	pcbReturn		// count of bytes read
+							);
+
+
+__declspec( dllexport )
+long  __stdcall AdsSyncReadWriteReqEx2( long	port,		// Ams port of ADS client
+							AmsAddr*				pServerAddr,	// Ams address of ADS server
+							unsigned long		indexGroup,		//	index group in ADS server interface
+							unsigned long		indexOffset,	// index offset in ADS server interface
+							unsigned long		cbReadLength,	// count of bytes to read
+							void*					pReadData,		// pointer to the client buffer
+							unsigned long		cbWriteLength, // count of bytes to write
+							void*					pWriteData,		// pointer to the client buffer
+							unsigned long*		pcbReturn		// count of bytes read
+							);											
+
+__declspec( dllexport )
+long  __stdcall AdsSyncReadDeviceInfoReqEx( long port,		// Ams port of ADS client
+										 AmsAddr*		pServerAddr,	// Ams address of ADS server
+										 char*			pDevName,		// fixed length string (16 Byte)
+										 AdsVersion*	pVersion			// client buffer to store server version
+										 );
+
+__declspec( dllexport )
+long  __stdcall AdsSyncWriteControlReqEx( long port,			// Ams port of ADS client
+									  AmsAddr*			pServerAddr,	// Ams address of ADS server
+									  unsigned short	adsState,		// index group in ADS server interface 
+									  unsigned short	deviceState,	// index offset in ADS server interface
+									  unsigned long	length,			// count of bytes to write
+									  void*				pData				// pointer to the client buffer        
+									  );
+
+__declspec( dllexport )
+long  __stdcall AdsSyncReadStateReqEx( long port,			// Ams port of ADS client
+								  AmsAddr*			pServerAddr,	// Ams address of ADS server
+								  unsigned short*	pAdsState,		// pointer to client buffer
+								  unsigned short*	pDeviceState	// pointer to the client buffer        
+								  );							          
+
+
+__declspec( dllexport )
+long  __stdcall AdsSyncAddDeviceNotificationReqEx(	long port,							// Ams port of ADS client
+													AmsAddr*			pServerAddr,					// Ams address of ADS ser
+													unsigned long indexGroup,						//	index group in ADS server interface
+													unsigned long indexOffset,						//	index offset in ADS server interface
+													AdsNotificationAttrib* pNoteAttrib,			// attributes of notification request
+													PAdsNotificationFuncEx pNoteFunc,				// address of notification callback
+													unsigned long hUser,								// user handle
+													unsigned long *pNotification					// pointer to notification handle (return value)
+													);
+__declspec( dllexport )
+long  __stdcall AdsSyncDelDeviceNotificationReqEx( long port,							// Ams port of ADS client
+													AmsAddr*			pServerAddr,					// Ams address of ADS ser
+													unsigned long hNotification					// notification handle
+													);
+
+__declspec( dllexport )
+long  __stdcall AdsSyncSetTimeoutEx(long port, // Ams port of ADS client
+							  long nMs ); // Set timeout in ms
+
+__declspec( dllexport )
+long  __stdcall AdsSyncGetTimeoutEx(long port,   // Ams port of ADS client
+							  long *pnMs ); // client buffer to store timeout
+
+__declspec( dllexport )
+long __stdcall AdsAmsPortEnabledEx(long nPort, BOOL *pbEnabled);
+
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif