diff --git a/c#/dynamixel_functions_csharp/win32/dynamixel.cs b/c#/dynamixel_functions_csharp/win32/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/dynamixel_functions_csharp/win32/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/dynamixel_functions_csharp/win64/dynamixel.cs b/c#/dynamixel_functions_csharp/win64/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/dynamixel_functions_csharp/win64/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/bulk_read/win32/.vs/bulk_read/v14/.suo b/c#/protocol1.0/bulk_read/win32/.vs/bulk_read/v14/.suo new file mode 100644 index 00000000..0413f956 Binary files /dev/null and b/c#/protocol1.0/bulk_read/win32/.vs/bulk_read/v14/.suo differ diff --git a/c#/protocol1.0/bulk_read/win32/bulk_read.sln b/c#/protocol1.0/bulk_read/win32/bulk_read.sln new file mode 100644 index 00000000..6898b7cf --- /dev/null +++ b/c#/protocol1.0/bulk_read/win32/bulk_read.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "bulk_read", "bulk_read\bulk_read.csproj", "{25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|Any CPU.Build.0 = Debug|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|x86.ActiveCfg = Debug|x86 + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|x86.Build.0 = Debug|x86 + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|Any CPU.ActiveCfg = Release|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|Any CPU.Build.0 = Release|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|x86.ActiveCfg = Release|x86 + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/bulk_read/win32/bulk_read/App.config b/c#/protocol1.0/bulk_read/win32/bulk_read/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win32/bulk_read/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/bulk_read/win32/bulk_read/BulkRead.cs b/c#/protocol1.0/bulk_read/win32/bulk_read/BulkRead.cs new file mode 100644 index 00000000..37acd838 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win32/bulk_read/BulkRead.cs @@ -0,0 +1,250 @@ +/* +* bulk_read.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Bulk Read Example ********* +// +// +// Available Dynamixel model on this example : MX or X series set to Protocol 1.0 +// This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000) +// + +using System; +using dynamixel_sdk; + +namespace bulk_read +{ + class BulkRead + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + public const int ADDR_MX_MOVING = 46; + + // Data Byte Length + public const int LEN_MX_GOAL_POSITION = 2; + public const int LEN_MX_PRESENT_POSITION = 2; + public const int LEN_MX_MOVING = 1; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupbulkread Structs + int group_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0; // Present position + byte dxl2_moving = 0; // Dynamixel moving status + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Add parameter storage for Dynamixel#1 present position value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 present moving value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL2_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Bulkread present position and moving status + dynamixel.groupBulkReadTxRxPacket(group_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL1_ID); + return; + } + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL2_ID); + return; + } + + // Get Dynamixel#1 present position value + dxl1_present_position = (UInt16)dynamixel.groupBulkReadGetData(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); + + // Get Dynamixel#2 moving status value + dxl2_moving = (byte)dynamixel.groupBulkReadGetData(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING); + + Console.WriteLine("[ID:{0}] Present Position : {1} [ID:{2}] Is Moving : {3}", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving); + + } while (Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/bulk_read/win32/bulk_read/Properties/AssemblyInfo.cs b/c#/protocol1.0/bulk_read/win32/bulk_read/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..e3b1468a --- /dev/null +++ b/c#/protocol1.0/bulk_read/win32/bulk_read/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("bulk_read")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("bulk_read")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("25a1bbb1-0e8d-4f9f-8e31-ec0c1e61db35")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/bulk_read/win32/bulk_read/bulk_read.csproj b/c#/protocol1.0/bulk_read/win32/bulk_read/bulk_read.csproj new file mode 100644 index 00000000..8126756e --- /dev/null +++ b/c#/protocol1.0/bulk_read/win32/bulk_read/bulk_read.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35} + Exe + Properties + bulk_read + bulk_read + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/bulk_read/win32/bulk_read/dynamixel.cs b/c#/protocol1.0/bulk_read/win32/bulk_read/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win32/bulk_read/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/bulk_read/win64/.vs/bulk_read/v14/.suo b/c#/protocol1.0/bulk_read/win64/.vs/bulk_read/v14/.suo new file mode 100644 index 00000000..78081c92 Binary files /dev/null and b/c#/protocol1.0/bulk_read/win64/.vs/bulk_read/v14/.suo differ diff --git a/c#/protocol1.0/bulk_read/win64/bulk_read.sln b/c#/protocol1.0/bulk_read/win64/bulk_read.sln new file mode 100644 index 00000000..aa1eeb20 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win64/bulk_read.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "bulk_read", "bulk_read\bulk_read.csproj", "{25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|Any CPU.Build.0 = Debug|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|x64.ActiveCfg = Release|x64 + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Debug|x64.Build.0 = Release|x64 + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|Any CPU.ActiveCfg = Release|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|Any CPU.Build.0 = Release|Any CPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|x64.ActiveCfg = Release|x64 + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/bulk_read/win64/bulk_read/App.config b/c#/protocol1.0/bulk_read/win64/bulk_read/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win64/bulk_read/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/bulk_read/win64/bulk_read/BulkRead.cs b/c#/protocol1.0/bulk_read/win64/bulk_read/BulkRead.cs new file mode 100644 index 00000000..37acd838 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win64/bulk_read/BulkRead.cs @@ -0,0 +1,250 @@ +/* +* bulk_read.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Bulk Read Example ********* +// +// +// Available Dynamixel model on this example : MX or X series set to Protocol 1.0 +// This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000) +// + +using System; +using dynamixel_sdk; + +namespace bulk_read +{ + class BulkRead + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + public const int ADDR_MX_MOVING = 46; + + // Data Byte Length + public const int LEN_MX_GOAL_POSITION = 2; + public const int LEN_MX_PRESENT_POSITION = 2; + public const int LEN_MX_MOVING = 1; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupbulkread Structs + int group_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0; // Present position + byte dxl2_moving = 0; // Dynamixel moving status + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Add parameter storage for Dynamixel#1 present position value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 present moving value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL2_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Bulkread present position and moving status + dynamixel.groupBulkReadTxRxPacket(group_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL1_ID); + return; + } + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL2_ID); + return; + } + + // Get Dynamixel#1 present position value + dxl1_present_position = (UInt16)dynamixel.groupBulkReadGetData(group_num, DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION); + + // Get Dynamixel#2 moving status value + dxl2_moving = (byte)dynamixel.groupBulkReadGetData(group_num, DXL2_ID, ADDR_MX_MOVING, LEN_MX_MOVING); + + Console.WriteLine("[ID:{0}] Present Position : {1} [ID:{2}] Is Moving : {3}", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_moving); + + } while (Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/bulk_read/win64/bulk_read/Properties/AssemblyInfo.cs b/c#/protocol1.0/bulk_read/win64/bulk_read/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..e3b1468a --- /dev/null +++ b/c#/protocol1.0/bulk_read/win64/bulk_read/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("bulk_read")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("bulk_read")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("25a1bbb1-0e8d-4f9f-8e31-ec0c1e61db35")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/bulk_read/win64/bulk_read/bulk_read.csproj b/c#/protocol1.0/bulk_read/win64/bulk_read/bulk_read.csproj new file mode 100644 index 00000000..67343260 --- /dev/null +++ b/c#/protocol1.0/bulk_read/win64/bulk_read/bulk_read.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {25A1BBB1-0E8D-4F9F-8E31-EC0C1E61DB35} + Exe + Properties + bulk_read + bulk_read + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/bulk_read/win64/bulk_read/dynamixel.cs b/c#/protocol1.0/bulk_read/win64/bulk_read/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol1.0/bulk_read/win64/bulk_read/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/multi_port/win32/.vs/multi_port/v14/.suo b/c#/protocol1.0/multi_port/win32/.vs/multi_port/v14/.suo new file mode 100644 index 00000000..74e1f960 Binary files /dev/null and b/c#/protocol1.0/multi_port/win32/.vs/multi_port/v14/.suo differ diff --git a/c#/protocol1.0/multi_port/win32/multi_port.sln b/c#/protocol1.0/multi_port/win32/multi_port.sln new file mode 100644 index 00000000..f74039b1 --- /dev/null +++ b/c#/protocol1.0/multi_port/win32/multi_port.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "multi_port", "multi_port\multi_port.csproj", "{E974210C-6AEC-47D6-8359-0601C9725C6F}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x86.ActiveCfg = Debug|x86 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x86.Build.0 = Debug|x86 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.Build.0 = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x86.ActiveCfg = Release|x86 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/multi_port/win32/multi_port/App.config b/c#/protocol1.0/multi_port/win32/multi_port/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/multi_port/win32/multi_port/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/multi_port/win32/multi_port/MultiPort.cs b/c#/protocol1.0/multi_port/win32/multi_port/MultiPort.cs new file mode 100644 index 00000000..3dbee7ee --- /dev/null +++ b/c#/protocol1.0/multi_port/win32/multi_port/MultiPort.cs @@ -0,0 +1,250 @@ +/* +* multi_port.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Multi Port Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using two Dynamixel MX-28, and two USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME1 = "COM8"; // Check which port is being used on your controller + public const string DEVICENAME2 = "COM4"; // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num1 = dynamixel.portHandler(DEVICENAME1); + int port_num2 = dynamixel.portHandler(DEVICENAME2); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port1 + if (dynamixel.openPort(port_num1)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Open port2 + if (dynamixel.openPort(port_num2)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port1 baudrate + if (dynamixel.setBaudRate(port_num1, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port2 baudrate + if (dynamixel.setBaudRate(port_num2, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL2_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write2ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = dynamixel.read2ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = dynamixel.read2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port1 + dynamixel.closePort(port_num1); + + // Close port2 + dynamixel.closePort(port_num2); + + return; + } + } +} diff --git a/c#/protocol1.0/multi_port/win32/multi_port/Properties/AssemblyInfo.cs b/c#/protocol1.0/multi_port/win32/multi_port/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..47770db0 --- /dev/null +++ b/c#/protocol1.0/multi_port/win32/multi_port/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("multi_port")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("multi_port")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("e974210c-6aec-47d6-8359-0601c9725c6f")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/multi_port/win32/multi_port/dynamixel.cs b/c#/protocol1.0/multi_port/win32/multi_port/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol1.0/multi_port/win32/multi_port/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/multi_port/win32/multi_port/multi_port.csproj b/c#/protocol1.0/multi_port/win32/multi_port/multi_port.csproj new file mode 100644 index 00000000..132f67ce --- /dev/null +++ b/c#/protocol1.0/multi_port/win32/multi_port/multi_port.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {E974210C-6AEC-47D6-8359-0601C9725C6F} + Exe + Properties + multi_port + multi_port + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/multi_port/win64/.vs/multi_port/v14/.suo b/c#/protocol1.0/multi_port/win64/.vs/multi_port/v14/.suo new file mode 100644 index 00000000..7e8068ce Binary files /dev/null and b/c#/protocol1.0/multi_port/win64/.vs/multi_port/v14/.suo differ diff --git a/c#/protocol1.0/multi_port/win64/multi_port.sln b/c#/protocol1.0/multi_port/win64/multi_port.sln new file mode 100644 index 00000000..f11ecf68 --- /dev/null +++ b/c#/protocol1.0/multi_port/win64/multi_port.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "multi_port", "multi_port\multi_port.csproj", "{E974210C-6AEC-47D6-8359-0601C9725C6F}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x64.ActiveCfg = Release|x64 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x64.Build.0 = Release|x64 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.Build.0 = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x64.ActiveCfg = Release|x64 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/multi_port/win64/multi_port/App.config b/c#/protocol1.0/multi_port/win64/multi_port/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/multi_port/win64/multi_port/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/multi_port/win64/multi_port/MultiPort.cs b/c#/protocol1.0/multi_port/win64/multi_port/MultiPort.cs new file mode 100644 index 00000000..3dbee7ee --- /dev/null +++ b/c#/protocol1.0/multi_port/win64/multi_port/MultiPort.cs @@ -0,0 +1,250 @@ +/* +* multi_port.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Multi Port Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using two Dynamixel MX-28, and two USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME1 = "COM8"; // Check which port is being used on your controller + public const string DEVICENAME2 = "COM4"; // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num1 = dynamixel.portHandler(DEVICENAME1); + int port_num2 = dynamixel.portHandler(DEVICENAME2); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port1 + if (dynamixel.openPort(port_num1)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Open port2 + if (dynamixel.openPort(port_num2)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port1 baudrate + if (dynamixel.setBaudRate(port_num1, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port2 baudrate + if (dynamixel.setBaudRate(port_num2, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL2_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write2ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = dynamixel.read2ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = dynamixel.read2ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port1 + dynamixel.closePort(port_num1); + + // Close port2 + dynamixel.closePort(port_num2); + + return; + } + } +} diff --git a/c#/protocol1.0/multi_port/win64/multi_port/Properties/AssemblyInfo.cs b/c#/protocol1.0/multi_port/win64/multi_port/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..47770db0 --- /dev/null +++ b/c#/protocol1.0/multi_port/win64/multi_port/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("multi_port")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("multi_port")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("e974210c-6aec-47d6-8359-0601c9725c6f")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/multi_port/win64/multi_port/dynamixel.cs b/c#/protocol1.0/multi_port/win64/multi_port/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol1.0/multi_port/win64/multi_port/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/multi_port/win64/multi_port/multi_port.csproj b/c#/protocol1.0/multi_port/win64/multi_port/multi_port.csproj new file mode 100644 index 00000000..c3730f9d --- /dev/null +++ b/c#/protocol1.0/multi_port/win64/multi_port/multi_port.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {E974210C-6AEC-47D6-8359-0601C9725C6F} + Exe + Properties + multi_port + multi_port + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/ping/win32/.vs/ping/v14/.suo b/c#/protocol1.0/ping/win32/.vs/ping/v14/.suo new file mode 100644 index 00000000..4396c67d Binary files /dev/null and b/c#/protocol1.0/ping/win32/.vs/ping/v14/.suo differ diff --git a/c#/protocol1.0/ping/win32/ping.sln b/c#/protocol1.0/ping/win32/ping.sln new file mode 100644 index 00000000..bd3e7438 --- /dev/null +++ b/c#/protocol1.0/ping/win32/ping.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ping", "ping\ping.csproj", "{4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.Build.0 = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x86.ActiveCfg = Debug|x86 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x86.Build.0 = Debug|x86 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.ActiveCfg = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.Build.0 = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x86.ActiveCfg = Release|x86 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/ping/win32/ping/App.config b/c#/protocol1.0/ping/win32/ping/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/ping/win32/ping/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/ping/win32/ping/Ping.cs b/c#/protocol1.0/ping/win32/ping/Ping.cs new file mode 100644 index 00000000..450615d1 --- /dev/null +++ b/c#/protocol1.0/ping/win32/ping/Ping.cs @@ -0,0 +1,99 @@ +/* +* ping.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* ping Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace ping +{ + class Ping + { + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl_model_number; // Dynamixel model number + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + // Try to ping the Dynamixel + // Get Dynamixel model number + dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] ping Succeeded. Dynamixel model number : {1}", DXL_ID, dxl_model_number); + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/ping/win32/ping/Properties/AssemblyInfo.cs b/c#/protocol1.0/ping/win32/ping/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..a97376c3 --- /dev/null +++ b/c#/protocol1.0/ping/win32/ping/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("ping")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("ping")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("4d6dfb5c-0e72-47b4-9d9a-ac58f5dd3abd")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/ping/win32/ping/dynamixel.cs b/c#/protocol1.0/ping/win32/ping/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol1.0/ping/win32/ping/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/ping/win32/ping/ping.csproj b/c#/protocol1.0/ping/win32/ping/ping.csproj new file mode 100644 index 00000000..6591c52b --- /dev/null +++ b/c#/protocol1.0/ping/win32/ping/ping.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD} + Exe + Properties + ping + ping + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/ping/win64/.vs/ping/v14/.suo b/c#/protocol1.0/ping/win64/.vs/ping/v14/.suo new file mode 100644 index 00000000..02241436 Binary files /dev/null and b/c#/protocol1.0/ping/win64/.vs/ping/v14/.suo differ diff --git a/c#/protocol1.0/ping/win64/ping.sln b/c#/protocol1.0/ping/win64/ping.sln new file mode 100644 index 00000000..8e2c6b75 --- /dev/null +++ b/c#/protocol1.0/ping/win64/ping.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ping", "ping\ping.csproj", "{4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.Build.0 = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x64.ActiveCfg = Release|x64 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x64.Build.0 = Release|x64 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.ActiveCfg = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.Build.0 = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x64.ActiveCfg = Release|x64 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/ping/win64/ping/App.config b/c#/protocol1.0/ping/win64/ping/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/ping/win64/ping/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/ping/win64/ping/Ping.cs b/c#/protocol1.0/ping/win64/ping/Ping.cs new file mode 100644 index 00000000..450615d1 --- /dev/null +++ b/c#/protocol1.0/ping/win64/ping/Ping.cs @@ -0,0 +1,99 @@ +/* +* ping.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* ping Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace ping +{ + class Ping + { + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl_model_number; // Dynamixel model number + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + // Try to ping the Dynamixel + // Get Dynamixel model number + dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] ping Succeeded. Dynamixel model number : {1}", DXL_ID, dxl_model_number); + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/ping/win64/ping/Properties/AssemblyInfo.cs b/c#/protocol1.0/ping/win64/ping/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..a97376c3 --- /dev/null +++ b/c#/protocol1.0/ping/win64/ping/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("ping")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("ping")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("4d6dfb5c-0e72-47b4-9d9a-ac58f5dd3abd")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/ping/win64/ping/dynamixel.cs b/c#/protocol1.0/ping/win64/ping/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol1.0/ping/win64/ping/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/ping/win64/ping/ping.csproj b/c#/protocol1.0/ping/win64/ping/ping.csproj new file mode 100644 index 00000000..36fea6a1 --- /dev/null +++ b/c#/protocol1.0/ping/win64/ping/ping.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD} + Exe + Properties + ping + ping + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/read_write/win32/.vs/read_write/v14/.suo b/c#/protocol1.0/read_write/win32/.vs/read_write/v14/.suo new file mode 100644 index 00000000..1496b139 Binary files /dev/null and b/c#/protocol1.0/read_write/win32/.vs/read_write/v14/.suo differ diff --git a/c#/protocol1.0/read_write/win32/read_write.sln b/c#/protocol1.0/read_write/win32/read_write.sln new file mode 100644 index 00000000..bda9dede --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "read_write", "read_write\read_write.csproj", "{D1B60768-6B6B-492D-8E0A-D258D9B2CA27}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x86.ActiveCfg = Debug|x86 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x86.Build.0 = Debug|x86 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.Build.0 = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x86.ActiveCfg = Release|x86 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/read_write/win32/read_write/App.config b/c#/protocol1.0/read_write/win32/read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/read_write/win32/read_write/Properties/AssemblyInfo.cs b/c#/protocol1.0/read_write/win32/read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..94be0145 --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("d1b60768-6b6b-492d-8e0a-d258d9b2ca27")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/read_write/win32/read_write/ReadWrite.cs b/c#/protocol1.0/read_write/win32/read_write/ReadWrite.cs new file mode 100644 index 00000000..185f9843 --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write/ReadWrite.cs @@ -0,0 +1,170 @@ +/* +* read_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Read and Write Example ********* +// +// +// Available DXL model on this example : All models using Protocol 1.0 +// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable DXL Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected "); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read present position + dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2}", DXL_ID, dxl_goal_position[index], dxl_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/read_write/win32/read_write/dynamixel.cs b/c#/protocol1.0/read_write/win32/read_write/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/read_write/win32/read_write/read_write.csproj b/c#/protocol1.0/read_write/win32/read_write/read_write.csproj new file mode 100644 index 00000000..131b619a --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write/read_write.csproj @@ -0,0 +1,108 @@ + + + + + Debug + AnyCPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27} + Exe + Properties + read_write + read_write + v4.5 + 512 + false + 게시\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + 0 + + + + + + + + + + + + + + + + + + + + + + False + Microsoft .NET Framework 4.5%28x86 및 x64%29 + true + + + False + .NET Framework 3.5 SP1 + false + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/read_write/win32/read_write/read_write.csproj.user b/c#/protocol1.0/read_write/win32/read_write/read_write.csproj.user new file mode 100644 index 00000000..04baf14c --- /dev/null +++ b/c#/protocol1.0/read_write/win32/read_write/read_write.csproj.user @@ -0,0 +1,17 @@ + + + + 게시\ + + + + + + ko-KR + false + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/read_write/win64/.vs/read_write/v14/.suo b/c#/protocol1.0/read_write/win64/.vs/read_write/v14/.suo new file mode 100644 index 00000000..dfbfac89 Binary files /dev/null and b/c#/protocol1.0/read_write/win64/.vs/read_write/v14/.suo differ diff --git a/c#/protocol1.0/read_write/win64/read_write.sln b/c#/protocol1.0/read_write/win64/read_write.sln new file mode 100644 index 00000000..6c6b9cde --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "read_write", "read_write\read_write.csproj", "{D1B60768-6B6B-492D-8E0A-D258D9B2CA27}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x64.ActiveCfg = Debug|x64 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x64.Build.0 = Debug|x64 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.Build.0 = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x64.ActiveCfg = Release|x64 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/read_write/win64/read_write/App.config b/c#/protocol1.0/read_write/win64/read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/read_write/win64/read_write/Properties/AssemblyInfo.cs b/c#/protocol1.0/read_write/win64/read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..94be0145 --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("d1b60768-6b6b-492d-8e0a-d258d9b2ca27")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/read_write/win64/read_write/ReadWrite.cs b/c#/protocol1.0/read_write/win64/read_write/ReadWrite.cs new file mode 100644 index 00000000..185f9843 --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write/ReadWrite.cs @@ -0,0 +1,170 @@ +/* +* read_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Read and Write Example ********* +// +// +// Available DXL model on this example : All models using Protocol 1.0 +// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable DXL Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected "); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read present position + dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2}", DXL_ID, dxl_goal_position[index], dxl_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/read_write/win64/read_write/dynamixel.cs b/c#/protocol1.0/read_write/win64/read_write/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/read_write/win64/read_write/read_write.csproj b/c#/protocol1.0/read_write/win64/read_write/read_write.csproj new file mode 100644 index 00000000..2104f52b --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write/read_write.csproj @@ -0,0 +1,107 @@ + + + + + Debug + AnyCPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27} + Exe + Properties + read_write + read_write + v4.5 + 512 + false + 게시\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + False + Microsoft .NET Framework 4.5%28x86 및 x64%29 + true + + + False + .NET Framework 3.5 SP1 + false + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/read_write/win64/read_write/read_write.csproj.user b/c#/protocol1.0/read_write/win64/read_write/read_write.csproj.user new file mode 100644 index 00000000..04baf14c --- /dev/null +++ b/c#/protocol1.0/read_write/win64/read_write/read_write.csproj.user @@ -0,0 +1,17 @@ + + + + 게시\ + + + + + + ko-KR + false + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/reset/win32/.vs/reset/v14/.suo b/c#/protocol1.0/reset/win32/.vs/reset/v14/.suo new file mode 100644 index 00000000..c658c113 Binary files /dev/null and b/c#/protocol1.0/reset/win32/.vs/reset/v14/.suo differ diff --git a/c#/protocol1.0/reset/win32/reset.sln b/c#/protocol1.0/reset/win32/reset.sln new file mode 100644 index 00000000..488fdcc9 --- /dev/null +++ b/c#/protocol1.0/reset/win32/reset.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "reset", "reset\reset.csproj", "{9E063190-2F38-4EC6-92B8-9106B4A076DA}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|Any CPU.Build.0 = Debug|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|x86.ActiveCfg = Debug|x86 + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|x86.Build.0 = Debug|x86 + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|Any CPU.ActiveCfg = Release|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|Any CPU.Build.0 = Release|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|x86.ActiveCfg = Release|x86 + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/reset/win32/reset/App.config b/c#/protocol1.0/reset/win32/reset/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/reset/win32/reset/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/reset/win32/reset/Properties/AssemblyInfo.cs b/c#/protocol1.0/reset/win32/reset/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..78520e53 --- /dev/null +++ b/c#/protocol1.0/reset/win32/reset/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("reset")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("reset")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("9e063190-2f38-4ec6-92b8-9106b4a076da")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/reset/win32/reset/Reset.cs b/c#/protocol1.0/reset/win32/reset/Reset.cs new file mode 100644 index 00000000..f1859dd7 --- /dev/null +++ b/c#/protocol1.0/reset/win32/reset/Reset.cs @@ -0,0 +1,193 @@ +/* +* reset.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Factory Reset Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +// Be aware that: +// This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +// + +using System; +using System.Threading; +using dynamixel_sdk; + +namespace reset +{ + class Reset + { + // Control table address + public const int ADDR_MX_BAUDRATE = 4; // Control table address is different in Dynamixel model + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int FACTORYRST_DEFAULTBAUDRATE = 57600; // Dynamixel baudrate set by factoryreset + public const int NEW_BAUDNUM = 1; // New baudnum to recover Dynamixel baudrate as it was + public const byte OPERATION_MODE = 0x00; // Mode is unavailable in Protocol 1.0 Reset + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + byte dxl_baudnum_read; // Read baudnum + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + + // Read present baudrate of the controller + Console.WriteLine("Now the controller baudrate is : {0}", dynamixel.getBaudRate(port_num)); + + // Try factoryreset + Console.WriteLine("[ID:{0}] Try factoryreset : ", DXL_ID); + dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine("Aborted"); + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + return; + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Wait for reset + Console.WriteLine("Wait for reset..."); + Thread.Sleep(2000); + + Console.WriteLine("[ID:{0}] factoryReset Success!", DXL_ID); + + // Set controller baudrate to dxl default baudrate + if (dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", FACTORYRST_DEFAULTBAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Write new baudnum + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Set Dynamixel baudnum to : {1}", DXL_ID, NEW_BAUDNUM); + } + + // Set port baudrate to BAUDRATE + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", BAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + Thread.Sleep(200); + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel Baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/reset/win32/reset/dynamixel.cs b/c#/protocol1.0/reset/win32/reset/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol1.0/reset/win32/reset/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/reset/win32/reset/reset.csproj b/c#/protocol1.0/reset/win32/reset/reset.csproj new file mode 100644 index 00000000..1fb98459 --- /dev/null +++ b/c#/protocol1.0/reset/win32/reset/reset.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA} + Exe + Properties + reset + reset + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/reset/win64/.vs/reset/v14/.suo b/c#/protocol1.0/reset/win64/.vs/reset/v14/.suo new file mode 100644 index 00000000..07f24377 Binary files /dev/null and b/c#/protocol1.0/reset/win64/.vs/reset/v14/.suo differ diff --git a/c#/protocol1.0/reset/win64/reset.sln b/c#/protocol1.0/reset/win64/reset.sln new file mode 100644 index 00000000..33def91a --- /dev/null +++ b/c#/protocol1.0/reset/win64/reset.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "reset", "reset\reset.csproj", "{9E063190-2F38-4EC6-92B8-9106B4A076DA}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|Any CPU.Build.0 = Debug|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|x64.ActiveCfg = Debug|x64 + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Debug|x64.Build.0 = Debug|x64 + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|Any CPU.ActiveCfg = Release|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|Any CPU.Build.0 = Release|Any CPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|x64.ActiveCfg = Release|x64 + {9E063190-2F38-4EC6-92B8-9106B4A076DA}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/reset/win64/reset/App.config b/c#/protocol1.0/reset/win64/reset/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/reset/win64/reset/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/reset/win64/reset/Properties/AssemblyInfo.cs b/c#/protocol1.0/reset/win64/reset/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..78520e53 --- /dev/null +++ b/c#/protocol1.0/reset/win64/reset/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("reset")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("reset")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("9e063190-2f38-4ec6-92b8-9106b4a076da")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/reset/win64/reset/Reset.cs b/c#/protocol1.0/reset/win64/reset/Reset.cs new file mode 100644 index 00000000..f1859dd7 --- /dev/null +++ b/c#/protocol1.0/reset/win64/reset/Reset.cs @@ -0,0 +1,193 @@ +/* +* reset.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Factory Reset Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using a Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +// Be aware that: +// This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 34 (Baudrate : 57600) +// + +using System; +using System.Threading; +using dynamixel_sdk; + +namespace reset +{ + class Reset + { + // Control table address + public const int ADDR_MX_BAUDRATE = 4; // Control table address is different in Dynamixel model + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int FACTORYRST_DEFAULTBAUDRATE = 57600; // Dynamixel baudrate set by factoryreset + public const int NEW_BAUDNUM = 1; // New baudnum to recover Dynamixel baudrate as it was + public const byte OPERATION_MODE = 0x00; // Mode is unavailable in Protocol 1.0 Reset + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + byte dxl_baudnum_read; // Read baudnum + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + + // Read present baudrate of the controller + Console.WriteLine("Now the controller baudrate is : {0}", dynamixel.getBaudRate(port_num)); + + // Try factoryreset + Console.WriteLine("[ID:{0}] Try factoryreset : ", DXL_ID); + dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine("Aborted"); + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + return; + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Wait for reset + Console.WriteLine("Wait for reset..."); + Thread.Sleep(2000); + + Console.WriteLine("[ID:{0}] factoryReset Success!", DXL_ID); + + // Set controller baudrate to dxl default baudrate + if (dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", FACTORYRST_DEFAULTBAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Write new baudnum + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Set Dynamixel baudnum to : {1}", DXL_ID, NEW_BAUDNUM); + } + + // Set port baudrate to BAUDRATE + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", BAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + Thread.Sleep(200); + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel Baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/reset/win64/reset/dynamixel.cs b/c#/protocol1.0/reset/win64/reset/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol1.0/reset/win64/reset/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/reset/win64/reset/reset.csproj b/c#/protocol1.0/reset/win64/reset/reset.csproj new file mode 100644 index 00000000..e91b53a4 --- /dev/null +++ b/c#/protocol1.0/reset/win64/reset/reset.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {9E063190-2F38-4EC6-92B8-9106B4A076DA} + Exe + Properties + reset + reset + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/sync_write/win32/.vs/sync_write/v14/.suo b/c#/protocol1.0/sync_write/win32/.vs/sync_write/v14/.suo new file mode 100644 index 00000000..e597a82c Binary files /dev/null and b/c#/protocol1.0/sync_write/win32/.vs/sync_write/v14/.suo differ diff --git a/c#/protocol1.0/sync_write/win32/sync_write.sln b/c#/protocol1.0/sync_write/win32/sync_write.sln new file mode 100644 index 00000000..93f94418 --- /dev/null +++ b/c#/protocol1.0/sync_write/win32/sync_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "sync_write", "sync_write\sync_write.csproj", "{23EAE323-3535-448B-BC18-2A516BB7759C}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|x86.ActiveCfg = Debug|x86 + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|x86.Build.0 = Debug|x86 + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|Any CPU.Build.0 = Release|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|x86.ActiveCfg = Release|x86 + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/sync_write/win32/sync_write/App.config b/c#/protocol1.0/sync_write/win32/sync_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/sync_write/win32/sync_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/sync_write/win32/sync_write/Properties/AssemblyInfo.cs b/c#/protocol1.0/sync_write/win32/sync_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..061150fb --- /dev/null +++ b/c#/protocol1.0/sync_write/win32/sync_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("sync_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("sync_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("23eae323-3535-448b-bc18-2a516bb7759c")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/sync_write/win32/sync_write/SyncWrite.cs b/c#/protocol1.0/sync_write/win32/sync_write/SyncWrite.cs new file mode 100644 index 00000000..40f9aab4 --- /dev/null +++ b/c#/protocol1.0/sync_write/win32/sync_write/SyncWrite.cs @@ -0,0 +1,230 @@ +/* +* sync_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Sync Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace sync_write +{ + class SyncWrite + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Data Byte Length + public const int LEN_MX_GOAL_POSITION = 2; + public const int LEN_MX_PRESENT_POSITION = 2; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupsyncwrite instance + int group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add Dynamixel#1 goal position value to the Syncwrite storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL1_ID); + return; + } + + // Add Dynamixel#2 goal position value to the Syncwrite parameter storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL2_ID); + return; + } + + // Syncwrite goal position + dynamixel.groupSyncWriteTxPacket(group_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear syncwrite parameter storage + dynamixel.groupSyncWriteClearParam(group_num); + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/sync_write/win32/sync_write/dynamixel.cs b/c#/protocol1.0/sync_write/win32/sync_write/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol1.0/sync_write/win32/sync_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/sync_write/win32/sync_write/sync_write.csproj b/c#/protocol1.0/sync_write/win32/sync_write/sync_write.csproj new file mode 100644 index 00000000..8af481f2 --- /dev/null +++ b/c#/protocol1.0/sync_write/win32/sync_write/sync_write.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {23EAE323-3535-448B-BC18-2A516BB7759C} + Exe + Properties + sync_write + sync_write + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/sync_write/win64/.vs/sync_write/v14/.suo b/c#/protocol1.0/sync_write/win64/.vs/sync_write/v14/.suo new file mode 100644 index 00000000..390ebea9 Binary files /dev/null and b/c#/protocol1.0/sync_write/win64/.vs/sync_write/v14/.suo differ diff --git a/c#/protocol1.0/sync_write/win64/sync_write.sln b/c#/protocol1.0/sync_write/win64/sync_write.sln new file mode 100644 index 00000000..83f8befc --- /dev/null +++ b/c#/protocol1.0/sync_write/win64/sync_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "sync_write", "sync_write\sync_write.csproj", "{23EAE323-3535-448B-BC18-2A516BB7759C}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|Any CPU.Build.0 = Debug|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|x64.ActiveCfg = Debug|x64 + {23EAE323-3535-448B-BC18-2A516BB7759C}.Debug|x64.Build.0 = Debug|x64 + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|Any CPU.ActiveCfg = Release|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|Any CPU.Build.0 = Release|Any CPU + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|x64.ActiveCfg = Release|x64 + {23EAE323-3535-448B-BC18-2A516BB7759C}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol1.0/sync_write/win64/sync_write/App.config b/c#/protocol1.0/sync_write/win64/sync_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol1.0/sync_write/win64/sync_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol1.0/sync_write/win64/sync_write/Properties/AssemblyInfo.cs b/c#/protocol1.0/sync_write/win64/sync_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..061150fb --- /dev/null +++ b/c#/protocol1.0/sync_write/win64/sync_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("sync_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("sync_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("23eae323-3535-448b-bc18-2a516bb7759c")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol1.0/sync_write/win64/sync_write/SyncWrite.cs b/c#/protocol1.0/sync_write/win64/sync_write/SyncWrite.cs new file mode 100644 index 00000000..40f9aab4 --- /dev/null +++ b/c#/protocol1.0/sync_write/win64/sync_write/SyncWrite.cs @@ -0,0 +1,230 @@ +/* +* sync_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Sync Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 +// This example is designed for using two Dynamixel MX-28, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel MX properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace sync_write +{ + class SyncWrite + { + // Control table address + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Data Byte Length + public const int LEN_MX_GOAL_POSITION = 2; + public const int LEN_MX_PRESENT_POSITION = 2; + + // Protocol version + public const int PROTOCOL_VERSION = 1; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 10; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupsyncwrite instance + int group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + UInt16[] dxl_goal_position = new UInt16[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add Dynamixel#1 goal position value to the Syncwrite storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL1_ID); + return; + } + + // Add Dynamixel#2 goal position value to the Syncwrite parameter storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL2_ID); + return; + } + + // Syncwrite goal position + dynamixel.groupSyncWriteTxPacket(group_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear syncwrite parameter storage + dynamixel.groupSyncWriteClearParam(group_num); + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol1.0/sync_write/win64/sync_write/dynamixel.cs b/c#/protocol1.0/sync_write/win64/sync_write/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol1.0/sync_write/win64/sync_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol1.0/sync_write/win64/sync_write/sync_write.csproj b/c#/protocol1.0/sync_write/win64/sync_write/sync_write.csproj new file mode 100644 index 00000000..5da748f7 --- /dev/null +++ b/c#/protocol1.0/sync_write/win64/sync_write/sync_write.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {23EAE323-3535-448B-BC18-2A516BB7759C} + Exe + Properties + sync_write + sync_write + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/broadcast_ping/win32/.vs/broadcast_ping/v14/.suo b/c#/protocol2.0/broadcast_ping/win32/.vs/broadcast_ping/v14/.suo new file mode 100644 index 00000000..9612045a Binary files /dev/null and b/c#/protocol2.0/broadcast_ping/win32/.vs/broadcast_ping/v14/.suo differ diff --git a/c#/protocol2.0/broadcast_ping/win32/broadcast_ping.sln b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping.sln new file mode 100644 index 00000000..133769ca --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "broadcast_ping", "broadcast_ping\broadcast_ping.csproj", "{AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|x86.ActiveCfg = Release|x86 + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|x86.Build.0 = Release|x86 + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|Any CPU.Build.0 = Release|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|x86.ActiveCfg = Release|x86 + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/App.config b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/BroadcastPing.cs b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/BroadcastPing.cs new file mode 100644 index 00000000..0882c447 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/BroadcastPing.cs @@ -0,0 +1,95 @@ +/* +* broadcast_ping.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* broadcastPing Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace broadcast_ping +{ + class BroadcastPing + { + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int MAX_ID = 252; // Maximum ID value + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + int id; + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Try to broadcast ping the Dynamixel + dynamixel.broadcastPing(port_num, PROTOCOL_VERSION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + Console.WriteLine("Detected Dynamixel :"); + for (id = 0; id < MAX_ID; id++) + { + if (dynamixel.getBroadcastPingResult(port_num, PROTOCOL_VERSION, id)) + Console.WriteLine("[ID:{0}]", id); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/Properties/AssemblyInfo.cs b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..0dc882b9 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("broadcast_ping")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("broadcast_ping")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("af5022f2-06a2-41d5-a04f-128c8c1e0a7f")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/broadcast_ping.csproj b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/broadcast_ping.csproj new file mode 100644 index 00000000..c5295ef8 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/broadcast_ping.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F} + Exe + Properties + broadcast_ping + broadcast_ping + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/dynamixel.cs b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win32/broadcast_ping/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/broadcast_ping/win64/.vs/broadcast_ping/v14/.suo b/c#/protocol2.0/broadcast_ping/win64/.vs/broadcast_ping/v14/.suo new file mode 100644 index 00000000..7b0ca422 Binary files /dev/null and b/c#/protocol2.0/broadcast_ping/win64/.vs/broadcast_ping/v14/.suo differ diff --git a/c#/protocol2.0/broadcast_ping/win64/broadcast_ping.sln b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping.sln new file mode 100644 index 00000000..26806165 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "broadcast_ping", "broadcast_ping\broadcast_ping.csproj", "{AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|x64.ActiveCfg = Release|x64 + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Debug|x64.Build.0 = Release|x64 + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|Any CPU.Build.0 = Release|Any CPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|x64.ActiveCfg = Release|x64 + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/App.config b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/BroadcastPing.cs b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/BroadcastPing.cs new file mode 100644 index 00000000..0882c447 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/BroadcastPing.cs @@ -0,0 +1,95 @@ +/* +* broadcast_ping.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* broadcastPing Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace broadcast_ping +{ + class BroadcastPing + { + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int MAX_ID = 252; // Maximum ID value + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + int id; + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Try to broadcast ping the Dynamixel + dynamixel.broadcastPing(port_num, PROTOCOL_VERSION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + Console.WriteLine("Detected Dynamixel :"); + for (id = 0; id < MAX_ID; id++) + { + if (dynamixel.getBroadcastPingResult(port_num, PROTOCOL_VERSION, id)) + Console.WriteLine("[ID:{0}]", id); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/Properties/AssemblyInfo.cs b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..0dc882b9 --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("broadcast_ping")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("broadcast_ping")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("af5022f2-06a2-41d5-a04f-128c8c1e0a7f")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/broadcast_ping.csproj b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/broadcast_ping.csproj new file mode 100644 index 00000000..9dfdbc7a --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/broadcast_ping.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {AF5022F2-06A2-41D5-A04F-128C8C1E0A7F} + Exe + Properties + broadcast_ping + broadcast_ping + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/dynamixel.cs b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/broadcast_ping/win64/broadcast_ping/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/bulk_read_write/win32/.vs/bulk_read_write/v14/.suo b/c#/protocol2.0/bulk_read_write/win32/.vs/bulk_read_write/v14/.suo new file mode 100644 index 00000000..50d3b863 Binary files /dev/null and b/c#/protocol2.0/bulk_read_write/win32/.vs/bulk_read_write/v14/.suo differ diff --git a/c#/protocol2.0/bulk_read_write/win32/bulk_read_write.sln b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write.sln new file mode 100644 index 00000000..71a1798f --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "bulk_read_write", "bulk_read_write\bulk_read_write.csproj", "{AE76B9AC-97EB-40B1-8161-576C3B15CCB8}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|Any CPU.Build.0 = Debug|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|x86.ActiveCfg = Debug|x86 + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|x86.Build.0 = Debug|x86 + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|Any CPU.ActiveCfg = Release|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|Any CPU.Build.0 = Release|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|x86.ActiveCfg = Release|x86 + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/App.config b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/BulkReadWrite.cs b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/BulkReadWrite.cs new file mode 100644 index 00000000..8562657e --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/BulkReadWrite.cs @@ -0,0 +1,257 @@ +/* +* bulk_read_write.c +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Bulk Read and Bulk Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using two Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace bulk_read_write +{ + class BulkReadWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_LED_RED = 563; + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + + // Data Byte Length + public const int LEN_PRO_LED_RED = 1; + public const int LEN_PRO_GOAL_POSITION = 4; + public const int LEN_PRO_PRESENT_POSITION = 4; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize GroupBulkWrite Struct + int groupwrite_num = dynamixel.groupBulkWrite(port_num, PROTOCOL_VERSION); + + // Initialize Groupbulkread Structs + int groupread_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + byte[] dxl_led_value = new byte[2]{ 0x00, 0xFF }; // Dynamixel LED value for write + Int32 dxl1_present_position = 0; // Present position + byte dxl2_led_value_read = 0; // Dynamixel moving status + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Add parameter storage for Dynamixel#1 present position value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 present moving value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL2_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add parameter storage for Dynamixel#1 goal position + dxl_addparam_result = dynamixel.groupBulkWriteAddParam(groupwrite_num, DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkWrite addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 LED value + dxl_addparam_result = dynamixel.groupBulkWriteAddParam(groupwrite_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, dxl_led_value[index], LEN_PRO_LED_RED); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkWrite addparam failed", DXL2_ID); + return; + } + + // Bulkwrite goal position and LED value + dynamixel.groupBulkWriteTxPacket(groupwrite_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear bulkwrite parameter storage + dynamixel.groupBulkWriteClearParam(groupwrite_num); + + do + { + // Bulkread present position and moving status + dynamixel.groupBulkReadTxRxPacket(groupread_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL1_ID); + return; + } + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL2_ID); + return; + } + + // Get Dynamixel#1 present position value + dxl1_present_position = (Int32)dynamixel.groupBulkReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + // Get Dynamixel#2 moving status value + dxl2_led_value_read = (byte)dynamixel.groupBulkReadGetData(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED); + + Console.WriteLine("[ID:{0}] Present Position : {1} [ID:{2}] LED Value : {3}", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read); + + } while (Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/Properties/AssemblyInfo.cs b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..fbef6b56 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("bulk_read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("bulk_read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("ae76b9ac-97eb-40b1-8161-576c3b15ccb8")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/bulk_read_write.csproj b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/bulk_read_write.csproj new file mode 100644 index 00000000..fdd87e13 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/bulk_read_write.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8} + Exe + Properties + bulk_read_write + bulk_read_write + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/dynamixel.cs b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win32/bulk_read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/bulk_read_write/win64/.vs/bulk_read_write/v14/.suo b/c#/protocol2.0/bulk_read_write/win64/.vs/bulk_read_write/v14/.suo new file mode 100644 index 00000000..fa096dca Binary files /dev/null and b/c#/protocol2.0/bulk_read_write/win64/.vs/bulk_read_write/v14/.suo differ diff --git a/c#/protocol2.0/bulk_read_write/win64/bulk_read_write.sln b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write.sln new file mode 100644 index 00000000..178ce4e7 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "bulk_read_write", "bulk_read_write\bulk_read_write.csproj", "{AE76B9AC-97EB-40B1-8161-576C3B15CCB8}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|Any CPU.Build.0 = Debug|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|x64.ActiveCfg = Debug|x64 + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Debug|x64.Build.0 = Debug|x64 + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|Any CPU.ActiveCfg = Release|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|Any CPU.Build.0 = Release|Any CPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|x64.ActiveCfg = Release|x64 + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/App.config b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/BulkReadWrite.cs b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/BulkReadWrite.cs new file mode 100644 index 00000000..8562657e --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/BulkReadWrite.cs @@ -0,0 +1,257 @@ +/* +* bulk_read_write.c +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Bulk Read and Bulk Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using two Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace bulk_read_write +{ + class BulkReadWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_LED_RED = 563; + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + + // Data Byte Length + public const int LEN_PRO_LED_RED = 1; + public const int LEN_PRO_GOAL_POSITION = 4; + public const int LEN_PRO_PRESENT_POSITION = 4; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize GroupBulkWrite Struct + int groupwrite_num = dynamixel.groupBulkWrite(port_num, PROTOCOL_VERSION); + + // Initialize Groupbulkread Structs + int groupread_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + byte[] dxl_led_value = new byte[2]{ 0x00, 0xFF }; // Dynamixel LED value for write + Int32 dxl1_present_position = 0; // Present position + byte dxl2_led_value_read = 0; // Dynamixel moving status + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Add parameter storage for Dynamixel#1 present position value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 present moving value + dxl_addparam_result = dynamixel.groupBulkReadAddParam(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead addparam failed", DXL2_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add parameter storage for Dynamixel#1 goal position + dxl_addparam_result = dynamixel.groupBulkWriteAddParam(groupwrite_num, DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkWrite addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 LED value + dxl_addparam_result = dynamixel.groupBulkWriteAddParam(groupwrite_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, dxl_led_value[index], LEN_PRO_LED_RED); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkWrite addparam failed", DXL2_ID); + return; + } + + // Bulkwrite goal position and LED value + dynamixel.groupBulkWriteTxPacket(groupwrite_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear bulkwrite parameter storage + dynamixel.groupBulkWriteClearParam(groupwrite_num); + + do + { + // Bulkread present position and moving status + dynamixel.groupBulkReadTxRxPacket(groupread_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL1_ID); + return; + } + + dxl_getdata_result = dynamixel.groupBulkReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupBulkRead getdata failed", DXL2_ID); + return; + } + + // Get Dynamixel#1 present position value + dxl1_present_position = (Int32)dynamixel.groupBulkReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + // Get Dynamixel#2 moving status value + dxl2_led_value_read = (byte)dynamixel.groupBulkReadGetData(groupread_num, DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED); + + Console.WriteLine("[ID:{0}] Present Position : {1} [ID:{2}] LED Value : {3}", DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read); + + } while (Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/Properties/AssemblyInfo.cs b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..fbef6b56 --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("bulk_read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("bulk_read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("ae76b9ac-97eb-40b1-8161-576c3b15ccb8")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/bulk_read_write.csproj b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/bulk_read_write.csproj new file mode 100644 index 00000000..8bbcd31c --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/bulk_read_write.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {AE76B9AC-97EB-40B1-8161-576C3B15CCB8} + Exe + Properties + bulk_read_write + bulk_read_write + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/dynamixel.cs b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/bulk_read_write/win64/bulk_read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/factory_reset/win32/.vs/factory_reset/v14/.suo b/c#/protocol2.0/factory_reset/win32/.vs/factory_reset/v14/.suo new file mode 100644 index 00000000..22b7db8e Binary files /dev/null and b/c#/protocol2.0/factory_reset/win32/.vs/factory_reset/v14/.suo differ diff --git a/c#/protocol2.0/factory_reset/win32/factory_reset.sln b/c#/protocol2.0/factory_reset/win32/factory_reset.sln new file mode 100644 index 00000000..b37b8333 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win32/factory_reset.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "factory_reset", "factory_reset\factory_reset.csproj", "{1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|x86.ActiveCfg = Debug|x86 + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|x86.Build.0 = Debug|x86 + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|Any CPU.Build.0 = Release|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|x86.ActiveCfg = Release|x86 + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/factory_reset/win32/factory_reset/App.config b/c#/protocol2.0/factory_reset/win32/factory_reset/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win32/factory_reset/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/factory_reset/win32/factory_reset/FactoryReset.cs b/c#/protocol2.0/factory_reset/win32/factory_reset/FactoryReset.cs new file mode 100644 index 00000000..86bd0978 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win32/factory_reset/FactoryReset.cs @@ -0,0 +1,195 @@ +/* +* factory_reset.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Factory Reset Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +// Be aware that: +// This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +using System; +using System.Threading; +using dynamixel_sdk; + +namespace factory_reset +{ + class FactoryReset + { + // Control table address + public const int ADDR_PRO_BAUDRATE = 8; // Control table address is different in Dynamixel model + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int FACTORYRST_DEFAULTBAUDRATE = 57600; // Dynamixel baudrate set by factoryreset + public const int NEW_BAUDNUM = 3; // New baudnum to recover Dynamixel baudrate as it was + public const byte OPERATION_MODE = 0x01; // 0xFF : reset all values + // 0x01 : reset all values except ID + // 0x02 : reset all values except ID and baudrate + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + byte dxl_baudnum_read; // Read baudnum + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + + // Read present baudrate of the controller + Console.WriteLine("Now the controller baudrate is : {0}", dynamixel.getBaudRate(port_num)); + + // Try factoryreset + Console.WriteLine("[ID:{0}] Try factoryreset : ", DXL_ID); + dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine("Aborted"); + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + return; + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Wait for reset + Console.WriteLine("Wait for reset..."); + Thread.Sleep(2000); + + Console.WriteLine("[ID:{0}] factoryReset Success!", DXL_ID); + + // Set controller baudrate to dxl default baudrate + if (dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", FACTORYRST_DEFAULTBAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Write new baudnum + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Set Dynamixel baudnum to : {1}", DXL_ID, NEW_BAUDNUM); + } + + // Set port baudrate to BAUDRATE + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", BAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + Thread.Sleep(200); + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel Baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/factory_reset/win32/factory_reset/Properties/AssemblyInfo.cs b/c#/protocol2.0/factory_reset/win32/factory_reset/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..b53b7246 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win32/factory_reset/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("factory_reset")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("factory_reset")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("1ed1b9dd-d30b-4aa0-92a0-84b8109f3c9b")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/factory_reset/win32/factory_reset/dynamixel.cs b/c#/protocol2.0/factory_reset/win32/factory_reset/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win32/factory_reset/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/factory_reset/win32/factory_reset/factory_reset.csproj b/c#/protocol2.0/factory_reset/win32/factory_reset/factory_reset.csproj new file mode 100644 index 00000000..a1310476 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win32/factory_reset/factory_reset.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B} + Exe + Properties + factory_reset + factory_reset + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/factory_reset/win64/.vs/factory_reset/v14/.suo b/c#/protocol2.0/factory_reset/win64/.vs/factory_reset/v14/.suo new file mode 100644 index 00000000..1428ed46 Binary files /dev/null and b/c#/protocol2.0/factory_reset/win64/.vs/factory_reset/v14/.suo differ diff --git a/c#/protocol2.0/factory_reset/win64/factory_reset.sln b/c#/protocol2.0/factory_reset/win64/factory_reset.sln new file mode 100644 index 00000000..227360f5 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win64/factory_reset.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "factory_reset", "factory_reset\factory_reset.csproj", "{1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|x64.ActiveCfg = Debug|x64 + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Debug|x64.Build.0 = Debug|x64 + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|Any CPU.Build.0 = Release|Any CPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|x64.ActiveCfg = Release|x64 + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/factory_reset/win64/factory_reset/App.config b/c#/protocol2.0/factory_reset/win64/factory_reset/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win64/factory_reset/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/factory_reset/win64/factory_reset/FactoryReset.cs b/c#/protocol2.0/factory_reset/win64/factory_reset/FactoryReset.cs new file mode 100644 index 00000000..86bd0978 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win64/factory_reset/FactoryReset.cs @@ -0,0 +1,195 @@ +/* +* factory_reset.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Factory Reset Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +// Be aware that: +// This example resets all properties of Dynamixel to default values, such as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +// + +using System; +using System.Threading; +using dynamixel_sdk; + +namespace factory_reset +{ + class FactoryReset + { + // Control table address + public const int ADDR_PRO_BAUDRATE = 8; // Control table address is different in Dynamixel model + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int FACTORYRST_DEFAULTBAUDRATE = 57600; // Dynamixel baudrate set by factoryreset + public const int NEW_BAUDNUM = 3; // New baudnum to recover Dynamixel baudrate as it was + public const byte OPERATION_MODE = 0x01; // 0xFF : reset all values + // 0x01 : reset all values except ID + // 0x02 : reset all values except ID and baudrate + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + byte dxl_baudnum_read; // Read baudnum + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + + // Read present baudrate of the controller + Console.WriteLine("Now the controller baudrate is : {0}", dynamixel.getBaudRate(port_num)); + + // Try factoryreset + Console.WriteLine("[ID:{0}] Try factoryreset : ", DXL_ID); + dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + Console.WriteLine("Aborted"); + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + return; + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Wait for reset + Console.WriteLine("Wait for reset..."); + Thread.Sleep(2000); + + Console.WriteLine("[ID:{0}] factoryReset Success!", DXL_ID); + + // Set controller baudrate to dxl default baudrate + if (dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", FACTORYRST_DEFAULTBAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Write new baudnum + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Set Dynamixel baudnum to : {1}", DXL_ID, NEW_BAUDNUM); + } + + // Set port baudrate to BAUDRATE + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeed to change the controller baudrate to : {0}", BAUDRATE); + } + else + { + Console.WriteLine("Failed to change the controller baudrate"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + Thread.Sleep(200); + + // Read Dynamixel baudnum + dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("[ID:{0}] Dynamixel Baudnum is now : {1}", DXL_ID, dxl_baudnum_read); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/factory_reset/win64/factory_reset/Properties/AssemblyInfo.cs b/c#/protocol2.0/factory_reset/win64/factory_reset/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..b53b7246 --- /dev/null +++ b/c#/protocol2.0/factory_reset/win64/factory_reset/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("factory_reset")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("factory_reset")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("1ed1b9dd-d30b-4aa0-92a0-84b8109f3c9b")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/factory_reset/win64/factory_reset/dynamixel.cs b/c#/protocol2.0/factory_reset/win64/factory_reset/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/factory_reset/win64/factory_reset/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/factory_reset/win64/factory_reset/factory_reset.csproj b/c#/protocol2.0/factory_reset/win64/factory_reset/factory_reset.csproj new file mode 100644 index 00000000..094a322f --- /dev/null +++ b/c#/protocol2.0/factory_reset/win64/factory_reset/factory_reset.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {1ED1B9DD-D30B-4AA0-92A0-84B8109F3C9B} + Exe + Properties + factory_reset + factory_reset + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/indirect_address/win32/.vs/indirect_address/v14/.suo b/c#/protocol2.0/indirect_address/win32/.vs/indirect_address/v14/.suo new file mode 100644 index 00000000..cf37f21e Binary files /dev/null and b/c#/protocol2.0/indirect_address/win32/.vs/indirect_address/v14/.suo differ diff --git a/c#/protocol2.0/indirect_address/win32/indirect_address.sln b/c#/protocol2.0/indirect_address/win32/indirect_address.sln new file mode 100644 index 00000000..b9a595aa --- /dev/null +++ b/c#/protocol2.0/indirect_address/win32/indirect_address.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "indirect_address", "indirect_address\indirect_address.csproj", "{9FAC7287-571C-43FB-8DD6-259B8CB51BFC}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|Any CPU.Build.0 = Debug|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|x86.ActiveCfg = Debug|x86 + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|x86.Build.0 = Debug|x86 + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|Any CPU.ActiveCfg = Release|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|Any CPU.Build.0 = Release|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|x86.ActiveCfg = Release|x86 + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/indirect_address/win32/indirect_address/App.config b/c#/protocol2.0/indirect_address/win32/indirect_address/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/indirect_address/win32/indirect_address/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/indirect_address/win32/indirect_address/IndirectAddress.cs b/c#/protocol2.0/indirect_address/win32/indirect_address/IndirectAddress.cs new file mode 100644 index 00000000..37a2c77b --- /dev/null +++ b/c#/protocol2.0/indirect_address/win32/indirect_address/IndirectAddress.cs @@ -0,0 +1,344 @@ +/* +* indirect_address.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Indirect Address Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace sync_write +{ + class SyncWrite + { + // Control table address // Control table address is different in Dynamixel model + public const int ADDR_PRO_INDIRECTADDRESS_FOR_WRITE = 49; // EEPROM region + public const int ADDR_PRO_INDIRECTADDRESS_FOR_READ = 59; // EEPROM region + public const int ADDR_PRO_TORQUE_ENABLE = 562; + public const int ADDR_PRO_LED_RED = 563; + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_MOVING = 610; + public const int ADDR_PRO_PRESENT_POSITION = 611; + public const int ADDR_PRO_INDIRECTDATA_FOR_WRITE = 634; + public const int ADDR_PRO_INDIRECTDATA_FOR_READ = 639; + + // Data Byte Length + public const int LEN_PRO_LED_RED = 1; + public const int LEN_PRO_GOAL_POSITION = 4; + public const int LEN_PRO_MOVING = 1; + public const int LEN_PRO_PRESENT_POSITION = 4; + public const int LEN_PRO_INDIRECTDATA_FOR_WRITE = 5; + public const int LEN_PRO_INDIRECTDATA_FOR_READ = 5; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MINIMUM_LED_VALUE = 0; // Dynamixel LED will light between this value + public const int DXL_MAXIMUM_LED_VALUE = 255; // and this value + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupsyncwrite instance + int groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE); + + // Initialize Groupsyncread Structs for Present Position + int groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + byte dxl_moving = 0; // Dynamixel moving status + byte[] dxl_led_value = new byte[2]{ 0x00, 0xFF }; // Dynamixel LED value + Int32 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Disable Dynamixel Torque : + // Indirect address would not accessible when the torque is already enabled + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected "); + } + + // INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Enable DXL Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Add parameter storage for Dynamixel present position value + dxl_addparam_result = dynamixel.groupSyncReadAddParam(groupread_num, DXL_ID); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead addparam failed", DXL_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add values to the Syncwrite storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL_ID); + return; + } + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], LEN_PRO_LED_RED); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL_ID); + return; + } + + // Syncwrite goal position + dynamixel.groupSyncWriteTxPacket(groupwrite_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear syncwrite parameter storage + dynamixel.groupSyncWriteClearParam(groupwrite_num); + + do + { + // Syncread present position from indirectdata2 + dynamixel.groupSyncReadTxRxPacket(groupread_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Check if groupsyncread data of Dyanamixel is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL_ID); + return; + } + + // Check if groupsyncread data of Dyanamixel is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL_ID); + return; + } + + // Get Dynamixel present position value + dxl_present_position = (Int32)dynamixel.groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION); + + // Get Dynamixel moving status value + dxl_moving = (byte)dynamixel.groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING); + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} IsMoving:{3}", DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving); + + } while (Math.Abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/indirect_address/win32/indirect_address/Properties/AssemblyInfo.cs b/c#/protocol2.0/indirect_address/win32/indirect_address/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..aed7cd15 --- /dev/null +++ b/c#/protocol2.0/indirect_address/win32/indirect_address/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("indirect_address")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("indirect_address")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("9fac7287-571c-43fb-8dd6-259b8cb51bfc")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/indirect_address/win32/indirect_address/dynamixel.cs b/c#/protocol2.0/indirect_address/win32/indirect_address/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/indirect_address/win32/indirect_address/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/indirect_address/win32/indirect_address/indirect_address.csproj b/c#/protocol2.0/indirect_address/win32/indirect_address/indirect_address.csproj new file mode 100644 index 00000000..3df91b3e --- /dev/null +++ b/c#/protocol2.0/indirect_address/win32/indirect_address/indirect_address.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC} + Exe + Properties + indirect_address + indirect_address + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/indirect_address/win64/.vs/indirect_address/v14/.suo b/c#/protocol2.0/indirect_address/win64/.vs/indirect_address/v14/.suo new file mode 100644 index 00000000..be9ff99f Binary files /dev/null and b/c#/protocol2.0/indirect_address/win64/.vs/indirect_address/v14/.suo differ diff --git a/c#/protocol2.0/indirect_address/win64/indirect_address.sln b/c#/protocol2.0/indirect_address/win64/indirect_address.sln new file mode 100644 index 00000000..70935824 --- /dev/null +++ b/c#/protocol2.0/indirect_address/win64/indirect_address.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "indirect_address", "indirect_address\indirect_address.csproj", "{9FAC7287-571C-43FB-8DD6-259B8CB51BFC}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|Any CPU.Build.0 = Debug|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|x64.ActiveCfg = Debug|x64 + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Debug|x64.Build.0 = Debug|x64 + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|Any CPU.ActiveCfg = Release|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|Any CPU.Build.0 = Release|Any CPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|x64.ActiveCfg = Release|x64 + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/indirect_address/win64/indirect_address/App.config b/c#/protocol2.0/indirect_address/win64/indirect_address/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/indirect_address/win64/indirect_address/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/indirect_address/win64/indirect_address/IndirectAddress.cs b/c#/protocol2.0/indirect_address/win64/indirect_address/IndirectAddress.cs new file mode 100644 index 00000000..37a2c77b --- /dev/null +++ b/c#/protocol2.0/indirect_address/win64/indirect_address/IndirectAddress.cs @@ -0,0 +1,344 @@ +/* +* indirect_address.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Indirect Address Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace sync_write +{ + class SyncWrite + { + // Control table address // Control table address is different in Dynamixel model + public const int ADDR_PRO_INDIRECTADDRESS_FOR_WRITE = 49; // EEPROM region + public const int ADDR_PRO_INDIRECTADDRESS_FOR_READ = 59; // EEPROM region + public const int ADDR_PRO_TORQUE_ENABLE = 562; + public const int ADDR_PRO_LED_RED = 563; + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_MOVING = 610; + public const int ADDR_PRO_PRESENT_POSITION = 611; + public const int ADDR_PRO_INDIRECTDATA_FOR_WRITE = 634; + public const int ADDR_PRO_INDIRECTDATA_FOR_READ = 639; + + // Data Byte Length + public const int LEN_PRO_LED_RED = 1; + public const int LEN_PRO_GOAL_POSITION = 4; + public const int LEN_PRO_MOVING = 1; + public const int LEN_PRO_PRESENT_POSITION = 4; + public const int LEN_PRO_INDIRECTDATA_FOR_WRITE = 5; + public const int LEN_PRO_INDIRECTDATA_FOR_READ = 5; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MINIMUM_LED_VALUE = 0; // Dynamixel LED will light between this value + public const int DXL_MAXIMUM_LED_VALUE = 255; // and this value + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupsyncwrite instance + int groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE); + + // Initialize Groupsyncread Structs for Present Position + int groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + byte dxl_moving = 0; // Dynamixel moving status + byte[] dxl_led_value = new byte[2]{ 0x00, 0xFF }; // Dynamixel LED value + Int32 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Disable Dynamixel Torque : + // Indirect address would not accessible when the torque is already enabled + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected "); + } + + // INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Enable DXL Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Add parameter storage for Dynamixel present position value + dxl_addparam_result = dynamixel.groupSyncReadAddParam(groupread_num, DXL_ID); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead addparam failed", DXL_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add values to the Syncwrite storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL_ID); + return; + } + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], LEN_PRO_LED_RED); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL_ID); + return; + } + + // Syncwrite goal position + dynamixel.groupSyncWriteTxPacket(groupwrite_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear syncwrite parameter storage + dynamixel.groupSyncWriteClearParam(groupwrite_num); + + do + { + // Syncread present position from indirectdata2 + dynamixel.groupSyncReadTxRxPacket(groupread_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Check if groupsyncread data of Dyanamixel is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL_ID); + return; + } + + // Check if groupsyncread data of Dyanamixel is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL_ID); + return; + } + + // Get Dynamixel present position value + dxl_present_position = (Int32)dynamixel.groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION); + + // Get Dynamixel moving status value + dxl_moving = (byte)dynamixel.groupSyncReadGetData(groupread_num, DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING); + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} IsMoving:{3}", DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving); + + } while (Math.Abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/indirect_address/win64/indirect_address/Properties/AssemblyInfo.cs b/c#/protocol2.0/indirect_address/win64/indirect_address/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..aed7cd15 --- /dev/null +++ b/c#/protocol2.0/indirect_address/win64/indirect_address/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("indirect_address")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("indirect_address")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("9fac7287-571c-43fb-8dd6-259b8cb51bfc")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/indirect_address/win64/indirect_address/dynamixel.cs b/c#/protocol2.0/indirect_address/win64/indirect_address/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/indirect_address/win64/indirect_address/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/indirect_address/win64/indirect_address/indirect_address.csproj b/c#/protocol2.0/indirect_address/win64/indirect_address/indirect_address.csproj new file mode 100644 index 00000000..016cf97e --- /dev/null +++ b/c#/protocol2.0/indirect_address/win64/indirect_address/indirect_address.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {9FAC7287-571C-43FB-8DD6-259B8CB51BFC} + Exe + Properties + indirect_address + indirect_address + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/multi_port/win32/.vs/multi_port/v14/.suo b/c#/protocol2.0/multi_port/win32/.vs/multi_port/v14/.suo new file mode 100644 index 00000000..074cccb6 Binary files /dev/null and b/c#/protocol2.0/multi_port/win32/.vs/multi_port/v14/.suo differ diff --git a/c#/protocol2.0/multi_port/win32/multi_port.sln b/c#/protocol2.0/multi_port/win32/multi_port.sln new file mode 100644 index 00000000..f74039b1 --- /dev/null +++ b/c#/protocol2.0/multi_port/win32/multi_port.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "multi_port", "multi_port\multi_port.csproj", "{E974210C-6AEC-47D6-8359-0601C9725C6F}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x86.ActiveCfg = Debug|x86 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x86.Build.0 = Debug|x86 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.Build.0 = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x86.ActiveCfg = Release|x86 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/multi_port/win32/multi_port/App.config b/c#/protocol2.0/multi_port/win32/multi_port/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/multi_port/win32/multi_port/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/multi_port/win32/multi_port/MultiPort.cs b/c#/protocol2.0/multi_port/win32/multi_port/MultiPort.cs new file mode 100644 index 00000000..5523577d --- /dev/null +++ b/c#/protocol2.0/multi_port/win32/multi_port/MultiPort.cs @@ -0,0 +1,249 @@ +/* +* multi_port.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* MultiPort Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using two Dynamixel PRO 54-200, and two USB2DYNAMIXELs. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000) +// + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME1 = "COM8"; // Check which port is being used on your controller + public const string DEVICENAME2 = "COM4"; // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num1 = dynamixel.portHandler(DEVICENAME1); + int port_num2 = dynamixel.portHandler(DEVICENAME2); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + Int32 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port1 + if (dynamixel.openPort(port_num1)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Open port2 + if (dynamixel.openPort(port_num2)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port1 baudrate + if (dynamixel.setBaudRate(port_num1, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port2 baudrate + if (dynamixel.setBaudRate(port_num2, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL2_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = (Int32)dynamixel.read4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = (Int32)dynamixel.read4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port1 + dynamixel.closePort(port_num1); + + // Close port2 + dynamixel.closePort(port_num2); + + return; + } + } +} diff --git a/c#/protocol2.0/multi_port/win32/multi_port/Properties/AssemblyInfo.cs b/c#/protocol2.0/multi_port/win32/multi_port/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..47770db0 --- /dev/null +++ b/c#/protocol2.0/multi_port/win32/multi_port/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("multi_port")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("multi_port")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("e974210c-6aec-47d6-8359-0601c9725c6f")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/multi_port/win32/multi_port/dynamixel.cs b/c#/protocol2.0/multi_port/win32/multi_port/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/multi_port/win32/multi_port/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/multi_port/win32/multi_port/multi_port.csproj b/c#/protocol2.0/multi_port/win32/multi_port/multi_port.csproj new file mode 100644 index 00000000..132f67ce --- /dev/null +++ b/c#/protocol2.0/multi_port/win32/multi_port/multi_port.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {E974210C-6AEC-47D6-8359-0601C9725C6F} + Exe + Properties + multi_port + multi_port + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/multi_port/win64/.vs/multi_port/v14/.suo b/c#/protocol2.0/multi_port/win64/.vs/multi_port/v14/.suo new file mode 100644 index 00000000..f0f9590a Binary files /dev/null and b/c#/protocol2.0/multi_port/win64/.vs/multi_port/v14/.suo differ diff --git a/c#/protocol2.0/multi_port/win64/multi_port.sln b/c#/protocol2.0/multi_port/win64/multi_port.sln new file mode 100644 index 00000000..708c3ce5 --- /dev/null +++ b/c#/protocol2.0/multi_port/win64/multi_port.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "multi_port", "multi_port\multi_port.csproj", "{E974210C-6AEC-47D6-8359-0601C9725C6F}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x64.ActiveCfg = Debug|x64 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Debug|x64.Build.0 = Debug|x64 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|Any CPU.Build.0 = Release|Any CPU + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x64.ActiveCfg = Release|x64 + {E974210C-6AEC-47D6-8359-0601C9725C6F}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/multi_port/win64/multi_port/App.config b/c#/protocol2.0/multi_port/win64/multi_port/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/multi_port/win64/multi_port/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/multi_port/win64/multi_port/MultiPort.cs b/c#/protocol2.0/multi_port/win64/multi_port/MultiPort.cs new file mode 100644 index 00000000..5523577d --- /dev/null +++ b/c#/protocol2.0/multi_port/win64/multi_port/MultiPort.cs @@ -0,0 +1,249 @@ +/* +* multi_port.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* MultiPort Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using two Dynamixel PRO 54-200, and two USB2DYNAMIXELs. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000) +// + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME1 = "COM8"; // Check which port is being used on your controller + public const string DEVICENAME2 = "COM4"; // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num1 = dynamixel.portHandler(DEVICENAME1); + int port_num2 = dynamixel.portHandler(DEVICENAME2); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + Int32 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port1 + if (dynamixel.openPort(port_num1)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Open port2 + if (dynamixel.openPort(port_num2)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port1 baudrate + if (dynamixel.setBaudRate(port_num1, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port2 baudrate + if (dynamixel.setBaudRate(port_num2, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL2_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = (Int32)dynamixel.read4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = (Int32)dynamixel.read4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port1 + dynamixel.closePort(port_num1); + + // Close port2 + dynamixel.closePort(port_num2); + + return; + } + } +} diff --git a/c#/protocol2.0/multi_port/win64/multi_port/Properties/AssemblyInfo.cs b/c#/protocol2.0/multi_port/win64/multi_port/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..47770db0 --- /dev/null +++ b/c#/protocol2.0/multi_port/win64/multi_port/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("multi_port")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("multi_port")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("e974210c-6aec-47d6-8359-0601c9725c6f")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/multi_port/win64/multi_port/dynamixel.cs b/c#/protocol2.0/multi_port/win64/multi_port/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/multi_port/win64/multi_port/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/multi_port/win64/multi_port/multi_port.csproj b/c#/protocol2.0/multi_port/win64/multi_port/multi_port.csproj new file mode 100644 index 00000000..c3730f9d --- /dev/null +++ b/c#/protocol2.0/multi_port/win64/multi_port/multi_port.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {E974210C-6AEC-47D6-8359-0601C9725C6F} + Exe + Properties + multi_port + multi_port + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/ping/win32/.vs/ping/v14/.suo b/c#/protocol2.0/ping/win32/.vs/ping/v14/.suo new file mode 100644 index 00000000..33364560 Binary files /dev/null and b/c#/protocol2.0/ping/win32/.vs/ping/v14/.suo differ diff --git a/c#/protocol2.0/ping/win32/ping.sln b/c#/protocol2.0/ping/win32/ping.sln new file mode 100644 index 00000000..bd3e7438 --- /dev/null +++ b/c#/protocol2.0/ping/win32/ping.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ping", "ping\ping.csproj", "{4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.Build.0 = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x86.ActiveCfg = Debug|x86 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x86.Build.0 = Debug|x86 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.ActiveCfg = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.Build.0 = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x86.ActiveCfg = Release|x86 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/ping/win32/ping/App.config b/c#/protocol2.0/ping/win32/ping/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/ping/win32/ping/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/ping/win32/ping/Ping.cs b/c#/protocol2.0/ping/win32/ping/Ping.cs new file mode 100644 index 00000000..b8f6f140 --- /dev/null +++ b/c#/protocol2.0/ping/win32/ping/Ping.cs @@ -0,0 +1,99 @@ +/* +* ping.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* ping Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace ping +{ + class Ping + { + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl_model_number; // Dynamixel model number + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + // Try to ping the Dynamixel + // Get Dynamixel model number + dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] ping Succeeded. Dynamixel model number : {1}", DXL_ID, dxl_model_number); + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/ping/win32/ping/Properties/AssemblyInfo.cs b/c#/protocol2.0/ping/win32/ping/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..a97376c3 --- /dev/null +++ b/c#/protocol2.0/ping/win32/ping/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("ping")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("ping")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("4d6dfb5c-0e72-47b4-9d9a-ac58f5dd3abd")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/ping/win32/ping/dynamixel.cs b/c#/protocol2.0/ping/win32/ping/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/ping/win32/ping/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/ping/win32/ping/ping.csproj b/c#/protocol2.0/ping/win32/ping/ping.csproj new file mode 100644 index 00000000..6591c52b --- /dev/null +++ b/c#/protocol2.0/ping/win32/ping/ping.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD} + Exe + Properties + ping + ping + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/ping/win64/.vs/ping/v14/.suo b/c#/protocol2.0/ping/win64/.vs/ping/v14/.suo new file mode 100644 index 00000000..5a848c37 Binary files /dev/null and b/c#/protocol2.0/ping/win64/.vs/ping/v14/.suo differ diff --git a/c#/protocol2.0/ping/win64/ping.sln b/c#/protocol2.0/ping/win64/ping.sln new file mode 100644 index 00000000..224ef50f --- /dev/null +++ b/c#/protocol2.0/ping/win64/ping.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ping", "ping\ping.csproj", "{4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|Any CPU.Build.0 = Debug|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x64.ActiveCfg = Debug|x64 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Debug|x64.Build.0 = Debug|x64 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.ActiveCfg = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|Any CPU.Build.0 = Release|Any CPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x64.ActiveCfg = Release|x64 + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/ping/win64/ping/App.config b/c#/protocol2.0/ping/win64/ping/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/ping/win64/ping/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/ping/win64/ping/Ping.cs b/c#/protocol2.0/ping/win64/ping/Ping.cs new file mode 100644 index 00000000..b8f6f140 --- /dev/null +++ b/c#/protocol2.0/ping/win64/ping/Ping.cs @@ -0,0 +1,99 @@ +/* +* ping.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* ping Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace ping +{ + class Ping + { + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl_model_number; // Dynamixel model number + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + // Try to ping the Dynamixel + // Get Dynamixel model number + dxl_model_number = dynamixel.pingGetModelNum(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] ping Succeeded. Dynamixel model number : {1}", DXL_ID, dxl_model_number); + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/ping/win64/ping/Properties/AssemblyInfo.cs b/c#/protocol2.0/ping/win64/ping/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..a97376c3 --- /dev/null +++ b/c#/protocol2.0/ping/win64/ping/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("ping")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("ping")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("4d6dfb5c-0e72-47b4-9d9a-ac58f5dd3abd")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/ping/win64/ping/dynamixel.cs b/c#/protocol2.0/ping/win64/ping/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/ping/win64/ping/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/ping/win64/ping/ping.csproj b/c#/protocol2.0/ping/win64/ping/ping.csproj new file mode 100644 index 00000000..36fea6a1 --- /dev/null +++ b/c#/protocol2.0/ping/win64/ping/ping.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {4D6DFB5C-0E72-47B4-9D9A-AC58F5DD3ABD} + Exe + Properties + ping + ping + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo b/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo new file mode 100644 index 00000000..7b2b4760 Binary files /dev/null and b/c#/protocol2.0/read_write/win32/.vs/read_write/v14/.suo differ diff --git a/c#/protocol2.0/read_write/win32/read_write.sln b/c#/protocol2.0/read_write/win32/read_write.sln new file mode 100644 index 00000000..bda9dede --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "read_write", "read_write\read_write.csproj", "{D1B60768-6B6B-492D-8E0A-D258D9B2CA27}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x86.ActiveCfg = Debug|x86 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x86.Build.0 = Debug|x86 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.Build.0 = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x86.ActiveCfg = Release|x86 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/read_write/win32/read_write/App.config b/c#/protocol2.0/read_write/win32/read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/read_write/win32/read_write/Properties/AssemblyInfo.cs b/c#/protocol2.0/read_write/win32/read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..94be0145 --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("d1b60768-6b6b-492d-8e0a-d258d9b2ca27")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/read_write/win32/read_write/ReadWrite.cs b/c#/protocol2.0/read_write/win32/read_write/ReadWrite.cs new file mode 100644 index 00000000..8b195296 --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write/ReadWrite.cs @@ -0,0 +1,170 @@ +/* +* read_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Read and Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + Int32 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable DXL Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected "); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read present position + dxl_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2}", DXL_ID, dxl_goal_position[index], dxl_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/read_write/win32/read_write/dynamixel.cs b/c#/protocol2.0/read_write/win32/read_write/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/read_write/win32/read_write/read_write.csproj b/c#/protocol2.0/read_write/win32/read_write/read_write.csproj new file mode 100644 index 00000000..131b619a --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write/read_write.csproj @@ -0,0 +1,108 @@ + + + + + Debug + AnyCPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27} + Exe + Properties + read_write + read_write + v4.5 + 512 + false + 게시\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + 0 + + + + + + + + + + + + + + + + + + + + + + False + Microsoft .NET Framework 4.5%28x86 및 x64%29 + true + + + False + .NET Framework 3.5 SP1 + false + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/read_write/win32/read_write/read_write.csproj.user b/c#/protocol2.0/read_write/win32/read_write/read_write.csproj.user new file mode 100644 index 00000000..04baf14c --- /dev/null +++ b/c#/protocol2.0/read_write/win32/read_write/read_write.csproj.user @@ -0,0 +1,17 @@ + + + + 게시\ + + + + + + ko-KR + false + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/read_write/win64/.vs/read_write/v14/.suo b/c#/protocol2.0/read_write/win64/.vs/read_write/v14/.suo new file mode 100644 index 00000000..3f5e8f9e Binary files /dev/null and b/c#/protocol2.0/read_write/win64/.vs/read_write/v14/.suo differ diff --git a/c#/protocol2.0/read_write/win64/read_write.sln b/c#/protocol2.0/read_write/win64/read_write.sln new file mode 100644 index 00000000..6c6b9cde --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "read_write", "read_write\read_write.csproj", "{D1B60768-6B6B-492D-8E0A-D258D9B2CA27}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x64.ActiveCfg = Debug|x64 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Debug|x64.Build.0 = Debug|x64 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|Any CPU.Build.0 = Release|Any CPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x64.ActiveCfg = Release|x64 + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/read_write/win64/read_write/App.config b/c#/protocol2.0/read_write/win64/read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/read_write/win64/read_write/Properties/AssemblyInfo.cs b/c#/protocol2.0/read_write/win64/read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..94be0145 --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("d1b60768-6b6b-492d-8e0a-d258d9b2ca27")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/read_write/win64/read_write/ReadWrite.cs b/c#/protocol2.0/read_write/win64/read_write/ReadWrite.cs new file mode 100644 index 00000000..8b195296 --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write/ReadWrite.cs @@ -0,0 +1,170 @@ +/* +* read_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Read and Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace read_write +{ + class ReadWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + Int32 dxl_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable DXL Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel has been successfully connected "); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + do + { + // Read present position + dxl_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2}", DXL_ID, dxl_goal_position[index], dxl_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/read_write/win64/read_write/dynamixel.cs b/c#/protocol2.0/read_write/win64/read_write/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/read_write/win64/read_write/read_write.csproj b/c#/protocol2.0/read_write/win64/read_write/read_write.csproj new file mode 100644 index 00000000..2104f52b --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write/read_write.csproj @@ -0,0 +1,107 @@ + + + + + Debug + AnyCPU + {D1B60768-6B6B-492D-8E0A-D258D9B2CA27} + Exe + Properties + read_write + read_write + v4.5 + 512 + false + 게시\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + true + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + False + Microsoft .NET Framework 4.5%28x86 및 x64%29 + true + + + False + .NET Framework 3.5 SP1 + false + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/read_write/win64/read_write/read_write.csproj.user b/c#/protocol2.0/read_write/win64/read_write/read_write.csproj.user new file mode 100644 index 00000000..04baf14c --- /dev/null +++ b/c#/protocol2.0/read_write/win64/read_write/read_write.csproj.user @@ -0,0 +1,17 @@ + + + + 게시\ + + + + + + ko-KR + false + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/reboot/win32/.vs/reboot/v14/.suo b/c#/protocol2.0/reboot/win32/.vs/reboot/v14/.suo new file mode 100644 index 00000000..0ebd696a Binary files /dev/null and b/c#/protocol2.0/reboot/win32/.vs/reboot/v14/.suo differ diff --git a/c#/protocol2.0/reboot/win32/reboot.sln b/c#/protocol2.0/reboot/win32/reboot.sln new file mode 100644 index 00000000..20f6b874 --- /dev/null +++ b/c#/protocol2.0/reboot/win32/reboot.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "reboot", "reboot\reboot.csproj", "{507A253C-9030-4EC6-8511-F8307D20C695}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|Any CPU.ActiveCfg = Release|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|Any CPU.Build.0 = Release|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|x86.ActiveCfg = Release|x86 + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|x86.Build.0 = Release|x86 + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|Any CPU.ActiveCfg = Release|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|Any CPU.Build.0 = Release|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|x86.ActiveCfg = Release|x86 + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/reboot/win32/reboot/App.config b/c#/protocol2.0/reboot/win32/reboot/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/reboot/win32/reboot/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/reboot/win32/reboot/Properties/AssemblyInfo.cs b/c#/protocol2.0/reboot/win32/reboot/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..16e33359 --- /dev/null +++ b/c#/protocol2.0/reboot/win32/reboot/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("reboot")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("reboot")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("507a253c-9030-4ec6-8511-f8307d20c695")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/reboot/win32/reboot/Reboot.cs b/c#/protocol2.0/reboot/win32/reboot/Reboot.cs new file mode 100644 index 00000000..b4d13175 --- /dev/null +++ b/c#/protocol2.0/reboot/win32/reboot/Reboot.cs @@ -0,0 +1,104 @@ +/* +* reboot.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* reboot Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace reboot +{ + class Reboot + { + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Trigger + Console.WriteLine("Press any key to reboot"); + Console.ReadKey(); + + Console.WriteLine("See the Dynamixel LED flickering"); + // Try reboot + // Dynamixel LED will flicker while it reboots + dynamixel.reboot(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] reboot Succeeded", DXL_ID); + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} \ No newline at end of file diff --git a/c#/protocol2.0/reboot/win32/reboot/dynamixel.cs b/c#/protocol2.0/reboot/win32/reboot/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/reboot/win32/reboot/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/reboot/win32/reboot/reboot.csproj b/c#/protocol2.0/reboot/win32/reboot/reboot.csproj new file mode 100644 index 00000000..6b2e3a7f --- /dev/null +++ b/c#/protocol2.0/reboot/win32/reboot/reboot.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {507A253C-9030-4EC6-8511-F8307D20C695} + Exe + Properties + reboot + reboot + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/reboot/win64/.vs/reboot/v14/.suo b/c#/protocol2.0/reboot/win64/.vs/reboot/v14/.suo new file mode 100644 index 00000000..b6dd33b1 Binary files /dev/null and b/c#/protocol2.0/reboot/win64/.vs/reboot/v14/.suo differ diff --git a/c#/protocol2.0/reboot/win64/reboot.sln b/c#/protocol2.0/reboot/win64/reboot.sln new file mode 100644 index 00000000..5c88904d --- /dev/null +++ b/c#/protocol2.0/reboot/win64/reboot.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "reboot", "reboot\reboot.csproj", "{507A253C-9030-4EC6-8511-F8307D20C695}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|Any CPU.Build.0 = Debug|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|x64.ActiveCfg = Release|x64 + {507A253C-9030-4EC6-8511-F8307D20C695}.Debug|x64.Build.0 = Release|x64 + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|Any CPU.ActiveCfg = Release|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|Any CPU.Build.0 = Release|Any CPU + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|x64.ActiveCfg = Release|x64 + {507A253C-9030-4EC6-8511-F8307D20C695}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/reboot/win64/reboot/App.config b/c#/protocol2.0/reboot/win64/reboot/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/reboot/win64/reboot/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/reboot/win64/reboot/Properties/AssemblyInfo.cs b/c#/protocol2.0/reboot/win64/reboot/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..16e33359 --- /dev/null +++ b/c#/protocol2.0/reboot/win64/reboot/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("reboot")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("reboot")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("507a253c-9030-4ec6-8511-f8307d20c695")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/reboot/win64/reboot/Reboot.cs b/c#/protocol2.0/reboot/win64/reboot/Reboot.cs new file mode 100644 index 00000000..b4d13175 --- /dev/null +++ b/c#/protocol2.0/reboot/win64/reboot/Reboot.cs @@ -0,0 +1,104 @@ +/* +* reboot.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* reboot Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace reboot +{ + class Reboot + { + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL_ID = 1; // Dynamixel ID: 1 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int dxl_comm_result = COMM_TX_FAIL; // Communication result + + byte dxl_error = 0; // Dynamixel error + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Trigger + Console.WriteLine("Press any key to reboot"); + Console.ReadKey(); + + Console.WriteLine("See the Dynamixel LED flickering"); + // Try reboot + // Dynamixel LED will flicker while it reboots + dynamixel.reboot(port_num, PROTOCOL_VERSION, DXL_ID); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + Console.WriteLine("[ID:{0}] reboot Succeeded", DXL_ID); + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} \ No newline at end of file diff --git a/c#/protocol2.0/reboot/win64/reboot/dynamixel.cs b/c#/protocol2.0/reboot/win64/reboot/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/reboot/win64/reboot/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/reboot/win64/reboot/reboot.csproj b/c#/protocol2.0/reboot/win64/reboot/reboot.csproj new file mode 100644 index 00000000..f61492da --- /dev/null +++ b/c#/protocol2.0/reboot/win64/reboot/reboot.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {507A253C-9030-4EC6-8511-F8307D20C695} + Exe + Properties + reboot + reboot + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/sync_read_write/win32/.vs/sync_read_write/v14/.suo b/c#/protocol2.0/sync_read_write/win32/.vs/sync_read_write/v14/.suo new file mode 100644 index 00000000..831ac0c4 Binary files /dev/null and b/c#/protocol2.0/sync_read_write/win32/.vs/sync_read_write/v14/.suo differ diff --git a/c#/protocol2.0/sync_read_write/win32/sync_read_write.sln b/c#/protocol2.0/sync_read_write/win32/sync_read_write.sln new file mode 100644 index 00000000..eb555e59 --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win32/sync_read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "sync_read_write", "sync_read_write\sync_read_write.csproj", "{886A86F8-4CC9-4595-9EAE-AF9DE6928753}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|Any CPU.Build.0 = Debug|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|x86.ActiveCfg = Debug|x86 + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|x86.Build.0 = Debug|x86 + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|Any CPU.ActiveCfg = Release|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|Any CPU.Build.0 = Release|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|x86.ActiveCfg = Release|x86 + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/sync_read_write/win32/sync_read_write/App.config b/c#/protocol2.0/sync_read_write/win32/sync_read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win32/sync_read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/sync_read_write/win32/sync_read_write/Properties/AssemblyInfo.cs b/c#/protocol2.0/sync_read_write/win32/sync_read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..b623627e --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win32/sync_read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("sync_read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("sync_read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("886a86f8-4cc9-4595-9eae-af9de6928753")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/sync_read_write/win32/sync_read_write/SyncReadWrite.cs b/c#/protocol2.0/sync_read_write/win32/sync_read_write/SyncReadWrite.cs new file mode 100644 index 00000000..8e94b235 --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win32/sync_read_write/SyncReadWrite.cs @@ -0,0 +1,254 @@ +/* +* sync_read_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Sync Read and Sync Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using two Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace sync_write +{ + class SyncWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Data Byte Length + public const int LEN_PRO_GOAL_POSITION = 4; + public const int LEN_PRO_PRESENT_POSITION = 4; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupsyncwrite instance + int groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION); + + // Initialize Groupsyncread Structs for Present Position + int groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + Int32 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Add parameter storage for Dynamixel#1 present position value + dxl_addparam_result = dynamixel.groupSyncReadAddParam(groupread_num, DXL1_ID); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 present position value + dxl_addparam_result = dynamixel.groupSyncReadAddParam(groupread_num, DXL2_ID); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead addparam failed", DXL2_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add Dynamixel#1 goal position value to the Syncwrite storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL1_ID, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL1_ID); + return; + } + + // Add Dynamixel#2 goal position value to the Syncwrite parameter storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL2_ID, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL2_ID); + return; + } + + // Syncwrite goal position + dynamixel.groupSyncWriteTxPacket(groupwrite_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear syncwrite parameter storage + dynamixel.groupSyncWriteClearParam(groupwrite_num); + + do + { + // Syncread present position + dynamixel.groupSyncReadTxRxPacket(groupread_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Check if groupsyncread data of Dynamixel#1 is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL1_ID); + return; + } + + // Check if groupsyncread data of Dynamixel#2 is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL2_ID); + return; + } + + // Get Dynamixel#1 present position value + dxl1_present_position = (Int32)dynamixel.groupSyncReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + // Get Dynamixel#2 present position value + dxl2_present_position = (Int32)dynamixel.groupSyncReadGetData(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/sync_read_write/win32/sync_read_write/dynamixel.cs b/c#/protocol2.0/sync_read_write/win32/sync_read_write/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win32/sync_read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/sync_read_write/win32/sync_read_write/sync_read_write.csproj b/c#/protocol2.0/sync_read_write/win32/sync_read_write/sync_read_write.csproj new file mode 100644 index 00000000..486c7cda --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win32/sync_read_write/sync_read_write.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753} + Exe + Properties + sync_read_write + sync_read_write + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/sync_read_write/win64/.vs/sync_read_write/v14/.suo b/c#/protocol2.0/sync_read_write/win64/.vs/sync_read_write/v14/.suo new file mode 100644 index 00000000..fe5e9259 Binary files /dev/null and b/c#/protocol2.0/sync_read_write/win64/.vs/sync_read_write/v14/.suo differ diff --git a/c#/protocol2.0/sync_read_write/win64/sync_read_write.sln b/c#/protocol2.0/sync_read_write/win64/sync_read_write.sln new file mode 100644 index 00000000..9d0c4cda --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win64/sync_read_write.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "sync_read_write", "sync_read_write\sync_read_write.csproj", "{886A86F8-4CC9-4595-9EAE-AF9DE6928753}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|Any CPU.ActiveCfg = Release|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|Any CPU.Build.0 = Release|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|x64.ActiveCfg = Debug|x64 + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Debug|x64.Build.0 = Debug|x64 + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|Any CPU.ActiveCfg = Release|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|Any CPU.Build.0 = Release|Any CPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|x64.ActiveCfg = Release|x64 + {886A86F8-4CC9-4595-9EAE-AF9DE6928753}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol2.0/sync_read_write/win64/sync_read_write/App.config b/c#/protocol2.0/sync_read_write/win64/sync_read_write/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win64/sync_read_write/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol2.0/sync_read_write/win64/sync_read_write/Properties/AssemblyInfo.cs b/c#/protocol2.0/sync_read_write/win64/sync_read_write/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..b623627e --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win64/sync_read_write/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("sync_read_write")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("sync_read_write")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("886a86f8-4cc9-4595-9eae-af9de6928753")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol2.0/sync_read_write/win64/sync_read_write/SyncReadWrite.cs b/c#/protocol2.0/sync_read_write/win64/sync_read_write/SyncReadWrite.cs new file mode 100644 index 00000000..8e94b235 --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win64/sync_read_write/SyncReadWrite.cs @@ -0,0 +1,254 @@ +/* +* sync_read_write.cs +* +* Created on: 2016. 5. 16. +* Author: Leon Ryu Woon Jung +*/ + +// +// ********* Sync Read and Sync Write Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 2.0 +// This example is designed for using two Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +// To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself. +// Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000 [1M]) +// + +using System; +using dynamixel_sdk; + +namespace sync_write +{ + class SyncWrite + { + // Control table address + public const int ADDR_PRO_TORQUE_ENABLE = 562; // Control table address is different in Dynamixel model + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Data Byte Length + public const int LEN_PRO_GOAL_POSITION = 4; + public const int LEN_PRO_PRESENT_POSITION = 4; + + // Protocol version + public const int PROTOCOL_VERSION = 2; // See which protocol version is used in the Dynamixel + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL_MINIMUM_POSITION_VALUE = -150000; // Dynamixel will rotate between this value + public const int DXL_MAXIMUM_POSITION_VALUE = 150000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL_MOVING_STATUS_THRESHOLD = 20; // Dynamixel moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + // Initialize Groupsyncwrite instance + int groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION); + + // Initialize Groupsyncread Structs for Present Position + int groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + bool dxl_addparam_result = false; // AddParam result + bool dxl_getdata_result = false; // GetParam result + int[] dxl_goal_position = new int[2]{ DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position + + byte dxl_error = 0; // Dynamixel error + Int32 dxl1_present_position = 0, dxl2_present_position = 0; // Present position + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + else + { + Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID); + } + + // Add parameter storage for Dynamixel#1 present position value + dxl_addparam_result = dynamixel.groupSyncReadAddParam(groupread_num, DXL1_ID); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead addparam failed", DXL1_ID); + return; + } + + // Add parameter storage for Dynamixel#2 present position value + dxl_addparam_result = dynamixel.groupSyncReadAddParam(groupread_num, DXL2_ID); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead addparam failed", DXL2_ID); + return; + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Add Dynamixel#1 goal position value to the Syncwrite storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL1_ID, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL1_ID); + return; + } + + // Add Dynamixel#2 goal position value to the Syncwrite parameter storage + dxl_addparam_result = dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL2_ID, (UInt32)dxl_goal_position[index], LEN_PRO_GOAL_POSITION); + if (dxl_addparam_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncWrite addparam failed", DXL2_ID); + return; + } + + // Syncwrite goal position + dynamixel.groupSyncWriteTxPacket(groupwrite_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Clear syncwrite parameter storage + dynamixel.groupSyncWriteClearParam(groupwrite_num); + + do + { + // Syncread present position + dynamixel.groupSyncReadTxRxPacket(groupread_num); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + + // Check if groupsyncread data of Dynamixel#1 is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL1_ID); + return; + } + + // Check if groupsyncread data of Dynamixel#2 is available + dxl_getdata_result = dynamixel.groupSyncReadIsAvailable(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + if (dxl_getdata_result != true) + { + Console.WriteLine("[ID:{0}] groupSyncRead getdata failed", DXL2_ID); + return; + } + + // Get Dynamixel#1 present position value + dxl1_present_position = (Int32)dynamixel.groupSyncReadGetData(groupread_num, DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + // Get Dynamixel#2 present position value + dxl2_present_position = (Int32)dynamixel.groupSyncReadGetData(groupread_num, DXL2_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION); + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol2.0/sync_read_write/win64/sync_read_write/dynamixel.cs b/c#/protocol2.0/sync_read_write/win64/sync_read_write/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win64/sync_read_write/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol2.0/sync_read_write/win64/sync_read_write/sync_read_write.csproj b/c#/protocol2.0/sync_read_write/win64/sync_read_write/sync_read_write.csproj new file mode 100644 index 00000000..9c2f4bcc --- /dev/null +++ b/c#/protocol2.0/sync_read_write/win64/sync_read_write/sync_read_write.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {886A86F8-4CC9-4595-9EAE-AF9DE6928753} + Exe + Properties + sync_read_write + sync_read_write + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol_combined/win32/.vs/protocol_combined/v14/.suo b/c#/protocol_combined/win32/.vs/protocol_combined/v14/.suo new file mode 100644 index 00000000..43c1b6f7 Binary files /dev/null and b/c#/protocol_combined/win32/.vs/protocol_combined/v14/.suo differ diff --git a/c#/protocol_combined/win32/protocol_combined.sln b/c#/protocol_combined/win32/protocol_combined.sln new file mode 100644 index 00000000..f080674a --- /dev/null +++ b/c#/protocol_combined/win32/protocol_combined.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "protocol_combined", "protocol_combined\protocol_combined.csproj", "{7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|x86.ActiveCfg = Debug|x86 + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|x86.Build.0 = Debug|x86 + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|Any CPU.Build.0 = Release|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|x86.ActiveCfg = Release|x86 + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|x86.Build.0 = Release|x86 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol_combined/win32/protocol_combined/App.config b/c#/protocol_combined/win32/protocol_combined/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol_combined/win32/protocol_combined/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol_combined/win32/protocol_combined/Properties/AssemblyInfo.cs b/c#/protocol_combined/win32/protocol_combined/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..155b2616 --- /dev/null +++ b/c#/protocol_combined/win32/protocol_combined/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("protocol_combined")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("protocol_combined")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("7bbebb23-23e4-4beb-8b7d-343b5218a19b")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol_combined/win32/protocol_combined/ProtocolCombined.cs b/c#/protocol_combined/win32/protocol_combined/ProtocolCombined.cs new file mode 100644 index 00000000..08bde040 --- /dev/null +++ b/c#/protocol_combined/win32/protocol_combined/ProtocolCombined.cs @@ -0,0 +1,234 @@ +/* + * protocol_combined.cs + * + * Created on: 2016. 5. 16. + * Author: Leon Ryu Woon Jung + */ + +// +// ********* Protocol Combined Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 and 2.0 +// This example is tested with a Dynamixel MX-28, a Dynamixel PRO 54-200 and an USB2DYNAMIXEL +// Be sure that properties of Dynamixel MX and PRO are already set as %% MX - ID : 1 / Baudnum : 1 (Baudrate : 1000000) , PRO - ID : 1 / Baudnum : 3 (Baudrate : 1000000) +// + +// Be aware that: +// This example configures two different control tables (especially, if it uses Dynamixel and Dynamixel PRO). It may modify critical Dynamixel parameter on the control table, if Dynamixels have wrong ID. +// + +using System; +using dynamixel_sdk; + +namespace protocol_combined +{ + class ProtocolCombined + { + // Control table address for Dynamixel MX + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Control table address for Dynamixel PRO + public const int ADDR_PRO_TORQUE_ENABLE = 562; + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Protocol version + public const int PROTOCOL_VERSION1 = 1; // See which protocol version is used in the Dynamixel + public const int PROTOCOL_VERSION2 = 2; + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL1_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL1_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL2_MINIMUM_POSITION_VALUE = -150000; + public const int DXL2_MAXIMUM_POSITION_VALUE = 150000; + public const int DXL1_MOVING_STATUS_THRESHOLD = 10; // Dynamixel MX moving status threshold + public const int DXL2_MOVING_STATUS_THRESHOLD = 20; // Dynamixel PRO moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + UInt16[] dxl1_goal_position = new UInt16[2]{ DXL1_MINIMUM_POSITION_VALUE, DXL1_MAXIMUM_POSITION_VALUE }; // Goal position of Dynamixel MX + int[] dxl2_goal_position = new int[2]{ DXL2_MINIMUM_POSITION_VALUE, DXL2_MAXIMUM_POSITION_VALUE }; // Goal position of Dynamixel PRO + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0; // Present position of Dynamixel MX + Int32 dxl2_present_position = 0; // Present position of Dynamixel PRO + + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL2_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl1_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl2_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl1_goal_position[index], dxl1_present_position, DXL2_ID, dxl2_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl1_goal_position[index] - dxl1_present_position) > DXL1_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl2_goal_position[index] - dxl2_present_position) > DXL2_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol_combined/win32/protocol_combined/dynamixel.cs b/c#/protocol_combined/win32/protocol_combined/dynamixel.cs new file mode 100644 index 00000000..235af316 --- /dev/null +++ b/c#/protocol_combined/win32/protocol_combined/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win32/output/dxl_x86_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj b/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj new file mode 100644 index 00000000..0015b985 --- /dev/null +++ b/c#/protocol_combined/win32/protocol_combined/protocol_combined.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B} + Exe + Properties + protocol_combined + protocol_combined + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x86\Debug\ + DEBUG;TRACE + full + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x86\Release\ + TRACE + true + pdbonly + x86 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/c#/protocol_combined/win64/.vs/protocol_combined/v14/.suo b/c#/protocol_combined/win64/.vs/protocol_combined/v14/.suo new file mode 100644 index 00000000..1be13fe7 Binary files /dev/null and b/c#/protocol_combined/win64/.vs/protocol_combined/v14/.suo differ diff --git a/c#/protocol_combined/win64/protocol_combined.sln b/c#/protocol_combined/win64/protocol_combined.sln new file mode 100644 index 00000000..d2dc9e68 --- /dev/null +++ b/c#/protocol_combined/win64/protocol_combined.sln @@ -0,0 +1,28 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Express 14 for Windows Desktop +VisualStudioVersion = 14.0.25123.0 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "protocol_combined", "protocol_combined\protocol_combined.csproj", "{7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|Any CPU.Build.0 = Debug|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|x64.ActiveCfg = Release|x64 + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Debug|x64.Build.0 = Release|x64 + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|Any CPU.ActiveCfg = Release|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|Any CPU.Build.0 = Release|Any CPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|x64.ActiveCfg = Release|x64 + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection +EndGlobal diff --git a/c#/protocol_combined/win64/protocol_combined/App.config b/c#/protocol_combined/win64/protocol_combined/App.config new file mode 100644 index 00000000..8e156463 --- /dev/null +++ b/c#/protocol_combined/win64/protocol_combined/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/c#/protocol_combined/win64/protocol_combined/Properties/AssemblyInfo.cs b/c#/protocol_combined/win64/protocol_combined/Properties/AssemblyInfo.cs new file mode 100644 index 00000000..155b2616 --- /dev/null +++ b/c#/protocol_combined/win64/protocol_combined/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// 어셈블리에 대한 일반 정보는 다음 특성 집합을 통해 +// 제어됩니다. 어셈블리와 관련된 정보를 수정하려면 +// 이러한 특성 값을 변경하세요. +[assembly: AssemblyTitle("protocol_combined")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("protocol_combined")] +[assembly: AssemblyCopyright("Copyright © 2016")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// ComVisible을 false로 설정하면 이 어셈블리의 형식이 COM 구성 요소에 +// 표시되지 않습니다. COM에서 이 어셈블리의 형식에 액세스하려면 +// 해당 형식에 대해 ComVisible 특성을 true로 설정하세요. +[assembly: ComVisible(false)] + +// 이 프로젝트가 COM에 노출되는 경우 다음 GUID는 typelib의 ID를 나타냅니다. +[assembly: Guid("7bbebb23-23e4-4beb-8b7d-343b5218a19b")] + +// 어셈블리의 버전 정보는 다음 네 가지 값으로 구성됩니다. +// +// 주 버전 +// 부 버전 +// 빌드 번호 +// 수정 버전 +// +// 모든 값을 지정하거나 아래와 같이 '*'를 사용하여 빌드 번호 및 수정 번호가 자동으로 +// 지정되도록 할 수 있습니다. +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/c#/protocol_combined/win64/protocol_combined/ProtocolCombined.cs b/c#/protocol_combined/win64/protocol_combined/ProtocolCombined.cs new file mode 100644 index 00000000..08bde040 --- /dev/null +++ b/c#/protocol_combined/win64/protocol_combined/ProtocolCombined.cs @@ -0,0 +1,234 @@ +/* + * protocol_combined.cs + * + * Created on: 2016. 5. 16. + * Author: Leon Ryu Woon Jung + */ + +// +// ********* Protocol Combined Example ********* +// +// +// Available Dynamixel model on this example : All models using Protocol 1.0 and 2.0 +// This example is tested with a Dynamixel MX-28, a Dynamixel PRO 54-200 and an USB2DYNAMIXEL +// Be sure that properties of Dynamixel MX and PRO are already set as %% MX - ID : 1 / Baudnum : 1 (Baudrate : 1000000) , PRO - ID : 1 / Baudnum : 3 (Baudrate : 1000000) +// + +// Be aware that: +// This example configures two different control tables (especially, if it uses Dynamixel and Dynamixel PRO). It may modify critical Dynamixel parameter on the control table, if Dynamixels have wrong ID. +// + +using System; +using dynamixel_sdk; + +namespace protocol_combined +{ + class ProtocolCombined + { + // Control table address for Dynamixel MX + public const int ADDR_MX_TORQUE_ENABLE = 24; // Control table address is different in Dynamixel model + public const int ADDR_MX_GOAL_POSITION = 30; + public const int ADDR_MX_PRESENT_POSITION = 36; + + // Control table address for Dynamixel PRO + public const int ADDR_PRO_TORQUE_ENABLE = 562; + public const int ADDR_PRO_GOAL_POSITION = 596; + public const int ADDR_PRO_PRESENT_POSITION = 611; + + // Protocol version + public const int PROTOCOL_VERSION1 = 1; // See which protocol version is used in the Dynamixel + public const int PROTOCOL_VERSION2 = 2; + + // Default setting + public const int DXL1_ID = 1; // Dynamixel ID: 1 + public const int DXL2_ID = 2; // Dynamixel ID: 2 + public const int BAUDRATE = 1000000; + public const string DEVICENAME = "COM8"; // Check which port is being used on your controller + // ex) "COM1" Linux: "/dev/ttyUSB0" + + public const int TORQUE_ENABLE = 1; // Value for enabling the torque + public const int TORQUE_DISABLE = 0; // Value for disabling the torque + public const int DXL1_MINIMUM_POSITION_VALUE = 100; // Dynamixel will rotate between this value + public const int DXL1_MAXIMUM_POSITION_VALUE = 4000; // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) + public const int DXL2_MINIMUM_POSITION_VALUE = -150000; + public const int DXL2_MAXIMUM_POSITION_VALUE = 150000; + public const int DXL1_MOVING_STATUS_THRESHOLD = 10; // Dynamixel MX moving status threshold + public const int DXL2_MOVING_STATUS_THRESHOLD = 20; // Dynamixel PRO moving status threshold + + public const byte ESC_ASCII_VALUE = 0x1b; + + public const int COMM_SUCCESS = 0; // Communication Success result value + public const int COMM_TX_FAIL = -1001; // Communication Tx Failed + + static void Main(string[] args) + { + // Initialize PortHandler Structs + // Set the port path + // Get methods and members of PortHandlerLinux or PortHandlerWindows + int port_num = dynamixel.portHandler(DEVICENAME); + + // Initialize PacketHandler Structs + dynamixel.packetHandler(); + + int index = 0; + int dxl_comm_result = COMM_TX_FAIL; // Communication result + UInt16[] dxl1_goal_position = new UInt16[2]{ DXL1_MINIMUM_POSITION_VALUE, DXL1_MAXIMUM_POSITION_VALUE }; // Goal position of Dynamixel MX + int[] dxl2_goal_position = new int[2]{ DXL2_MINIMUM_POSITION_VALUE, DXL2_MAXIMUM_POSITION_VALUE }; // Goal position of Dynamixel PRO + + byte dxl_error = 0; // Dynamixel error + UInt16 dxl1_present_position = 0; // Present position of Dynamixel MX + Int32 dxl2_present_position = 0; // Present position of Dynamixel PRO + + + // Open port + if (dynamixel.openPort(port_num)) + { + Console.WriteLine("Succeeded to open the port!"); + } + else + { + Console.WriteLine("Failed to open the port!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Set port baudrate + if (dynamixel.setBaudRate(port_num, BAUDRATE)) + { + Console.WriteLine("Succeeded to change the baudrate!"); + } + else + { + Console.WriteLine("Failed to change the baudrate!"); + Console.WriteLine("Press any key to terminate..."); + Console.ReadKey(); + return; + } + + // Enable Dynamixel#1 torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL1_ID); + } + + // Enable Dynamixel#2 torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + else + { + Console.WriteLine("Dynamixel#{0} has been successfully connected ", DXL2_ID); + } + + while (true) + { + Console.WriteLine("Press any key to continue! (or press ESC to quit!)"); + if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE) + break; + + // Write Dynamixel#1 goal position + dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_GOAL_POSITION, dxl1_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + + // Write Dynamixel#2 goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_GOAL_POSITION, (UInt32)dxl2_goal_position[index]); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + + do + { + // Read Dynamixel#1 present position + dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + + // Read Dynamixel#2 present position + dxl2_present_position = (Int32)dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_PRESENT_POSITION); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + + Console.WriteLine("[ID:{0}] GoalPos:{1} PresPos:{2} [ID:{3}] GoalPos:{4} PresPos:{5}", DXL1_ID, dxl1_goal_position[index], dxl1_present_position, DXL2_ID, dxl2_goal_position[index], dxl2_present_position); + + } while ((Math.Abs(dxl1_goal_position[index] - dxl1_present_position) > DXL1_MOVING_STATUS_THRESHOLD) || (Math.Abs(dxl2_goal_position[index] - dxl2_present_position) > DXL2_MOVING_STATUS_THRESHOLD)); + + // Change goal position + if (index == 0) + { + index = 1; + } + else + { + index = 0; + } + } + + // Disable Dynamixel#1 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION1, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION1)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION1, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION1)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION1, dxl_error); + } + + // Disable Dynamixel#2 Torque + dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION2, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE); + if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION2)) != COMM_SUCCESS) + { + dynamixel.printTxRxResult(PROTOCOL_VERSION2, dxl_comm_result); + } + else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION2)) != 0) + { + dynamixel.printRxPacketError(PROTOCOL_VERSION2, dxl_error); + } + + // Close port + dynamixel.closePort(port_num); + + return; + } + } +} diff --git a/c#/protocol_combined/win64/protocol_combined/dynamixel.cs b/c#/protocol_combined/win64/protocol_combined/dynamixel.cs new file mode 100644 index 00000000..d940e04b --- /dev/null +++ b/c#/protocol_combined/win64/protocol_combined/dynamixel.cs @@ -0,0 +1,229 @@ +using System; +using System.Runtime.InteropServices; + +namespace dynamixel_sdk +{ + class dynamixel + { + const string dll_path = "../../../c/build/win64/output/dxl_x64_c.dll"; + + #region PortHandler + [DllImport(dll_path)] + public static extern int portHandler(string port_name); + + [DllImport(dll_path)] + public static extern bool openPort(int port_num); + [DllImport(dll_path)] + public static extern void closePort(int port_num); + [DllImport(dll_path)] + public static extern void clearPort(int port_num); + + [DllImport(dll_path)] + public static extern void setPortName(int port_num, string port_name); + [DllImport(dll_path)] + public static extern string getPortName(int port_num); + + [DllImport(dll_path)] + public static extern bool setBaudRate(int port_num, int baudrate); + [DllImport(dll_path)] + public static extern int getBaudRate(int port_num); + #endregion + + #region PacketHandler + [DllImport(dll_path)] + public static extern void packetHandler(); + + [DllImport(dll_path)] + public static extern void printTxRxResult(int protocol_version, int result); + [DllImport(dll_path)] + public static extern void printRxPacketError(int protocol_version, byte error); + + [DllImport(dll_path)] + public static extern int getLastTxRxResult(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte getLastRxPacketError(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void setDataWrite(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos, UInt32 data); + [DllImport(dll_path)] + public static extern UInt32 getDataRead(int port_num, int protocol_version, UInt16 data_length, UInt16 data_pos); + + [DllImport(dll_path)] + public static extern void txPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void rxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void txRxPacket(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern void ping(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern UInt16 pingGetModelNum(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void broadcastPing(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern bool getBroadcastPingResult(int port_num, int protocol_version, int id); + + [DllImport(dll_path)] + public static extern void reboot(int port_num, int protocol_version, byte id); + + [DllImport(dll_path)] + public static extern void factoryReset(int port_num, int protocol_version, byte id, byte option); + + [DllImport(dll_path)] + public static extern void readTx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void readRx(int port_num, int protocol_version, UInt16 length); + [DllImport(dll_path)] + public static extern void readTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void read1ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern byte read1ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern byte read1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read2ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt16 read2ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt16 read2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void read4ByteTx(int port_num, int protocol_version, byte id, UInt16 address); + [DllImport(dll_path)] + public static extern UInt32 read4ByteRx(int port_num, int protocol_version); + [DllImport(dll_path)] + public static extern UInt32 read4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address); + + [DllImport(dll_path)] + public static extern void writeTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void writeTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void write1ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, byte data); + [DllImport(dll_path)] + public static extern void write1ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, byte data); + + [DllImport(dll_path)] + public static extern void write2ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + [DllImport(dll_path)] + public static extern void write2ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 data); + + [DllImport(dll_path)] + public static extern void write4ByteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + [DllImport(dll_path)] + public static extern void write4ByteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt32 data); + + [DllImport(dll_path)] + public static extern void regWriteTxOnly(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + [DllImport(dll_path)] + public static extern void regWriteTxRx(int port_num, int protocol_version, byte id, UInt16 address, UInt16 length); + + [DllImport(dll_path)] + public static extern void syncReadTx(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + // syncReadRx -> GroupSyncRead + // syncReadTxRx -> GroupSyncRead + + [DllImport(dll_path)] + public static extern void syncWriteTxOnly(int port_num, int protocol_version, UInt16 start_address, UInt16 data_length, UInt16 param_length); + + [DllImport(dll_path)] + public static extern void bulkReadTx(int port_num, int protocol_version, UInt16 param_length); + // bulkReadRx -> GroupBulkRead + // bulkReadTxRx -> GroupBulkRead + + [DllImport(dll_path)] + public static extern void bulkWriteTxOnly(int port_num, int protocol_version, UInt16 param_length); + #endregion + + #region GroupBulkRead + [DllImport(dll_path)] + public static extern int groupBulkRead(int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkReadAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupBulkReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupBulkReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupBulkReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupBulkReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupBulkReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupBulkWrite + [DllImport(dll_path)] + public static extern int groupBulkWrite (int port_num, int protocol_version); + + [DllImport(dll_path)] + public static extern bool groupBulkWriteAddParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length); + [DllImport(dll_path)] + public static extern void groupBulkWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupBulkWriteChangeParam (int group_num, byte id, UInt16 start_address, UInt16 data_length, UInt32 data, UInt16 input_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupBulkWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupBulkWriteTxPacket (int group_num); + #endregion + + #region GroupSyncRead + [DllImport(dll_path)] + public static extern int groupSyncRead (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncReadAddParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern void groupSyncReadClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncReadTxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadRxPacket (int group_num); + [DllImport(dll_path)] + public static extern void groupSyncReadTxRxPacket (int group_num); + + [DllImport(dll_path)] + public static extern bool groupSyncReadIsAvailable (int group_num, byte id, UInt16 address, UInt16 data_length); + [DllImport(dll_path)] + public static extern UInt32 groupSyncReadGetData (int group_num, byte id, UInt16 address, UInt16 data_length); + #endregion + + #region GroupSyncWrite + [DllImport(dll_path)] + public static extern int groupSyncWrite (int port_num, int protocol_version, UInt16 start_address, UInt16 data_length); + + [DllImport(dll_path)] + public static extern bool groupSyncWriteAddParam(int group_num, byte id, UInt32 data, UInt16 data_length); + [DllImport(dll_path)] + public static extern void groupSyncWriteRemoveParam (int group_num, byte id); + [DllImport(dll_path)] + public static extern bool groupSyncWriteChangeParam (int group_num, byte id, UInt32 data, UInt16 data_length, UInt16 data_pos); + [DllImport(dll_path)] + public static extern void groupSyncWriteClearParam (int group_num); + + [DllImport(dll_path)] + public static extern void groupSyncWriteTxPacket(int group_num); + #endregion + } +} diff --git a/c#/protocol_combined/win64/protocol_combined/protocol_combined.csproj b/c#/protocol_combined/win64/protocol_combined/protocol_combined.csproj new file mode 100644 index 00000000..a78b4d0e --- /dev/null +++ b/c#/protocol_combined/win64/protocol_combined/protocol_combined.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {7BBEBB23-23E4-4BEB-8B7D-343B5218A19B} + Exe + Properties + protocol_combined + protocol_combined + v4.5 + 512 + + + AnyCPU + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + true + bin\x64\Debug\ + DEBUG;TRACE + full + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + bin\x64\Release\ + TRACE + true + pdbonly + x64 + prompt + MinimumRecommendedRules.ruleset + true + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file