BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- delete dynamixel_sdk (Use ROBOTIS-GIT/DynamixelSDK repository)

Browse Source
This commit is contained in:
ROBOTIS-zerom
2016-08-12 10:47:06 +09:00
parent edfe808b0b
commit a415fff32f
23 changed files with 0 additions and 4337 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
dynamixel_sdk/CMakeLists.txt
View File

@ -1,38 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(dynamixel_sdk)
find_package(catkin REQUIRED COMPONENTS
roscpp
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES dynamixel_sdk
# CATKIN_DEPENDS roscpp
# DEPENDS system_lib
)
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(dynamixel_sdk
src/${PROJECT_NAME}/packet_handler.cpp
src/${PROJECT_NAME}/protocol1_packet_handler.cpp
src/${PROJECT_NAME}/protocol2_packet_handler.cpp
src/${PROJECT_NAME}/group_sync_read.cpp
src/${PROJECT_NAME}/group_sync_write.cpp
src/${PROJECT_NAME}/group_bulk_read.cpp
src/${PROJECT_NAME}/group_bulk_write.cpp
src/${PROJECT_NAME}/port_handler.cpp
src/${PROJECT_NAME}_linux/port_handler_linux.cpp
)
## Specify libraries to link a library or executable target against
target_link_libraries(dynamixel_sdk
${catkin_LIBRARIES}
)

59
dynamixel_sdk/include/dynamixel_sdk.h
View File

@ -1,59 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* dynamixel_sdk.h
*
* Created on: 2016. 3. 8.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_
#include "dynamixel_sdk/group_bulk_read.h"
#include "dynamixel_sdk/group_bulk_write.h"
#include "dynamixel_sdk/group_sync_read.h"
#include "dynamixel_sdk/group_sync_write.h"
#include "dynamixel_sdk/protocol1_packet_handler.h"
#include "dynamixel_sdk/protocol2_packet_handler.h"
#ifdef __linux__
#include "dynamixel_sdk_linux/port_handler_linux.h"
#endif
#if defined(_WIN32) || defined(_WIN64)
#include "dynamixel_sdk_windows/port_handler_windows.h"
#endif
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */

92
dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h
View File

@ -1,92 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_bulk_read.h
*
* Created on: 2016. 1. 28.
* Author: zerom, leon
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_
#include <map>
#include <vector>
#include "port_handler.h"
#include "packet_handler.h"
namespace dynamixel
{
class WINDECLSPEC GroupBulkRead
{
private:
PortHandler *port_;
PacketHandler *ph_;
std::vector<uint8_t> id_list_;
std::map<uint8_t, uint16_t> address_list_; // <id, start_address>
std::map<uint8_t, uint16_t> length_list_; // <id, data_length>
std::map<uint8_t, uint8_t *> data_list_; // <id, data>
bool last_result_;
bool is_param_changed_;
uint8_t *param_;
void makeParam();
public:
GroupBulkRead(PortHandler *port, PacketHandler *ph);
~GroupBulkRead() { clearParam(); }
PortHandler *getPortHandler() { return port_; }
PacketHandler *getPacketHandler() { return ph_; }
bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length);
void removeParam (uint8_t id);
void clearParam ();
int txPacket();
int rxPacket();
int txRxPacket();
bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length);
uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length);
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */

87
dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h
View File

@ -1,87 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_bulk_write.h
*
* Created on: 2016. 2. 2.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_
#include <map>
#include <vector>
#include "port_handler.h"
#include "packet_handler.h"
namespace dynamixel
{
class WINDECLSPEC GroupBulkWrite
{
private:
PortHandler *port_;
PacketHandler *ph_;
std::vector<uint8_t> id_list_;
std::map<uint8_t, uint16_t> address_list_; // <id, start_address>
std::map<uint8_t, uint16_t> length_list_; // <id, data_length>
std::map<uint8_t, uint8_t *> data_list_; // <id, data>
bool is_param_changed_;
uint8_t *param_;
uint16_t param_length_;
void makeParam();
public:
GroupBulkWrite(PortHandler *port, PacketHandler *ph);
~GroupBulkWrite() { clearParam(); }
PortHandler *getPortHandler() { return port_; }
PacketHandler *getPacketHandler() { return ph_; }
bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data);
void removeParam (uint8_t id);
bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data);
void clearParam ();
int txPacket();
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */

91
dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h
View File

@ -1,91 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_sync_read.h
*
* Created on: 2016. 2. 2.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_
#include <map>
#include <vector>
#include "port_handler.h"
#include "packet_handler.h"
namespace dynamixel
{
class WINDECLSPEC GroupSyncRead
{
private:
PortHandler *port_;
PacketHandler *ph_;
std::vector<uint8_t> id_list_;
std::map<uint8_t, uint8_t* > data_list_; // <id, data>
bool last_result_;
bool is_param_changed_;
uint8_t *param_;
uint16_t start_address_;
uint16_t data_length_;
void makeParam();
public:
GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length);
~GroupSyncRead() { clearParam(); }
PortHandler *getPortHandler() { return port_; }
PacketHandler *getPacketHandler() { return ph_; }
bool addParam (uint8_t id);
void removeParam (uint8_t id);
void clearParam ();
int txPacket();
int rxPacket();
int txRxPacket();
bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length);
uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length);
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */

86
dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h
View File

@ -1,86 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_sync_write.h
*
* Created on: 2016. 1. 28.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_
#include <map>
#include <vector>
#include "port_handler.h"
#include "packet_handler.h"
namespace dynamixel
{
class WINDECLSPEC GroupSyncWrite
{
private:
PortHandler *port_;
PacketHandler *ph_;
std::vector<uint8_t> id_list_;
std::map<uint8_t, uint8_t* > data_list_; // <id, data>
bool is_param_changed_;
uint8_t *param_;
uint16_t start_address_;
uint16_t data_length_;
void makeParam();
public:
GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length);
~GroupSyncWrite() { clearParam(); }
PortHandler *getPortHandler() { return port_; }
PacketHandler *getPacketHandler() { return ph_; }
bool addParam (uint8_t id, uint8_t *data);
void removeParam (uint8_t id);
bool changeParam (uint8_t id, uint8_t *data);
void clearParam ();
int txPacket();
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */

164
dynamixel_sdk/include/dynamixel_sdk/packet_handler.h
View File

@ -1,164 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* packet_handler.h
*
* Created on: 2016. 1. 26.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_
#include <stdio.h>
#include <vector>
#include "port_handler.h"
#define BROADCAST_ID 0xFE // 254
#define MAX_ID 0xFC // 252
/* Macro for Control Table Value */
#define DXL_MAKEWORD(a, b) ((unsigned short)(((unsigned char)(((unsigned long)(a)) & 0xff)) | ((unsigned short)((unsigned char)(((unsigned long)(b)) & 0xff))) << 8))
#define DXL_MAKEDWORD(a, b) ((unsigned int)(((unsigned short)(((unsigned long)(a)) & 0xffff)) | ((unsigned int)((unsigned short)(((unsigned long)(b)) & 0xffff))) << 16))
#define DXL_LOWORD(l) ((unsigned short)(((unsigned long)(l)) & 0xffff))
#define DXL_HIWORD(l) ((unsigned short)((((unsigned long)(l)) >> 16) & 0xffff))
#define DXL_LOBYTE(w) ((unsigned char)(((unsigned long)(w)) & 0xff))
#define DXL_HIBYTE(w) ((unsigned char)((((unsigned long)(w)) >> 8) & 0xff))
/* Instruction for DXL Protocol */
#define INST_PING 1
#define INST_READ 2
#define INST_WRITE 3
#define INST_REG_WRITE 4
#define INST_ACTION 5
#define INST_FACTORY_RESET 6
#define INST_SYNC_WRITE 131 // 0x83
#define INST_BULK_READ 146 // 0x92
// --- Only for 2.0 --- //
#define INST_REBOOT 8
#define INST_STATUS 85 // 0x55
#define INST_SYNC_READ 130 // 0x82
#define INST_BULK_WRITE 147 // 0x93
// Communication Result
#define COMM_SUCCESS 0 // tx or rx packet communication success
#define COMM_PORT_BUSY -1000 // Port is busy (in use)
#define COMM_TX_FAIL -1001 // Failed transmit instruction packet
#define COMM_RX_FAIL -1002 // Failed get status packet
#define COMM_TX_ERROR -2000 // Incorrect instruction packet
#define COMM_RX_WAITING -3000 // Now recieving status packet
#define COMM_RX_TIMEOUT -3001 // There is no status packet
#define COMM_RX_CORRUPT -3002 // Incorrect status packet
#define COMM_NOT_AVAILABLE -9000 //
namespace dynamixel
{
class WINDECLSPEC PacketHandler
{
protected:
PacketHandler() { }
public:
static PacketHandler *getPacketHandler(float protocol_version = 2.0);
virtual ~PacketHandler() { }
virtual float getProtocolVersion() = 0;
virtual void printTxRxResult(int result) = 0;
virtual void printRxPacketError(uint8_t error) = 0;
virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0;
virtual int rxPacket (PortHandler *port, uint8_t *rxpacket) = 0;
virtual int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0;
virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0;
virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0;
// broadcastPing
virtual int broadcastPing (PortHandler *port, std::vector<uint8_t> &id_list) = 0;
virtual int action (PortHandler *port, uint8_t id) = 0;
virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0;
virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0;
virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0;
virtual int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0;
virtual int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0) = 0;
virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0;
virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0;
virtual int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0) = 0;
virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0;
virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0;
virtual int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0) = 0;
virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0;
virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0;
virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0;
virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0;
virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0;
virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0;
virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0;
virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0;
virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0;
virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0;
virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0;
// SyncReadRx -> GroupSyncRead class
// SyncReadTxRx -> GroupSyncRead class
virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0;
virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0;
// BulkReadRx -> GroupBulkRead class
// BulkReadTxRx -> GroupBulkRead class
virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0;
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */

94
dynamixel_sdk/include/dynamixel_sdk/port_handler.h
View File

@ -1,94 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* port_handler.h
*
* Created on: 2016. 1. 26.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_
#undef __linux__
#define __linux__
#ifdef __linux__
#define WINDECLSPEC
#elif defined(_WIN32) || defined(_WIN64)
#ifdef WINDLLEXPORT
#define WINDECLSPEC __declspec(dllexport)
#else
#define WINDECLSPEC __declspec(dllimport)
#endif
#endif
#include <stdint.h>
namespace dynamixel
{
class WINDECLSPEC PortHandler
{
public:
static const int DEFAULT_BAUDRATE_ = 1000000;
static PortHandler *getPortHandler(const char *port_name);
bool is_using_;
virtual ~PortHandler() { }
virtual bool openPort() = 0;
virtual void closePort() = 0;
virtual void clearPort() = 0;
virtual void setPortName(const char* port_name) = 0;
virtual char *getPortName() = 0;
virtual bool setBaudRate(const int baudrate) = 0;
virtual int getBaudRate() = 0;
virtual int getBytesAvailable() = 0;
virtual int readPort(uint8_t *packet, int length) = 0;
virtual int writePort(uint8_t *packet, int length) = 0;
virtual void setPacketTimeout(uint16_t packet_length) = 0;
virtual void setPacketTimeout(double msec) = 0;
virtual bool isPacketTimeout() = 0;
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */

129
dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h
View File

@ -1,129 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* protocol1_packet_handler.h
*
* Created on: 2016. 1. 26.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_
#include "packet_handler.h"
namespace dynamixel
{
class WINDECLSPEC Protocol1PacketHandler : public PacketHandler
{
private:
static Protocol1PacketHandler *unique_instance_;
Protocol1PacketHandler();
public:
static Protocol1PacketHandler *getInstance() { return unique_instance_; }
virtual ~Protocol1PacketHandler() { }
float getProtocolVersion() { return 1.0; }
void printTxRxResult(int result);
void printRxPacketError(uint8_t error);
int txPacket (PortHandler *port, uint8_t *txpacket);
int rxPacket (PortHandler *port, uint8_t *rxpacket);
int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0);
int ping (PortHandler *port, uint8_t id, uint8_t *error = 0);
int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0);
// broadcastPing
int broadcastPing (PortHandler *port, std::vector<uint8_t> &id_list);
int action (PortHandler *port, uint8_t id);
int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0);
int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0);
int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length);
int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0);
int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address);
int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0);
int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0);
int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address);
int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0);
int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0);
int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address);
int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0);
int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0);
int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data);
int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0);
int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data);
int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0);
int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data);
int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0);
int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length);
// SyncReadRx -> GroupSyncRead class
// SyncReadTxRx -> GroupSyncRead class
// param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn
int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length);
// param : LEN1 ID1 ADDR1 LEN2 ID2 ADDR2 ...
int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length);
// BulkReadRx -> GroupBulkRead class
// BulkReadTxRx -> GroupBulkRead class
int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length);
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */

134
dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h
View File

@ -1,134 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* protocol2_packet_handler.h
*
* Created on: 2016. 1. 26.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_
#include "packet_handler.h"
namespace dynamixel
{
class WINDECLSPEC Protocol2PacketHandler : public PacketHandler
{
private:
static Protocol2PacketHandler *unique_instance_;
Protocol2PacketHandler();
uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size);
void addStuffing(uint8_t *packet);
void removeStuffing(uint8_t *packet);
public:
static Protocol2PacketHandler *getInstance() { return unique_instance_; }
virtual ~Protocol2PacketHandler() { }
float getProtocolVersion() { return 2.0; }
void printTxRxResult(int result);
void printRxPacketError(uint8_t error);
int txPacket (PortHandler *port, uint8_t *txpacket);
int rxPacket (PortHandler *port, uint8_t *rxpacket);
int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0);
int ping (PortHandler *port, uint8_t id, uint8_t *error = 0);
int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0);
// broadcastPing
int broadcastPing (PortHandler *port, std::vector<uint8_t> &id_list);
int action (PortHandler *port, uint8_t id);
int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0);
int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0);
int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length);
int readRx (PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error = 0);
int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address);
int read1ByteRx (PortHandler *port, uint8_t *data, uint8_t *error = 0);
int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0);
int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address);
int read2ByteRx (PortHandler *port, uint16_t *data, uint8_t *error = 0);
int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0);
int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address);
int read4ByteRx (PortHandler *port, uint32_t *data, uint8_t *error = 0);
int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0);
int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data);
int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0);
int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data);
int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0);
int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data);
int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0);
int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0);
int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length);
// SyncReadRx -> GroupSyncRead class
// SyncReadTxRx -> GroupSyncRead class
// param : ID1 DATA0 DATA1 ... DATAn ID2 DATA0 DATA1 ... DATAn ID3 DATA0 DATA1 ... DATAn
int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length);
// param : ID1 ADDR_L1 ADDR_H1 LEN_L1 LEN_H1 ID2 ADDR_L2 ADDR_H2 LEN_L2 LEN_H2 ...
int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length);
// BulkReadRx -> GroupBulkRead class
// BulkReadTxRx -> GroupBulkRead class
// param : ID1 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn ID2 START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H DATA0 DATA1 ... DATAn
int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length);
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */

93
dynamixel_sdk/include/dynamixel_sdk_linux/port_handler_linux.h
View File

@ -1,93 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* port_handler_linux.h
*
* Created on: 2016. 1. 26.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_
#include "dynamixel_sdk/port_handler.h"
namespace dynamixel
{
class PortHandlerLinux : public PortHandler
{
private:
int socket_fd_;
int baudrate_;
char port_name_[30];
double packet_start_time_;
double packet_timeout_;
double tx_time_per_byte;
bool setupPort(const int cflag_baud);
bool setCustomBaudrate(int speed);
int getCFlagBaud(const int baudrate);
double getCurrentTime();
double getTimeSinceStart();
public:
PortHandlerLinux(const char *port_name);
virtual ~PortHandlerLinux() { closePort(); }
bool openPort();
void closePort();
void clearPort();
void setPortName(const char *port_name);
char *getPortName();
bool setBaudRate(const int baudrate);
int getBaudRate();
int getBytesAvailable();
int readPort(uint8_t *packet, int length);
int writePort(uint8_t *packet, int length);
void setPacketTimeout(uint16_t packet_length);
void setPacketTimeout(double msec);
bool isPacketTimeout();
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */

93
dynamixel_sdk/include/dynamixel_sdk_windows/port_handler_windows.h
View File

@ -1,93 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: Leon Ryu Woon Jung */
/*
* port_handler_windows.h
*
* Created on: 2016. 4. 26.
*/
#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_
#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_
#include <Windows.h>
#include "dynamixel_sdk/port_handler.h"
namespace dynamixel
{
class WINDECLSPEC PortHandlerWindows : public PortHandler
{
private:
HANDLE serial_handle_;
LARGE_INTEGER freq_, counter_;
int baudrate_;
char port_name_[30];
double packet_start_time_;
double packet_timeout_;
double tx_time_per_byte_;
bool setupPort(const int baudrate);
double getCurrentTime();
double getTimeSinceStart();
public:
PortHandlerWindows(const char *port_name);
virtual ~PortHandlerWindows() { closePort(); }
bool openPort();
void closePort();
void clearPort();
void setPortName(const char *port_name);
char *getPortName();
bool setBaudRate(const int baudrate);
int getBaudRate();
int getBytesAvailable();
int readPort(uint8_t *packet, int length);
int writePort(uint8_t *packet, int length);
void setPacketTimeout(uint16_t packet_length);
void setPacketTimeout(double msec);
bool isPacketTimeout();
};
}
#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERWINDOWS_H_ */

15
dynamixel_sdk/package.xml
View File

@ -1,15 +0,0 @@
<?xml version="1.0"?>
<package>
<name>dynamixel_sdk</name>
<version>0.1.0</version>
<description>The dynamixel_sdk package</description>
<maintainer email="zerom@robotis.com">robotis</maintainer>
<license>BSD</license>
<!-- <url type="website">http://wiki.ros.org/dynamixel_sdk</url> -->
<author email="zerom@robotis.com">ROBOTIS</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>
</package>

237
dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp
View File

@ -1,237 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_bulk_read.cpp
*
* Created on: 2016. 1. 28.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include <stdio.h>
#include <algorithm>
#include "dynamixel_sdk/group_bulk_read.h"
using namespace dynamixel;
GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph)
: port_(port),
ph_(ph),
last_result_(false),
is_param_changed_(false),
param_(0)
{
clearParam();
}
void GroupBulkRead::makeParam()
{
if (id_list_.size() == 0)
return;
if (param_ != 0)
delete[] param_;
param_ = 0;
if (ph_->getProtocolVersion() == 1.0)
{
param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1)
}
else // 2.0
{
param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2)
}
int idx = 0;
for (unsigned int i = 0; i < id_list_.size(); i++)
{
uint8_t id = id_list_[i];
if (ph_->getProtocolVersion() == 1.0)
{
param_[idx++] = (uint8_t)length_list_[id]; // LEN
param_[idx++] = id; // ID
param_[idx++] = (uint8_t)address_list_[id]; // ADDR
}
else // 2.0
{
param_[idx++] = id; // ID
param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L
param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H
param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L
param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H
}
}
}
bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length)
{
if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist
return false;
id_list_.push_back(id);
length_list_[id] = data_length;
address_list_[id] = start_address;
data_list_[id] = new uint8_t[data_length];
is_param_changed_ = true;
return true;
}
void GroupBulkRead::removeParam(uint8_t id)
{
std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
if (it == id_list_.end()) // NOT exist
return;
id_list_.erase(it);
address_list_.erase(id);
length_list_.erase(id);
delete[] data_list_[id];
data_list_.erase(id);
is_param_changed_ = true;
}
void GroupBulkRead::clearParam()
{
if (id_list_.size() == 0)
return;
for (unsigned int i = 0; i < id_list_.size(); i++)
delete[] data_list_[id_list_[i]];
id_list_.clear();
address_list_.clear();
length_list_.clear();
data_list_.clear();
if (param_ != 0)
delete[] param_;
param_ = 0;
}
int GroupBulkRead::txPacket()
{
if (id_list_.size() == 0)
return COMM_NOT_AVAILABLE;
if (is_param_changed_ == true)
makeParam();
if (ph_->getProtocolVersion() == 1.0)
{
return ph_->bulkReadTx(port_, param_, id_list_.size() * 3);
}
else // 2.0
{
return ph_->bulkReadTx(port_, param_, id_list_.size() * 5);
}
}
int GroupBulkRead::rxPacket()
{
int cnt = id_list_.size();
int result = COMM_RX_FAIL;
last_result_ = false;
if (cnt == 0)
return COMM_NOT_AVAILABLE;
for (int i = 0; i < cnt; i++)
{
uint8_t id = id_list_[i];
result = ph_->readRx(port_, length_list_[id], data_list_[id]);
if (result != COMM_SUCCESS)
{
fprintf(stderr, "[GroupBulkRead::rxPacket] ID %d result : %d !!!!!!!!!!\n", id, result);
return result;
}
}
if (result == COMM_SUCCESS)
last_result_ = true;
return result;
}
int GroupBulkRead::txRxPacket()
{
int result = COMM_TX_FAIL;
result = txPacket();
if (result != COMM_SUCCESS)
return result;
return rxPacket();
}
bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
{
uint16_t start_addr;
if (last_result_ == false || data_list_.find(id) == data_list_.end())
return false;
start_addr = address_list_[id];
if (address < start_addr || start_addr + length_list_[id] - data_length < address)
return false;
return true;
}
uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length)
{
if (isAvailable(id, address, data_length) == false)
return 0;
uint16_t start_addr = address_list_[id];
switch(data_length)
{
case 1:
return data_list_[id][address - start_addr];
case 2:
return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]);
case 4:
return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]),
DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3]));
default:
return 0;
}
}

168
dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp
View File

@ -1,168 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_bulk_write.cpp
*
* Created on: 2016. 2. 2.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include <algorithm>
#include "dynamixel_sdk/group_bulk_write.h"
using namespace dynamixel;
GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph)
: port_(port),
ph_(ph),
is_param_changed_(false),
param_(0),
param_length_(0)
{
clearParam();
}
void GroupBulkWrite::makeParam()
{
if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
return;
if (param_ != 0)
delete[] param_;
param_ = 0;
param_length_ = 0;
for (unsigned int i = 0; i < id_list_.size(); i++)
param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]];
param_ = new uint8_t[param_length_];
int idx = 0;
for (unsigned int i = 0; i < id_list_.size(); i++)
{
uint8_t id = id_list_[i];
if (data_list_[id] == 0)
return;
param_[idx++] = id;
param_[idx++] = DXL_LOBYTE(address_list_[id]);
param_[idx++] = DXL_HIBYTE(address_list_[id]);
param_[idx++] = DXL_LOBYTE(length_list_[id]);
param_[idx++] = DXL_HIBYTE(length_list_[id]);
for (int c = 0; c < length_list_[id]; c++)
param_[idx++] = (data_list_[id])[c];
}
}
bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data)
{
if (ph_->getProtocolVersion() == 1.0)
return false;
if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist
return false;
id_list_.push_back(id);
address_list_[id] = start_address;
length_list_[id] = data_length;
data_list_[id] = new uint8_t[data_length];
for (int c = 0; c < data_length; c++)
data_list_[id][c] = data[c];
is_param_changed_ = true;
return true;
}
void GroupBulkWrite::removeParam(uint8_t id)
{
if (ph_->getProtocolVersion() == 1.0)
return;
std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
if (it == id_list_.end()) // NOT exist
return;
id_list_.erase(it);
address_list_.erase(id);
length_list_.erase(id);
delete[] data_list_[id];
data_list_.erase(id);
is_param_changed_ = true;
}
bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data)
{
if (ph_->getProtocolVersion() == 1.0)
return false;
std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
if (it == id_list_.end()) // NOT exist
return false;
address_list_[id] = start_address;
length_list_[id] = data_length;
delete[] data_list_[id];
data_list_[id] = new uint8_t[data_length];
for (int c = 0; c < data_length; c++)
data_list_[id][c] = data[c];
is_param_changed_ = true;
return true;
}
void GroupBulkWrite::clearParam()
{
if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
return;
for (unsigned int i = 0; i < id_list_.size(); i++)
delete[] data_list_[id_list_[i]];
id_list_.clear();
address_list_.clear();
length_list_.clear();
data_list_.clear();
if (param_ != 0)
delete[] param_;
param_ = 0;
}
int GroupBulkWrite::txPacket()
{
if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
return COMM_NOT_AVAILABLE;
if (is_param_changed_ == true)
makeParam();
return ph_->bulkWriteTxOnly(port_, param_, param_length_);
}

203
dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp
View File

@ -1,203 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_sync_read.cpp
*
* Created on: 2016. 2. 2.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include <algorithm>
#include "dynamixel_sdk/group_sync_read.h"
using namespace dynamixel;
GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length)
: port_(port),
ph_(ph),
last_result_(false),
is_param_changed_(false),
param_(0),
start_address_(start_address),
data_length_(data_length)
{
clearParam();
}
void GroupSyncRead::makeParam()
{
if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
return;
if (param_ != 0)
delete[] param_;
param_ = 0;
param_ = new uint8_t[id_list_.size() * 1]; // ID(1)
int idx = 0;
for (unsigned int i = 0; i < id_list_.size(); i++)
param_[idx++] = id_list_[i];
}
bool GroupSyncRead::addParam(uint8_t id)
{
if (ph_->getProtocolVersion() == 1.0)
return false;
if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist
return false;
id_list_.push_back(id);
data_list_[id] = new uint8_t[data_length_];
is_param_changed_ = true;
return true;
}
void GroupSyncRead::removeParam(uint8_t id)
{
if (ph_->getProtocolVersion() == 1.0)
return;
std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
if (it == id_list_.end()) // NOT exist
return;
id_list_.erase(it);
delete[] data_list_[id];
data_list_.erase(id);
is_param_changed_ = true;
}
void GroupSyncRead::clearParam()
{
if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
return;
for (unsigned int i = 0; i < id_list_.size(); i++)
delete[] data_list_[id_list_[i]];
id_list_.clear();
data_list_.clear();
if (param_ != 0)
delete[] param_;
param_ = 0;
}
int GroupSyncRead::txPacket()
{
if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0)
return COMM_NOT_AVAILABLE;
if (is_param_changed_ == true)
makeParam();
return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1);
}
int GroupSyncRead::rxPacket()
{
last_result_ = false;
if (ph_->getProtocolVersion() == 1.0)
return COMM_NOT_AVAILABLE;
int cnt = id_list_.size();
int result = COMM_RX_FAIL;
if (cnt == 0)
return COMM_NOT_AVAILABLE;
for (int i = 0; i < cnt; i++)
{
uint8_t id = id_list_[i];
result = ph_->readRx(port_, data_length_, data_list_[id]);
if (result != COMM_SUCCESS)
return result;
}
if (result == COMM_SUCCESS)
last_result_ = true;
return result;
}
int GroupSyncRead::txRxPacket()
{
if (ph_->getProtocolVersion() == 1.0)
return COMM_NOT_AVAILABLE;
int result = COMM_TX_FAIL;
result = txPacket();
if (result != COMM_SUCCESS)
return result;
return rxPacket();
}
bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
{
if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end())
return false;
if (address < start_address_ || start_address_ + data_length_ - data_length < address)
return false;
return true;
}
uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length)
{
if (isAvailable(id, address, data_length) == false)
return 0;
switch(data_length)
{
case 1:
return data_list_[id][address - start_address_];
case 2:
return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]);
case 4:
return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]),
DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3]));
default:
return 0;
}
}

147
dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp
View File

@ -1,147 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* group_sync_write.cpp
*
* Created on: 2016. 1. 28.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include <algorithm>
#include "dynamixel_sdk/group_sync_write.h"
using namespace dynamixel;
GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length)
: port_(port),
ph_(ph),
is_param_changed_(false),
param_(0),
start_address_(start_address),
data_length_(data_length)
{
clearParam();
}
void GroupSyncWrite::makeParam()
{
if (id_list_.size() == 0) return;
if (param_ != 0)
delete[] param_;
param_ = 0;
param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length)
int idx = 0;
for (unsigned int i = 0; i < id_list_.size(); i++)
{
uint8_t id = id_list_[i];
if (data_list_[id] == 0)
return;
param_[idx++] = id;
for (int c = 0; c < data_length_; c++)
param_[idx++] = (data_list_[id])[c];
}
}
bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data)
{
if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist
return false;
id_list_.push_back(id);
data_list_[id] = new uint8_t[data_length_];
for (int c = 0; c < data_length_; c++)
data_list_[id][c] = data[c];
is_param_changed_ = true;
return true;
}
void GroupSyncWrite::removeParam(uint8_t id)
{
std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
if (it == id_list_.end()) // NOT exist
return;
id_list_.erase(it);
delete[] data_list_[id];
data_list_.erase(id);
is_param_changed_ = true;
}
bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data)
{
std::vector<uint8_t>::iterator it = std::find(id_list_.begin(), id_list_.end(), id);
if (it == id_list_.end()) // NOT exist
return false;
delete[] data_list_[id];
data_list_[id] = new uint8_t[data_length_];
for (int c = 0; c < data_length_; c++)
data_list_[id][c] = data[c];
is_param_changed_ = true;
return true;
}
void GroupSyncWrite::clearParam()
{
if (id_list_.size() == 0)
return;
for (unsigned int i = 0; i < id_list_.size(); i++)
delete[] data_list_[id_list_[i]];
id_list_.clear();
data_list_.clear();
if (param_ != 0)
delete[] param_;
param_ = 0;
}
int GroupSyncWrite::txPacket()
{
if (id_list_.size() == 0)
return COMM_NOT_AVAILABLE;
if (is_param_changed_ == true)
makeParam();
return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_));
}

60
dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp
View File

@ -1,60 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* packet_handler.cpp
*
* Created on: 2016. 1. 26.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include "dynamixel_sdk/packet_handler.h"
#include "dynamixel_sdk/protocol1_packet_handler.h"
#include "dynamixel_sdk/protocol2_packet_handler.h"
using namespace dynamixel;
PacketHandler *PacketHandler::getPacketHandler(float protocol_version)
{
if (protocol_version == 1.0)
{
return (PacketHandler *)(Protocol1PacketHandler::getInstance());
}
else if (protocol_version == 2.0)
{
return (PacketHandler *)(Protocol2PacketHandler::getInstance());
}
return (PacketHandler *)(Protocol2PacketHandler::getInstance());
}

63
dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp
View File

@ -1,63 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* port_handler.cpp
*
* Created on: 2016. 2. 5.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include "dynamixel_sdk/port_handler.h"
#ifdef __linux__
#include "dynamixel_sdk_linux/port_handler_linux.h"
#endif
#if defined(_WIN32) || defined(_WIN64)
#include "dynamixel_sdk_windows/port_handler_windows.h"
#endif
using namespace dynamixel;
PortHandler *PortHandler::getPortHandler(const char *port_name)
{
#ifdef __linux__
return (PortHandler *)(new PortHandlerLinux(port_name));
#endif
#if defined(_WIN32) || defined(_WIN64)
return (PortHandler *)(new PortHandlerWindows(port_name));
#endif
}

727
dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp
View File

@ -1,727 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* protocol1_packet_handler.cpp
*
* Created on: 2016. 1. 26.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include <string.h>
#include <stdlib.h>
#include "dynamixel_sdk/protocol1_packet_handler.h"
#define TXPACKET_MAX_LEN (250)
#define RXPACKET_MAX_LEN (250)
///////////////// for Protocol 1.0 Packet /////////////////
#define PKT_HEADER0 0
#define PKT_HEADER1 1
#define PKT_ID 2
#define PKT_LENGTH 3
#define PKT_INSTRUCTION 4
#define PKT_ERROR 4
#define PKT_PARAMETER0 5
///////////////// Protocol 1.0 Error bit /////////////////
#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table)
#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit)
#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table)
#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use.
#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect.
#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque.
#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command.
using namespace dynamixel;
Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler();
Protocol1PacketHandler::Protocol1PacketHandler() { }
void Protocol1PacketHandler::printTxRxResult(int result)
{
switch(result)
{
case COMM_SUCCESS:
printf("[TxRxResult] Communication success.\n");
break;
case COMM_PORT_BUSY:
printf("[TxRxResult] Port is in use!\n");
break;
case COMM_TX_FAIL:
printf("[TxRxResult] Failed transmit instruction packet!\n");
break;
case COMM_RX_FAIL:
printf("[TxRxResult] Failed get status packet from device!\n");
break;
case COMM_TX_ERROR:
printf("[TxRxResult] Incorrect instruction packet!\n");
break;
case COMM_RX_WAITING:
printf("[TxRxResult] Now recieving status packet!\n");
break;
case COMM_RX_TIMEOUT:
printf("[TxRxResult] There is no status packet!\n");
break;
case COMM_RX_CORRUPT:
printf("[TxRxResult] Incorrect status packet!\n");
break;
case COMM_NOT_AVAILABLE:
printf("[TxRxResult] Protocol does not support This function!\n");
break;
default:
break;
}
}
void Protocol1PacketHandler::printRxPacketError(uint8_t error)
{
if (error & ERRBIT_VOLTAGE)
printf("[RxPacketError] Input voltage error!\n");
if (error & ERRBIT_ANGLE)
printf("[RxPacketError] Angle limit error!\n");
if (error & ERRBIT_OVERHEAT)
printf("[RxPacketError] Overheat error!\n");
if (error & ERRBIT_RANGE)
printf("[RxPacketError] Out of range error!\n");
if (error & ERRBIT_CHECKSUM)
printf("[RxPacketError] Checksum error!\n");
if (error & ERRBIT_OVERLOAD)
printf("[RxPacketError] Overload error!\n");
if (error & ERRBIT_INSTRUCTION)
printf("[RxPacketError] Instruction code error!\n");
}
int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket)
{
uint8_t checksum = 0;
uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
uint8_t written_packet_length = 0;
if (port->is_using_)
return COMM_PORT_BUSY;
port->is_using_ = true;
// check max packet length
if (total_packet_length > TXPACKET_MAX_LEN)
{
port->is_using_ = false;
return COMM_TX_ERROR;
}
// make packet header
txpacket[PKT_HEADER0] = 0xFF;
txpacket[PKT_HEADER1] = 0xFF;
// add a checksum to the packet
for (int idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum
checksum += txpacket[idx];
txpacket[total_packet_length - 1] = ~checksum;
// tx packet
port->clearPort();
written_packet_length = port->writePort(txpacket, total_packet_length);
if (total_packet_length != written_packet_length)
{
port->is_using_ = false;
return COMM_TX_FAIL;
}
return COMM_SUCCESS;
}
int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket)
{
int result = COMM_TX_FAIL;
uint8_t checksum = 0;
uint8_t rx_length = 0;
uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
while(true)
{
rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
if (rx_length >= wait_length)
{
uint8_t idx = 0;
// find packet header
for (idx = 0; idx < (rx_length - 1); idx++)
{
if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF)
break;
}
if (idx == 0) // found at the beginning of the packet
{
if (rxpacket[PKT_ID] > 0xFD || // unavailable ID
rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length
rxpacket[PKT_ERROR] >= 0x64) // unavailable Error
{
// remove the first byte in the packet
for (uint8_t s = 0; s < rx_length - 1; s++)
rxpacket[s] = rxpacket[1 + s];
//memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx);
rx_length -= 1;
continue;
}
// re-calculate the exact length of the rx packet
if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1)
{
wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
continue;
}
if (rx_length < wait_length)
{
// check timeout
if (port->isPacketTimeout() == true)
{
if (rx_length == 0)
{
result = COMM_RX_TIMEOUT;
}
else
{
result = COMM_RX_CORRUPT;
}
break;
}
else
{
continue;
}
}
// calculate checksum
for (int i = 2; i < wait_length - 1; i++) // except header, checksum
checksum += rxpacket[i];
checksum = ~checksum;
// verify checksum
if (rxpacket[wait_length - 1] == checksum)
{
result = COMM_SUCCESS;
}
else
{
result = COMM_RX_CORRUPT;
}
break;
}
else
{
// remove unnecessary packets
for (uint8_t s = 0; s < rx_length - idx; s++)
rxpacket[s] = rxpacket[idx + s];
//memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx);
rx_length -= idx;
}
}
else
{
// check timeout
if (port->isPacketTimeout() == true)
{
if (rx_length == 0)
{
result = COMM_RX_TIMEOUT;
}
else
{
result = COMM_RX_CORRUPT;
}
break;
}
}
}
port->is_using_ = false;
return result;
}
// NOT for BulkRead instruction
int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error)
{
int result = COMM_TX_FAIL;
// tx packet
result = txPacket(port, txpacket);
if (result != COMM_SUCCESS)
return result;
// (ID == Broadcast ID && NOT BulkRead) == no need to wait for status packet
// (Instruction == action) == no need to wait for status packet
if ((txpacket[PKT_ID] == BROADCAST_ID && txpacket[PKT_INSTRUCTION] != INST_BULK_READ) ||
(txpacket[PKT_INSTRUCTION] == INST_ACTION))
{
port->is_using_ = false;
return result;
}
// set packet timeout
if (txpacket[PKT_INSTRUCTION] == INST_READ)
{
port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6));
}
else
{
port->setPacketTimeout((uint16_t)6);
}
// rx packet
result = rxPacket(port, rxpacket);
// check txpacket ID == rxpacket ID
if (txpacket[PKT_ID] != rxpacket[PKT_ID])
result = rxPacket(port, rxpacket);
if (result == COMM_SUCCESS && txpacket[PKT_ID] != BROADCAST_ID)
{
if (error != 0)
*error = (uint8_t)rxpacket[PKT_ERROR];
}
return result;
}
int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error)
{
return ping(port, id, 0, error);
}
int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error)
{
int result = COMM_TX_FAIL;
uint8_t txpacket[6] = {0};
uint8_t rxpacket[6] = {0};
if (id >= BROADCAST_ID)
return COMM_NOT_AVAILABLE;
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = 2;
txpacket[PKT_INSTRUCTION] = INST_PING;
result = txRxPacket(port, txpacket, rxpacket, error);
if (result == COMM_SUCCESS && model_number != 0)
{
uint8_t data_read[2] = {0};
result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number
if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]);
}
return result;
}
int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t> &id_list)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::action(PortHandler *port, uint8_t id)
{
uint8_t txpacket[6] = {0};
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = 2;
txpacket[PKT_INSTRUCTION] = INST_ACTION;
return txRxPacket(port, txpacket, 0);
}
int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error)
{
uint8_t txpacket[6] = {0};
uint8_t rxpacket[6] = {0};
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = 2;
txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET;
return txRxPacket(port, txpacket, rxpacket, error);
}
int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length)
{
int result = COMM_TX_FAIL;
uint8_t txpacket[8] = {0};
if (id >= BROADCAST_ID)
return COMM_NOT_AVAILABLE;
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = 4;
txpacket[PKT_INSTRUCTION] = INST_READ;
txpacket[PKT_PARAMETER0+0] = (uint8_t)address;
txpacket[PKT_PARAMETER0+1] = (uint8_t)length;
result = txPacket(port, txpacket);
// set packet timeout
if (result == COMM_SUCCESS)
port->setPacketTimeout((uint16_t)(length+6));
return result;
}
int Protocol1PacketHandler::readRx(PortHandler *port, uint16_t length, uint8_t *data, uint8_t *error)
{
int result = COMM_TX_FAIL;
uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6);
//uint8_t *rxpacket = new uint8_t[length+6];
result = rxPacket(port, rxpacket);
if (result == COMM_SUCCESS)
{
if (error != 0)
{
*error = (uint8_t)rxpacket[PKT_ERROR];
}
for (uint8_t s = 0; s < length; s++)
{
data[s] = rxpacket[PKT_PARAMETER0 + s];
}
//memcpy(data, &rxpacket[PKT_PARAMETER0], length);
}
free(rxpacket);
//delete[] rxpacket;
return result;
}
int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
{
int result = COMM_TX_FAIL;
uint8_t txpacket[8] = {0};
uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6);
if (id >= BROADCAST_ID)
return COMM_NOT_AVAILABLE;
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = 4;
txpacket[PKT_INSTRUCTION] = INST_READ;
txpacket[PKT_PARAMETER0+0] = (uint8_t)address;
txpacket[PKT_PARAMETER0+1] = (uint8_t)length;
result = txRxPacket(port, txpacket, rxpacket, error);
if (result == COMM_SUCCESS)
{
if (error != 0)
{
*error = (uint8_t)rxpacket[PKT_ERROR];
}
for (uint8_t s = 0; s < length; s++)
{
data[s] = rxpacket[PKT_PARAMETER0 + s];
}
//memcpy(data, &rxpacket[PKT_PARAMETER0], length);
}
free(rxpacket);
//delete[] rxpacket;
return result;
}
int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address)
{
return readTx(port, id, address, 1);
}
int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t *data, uint8_t *error)
{
uint8_t data_read[1] = {0};
int result = readRx(port, 1, data_read, error);
if (result == COMM_SUCCESS)
*data = data_read[0];
return result;
}
int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error)
{
uint8_t data_read[1] = {0};
int result = readTxRx(port, id, address, 1, data_read, error);
if (result == COMM_SUCCESS)
*data = data_read[0];
return result;
}
int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address)
{
return readTx(port, id, address, 2);
}
int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint16_t *data, uint8_t *error)
{
uint8_t data_read[2] = {0};
int result = readRx(port, 2, data_read, error);
if (result == COMM_SUCCESS)
*data = DXL_MAKEWORD(data_read[0], data_read[1]);
return result;
}
int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error)
{
uint8_t data_read[2] = {0};
int result = readTxRx(port, id, address, 2, data_read, error);
if (result == COMM_SUCCESS)
*data = DXL_MAKEWORD(data_read[0], data_read[1]);
return result;
}
int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint32_t *data, uint8_t *error)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
{
int result = COMM_TX_FAIL;
uint8_t *txpacket = (uint8_t *)malloc(length+7);
//uint8_t *txpacket = new uint8_t[length+7];
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = length+3;
txpacket[PKT_INSTRUCTION] = INST_WRITE;
txpacket[PKT_PARAMETER0] = (uint8_t)address;
for (uint8_t s = 0; s < length; s++)
txpacket[PKT_PARAMETER0+1+s] = data[s];
//memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
result = txPacket(port, txpacket);
port->is_using_ = false;
free(txpacket);
//delete[] txpacket;
return result;
}
int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
{
int result = COMM_TX_FAIL;
uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7
//uint8_t *txpacket = new uint8_t[length+7];
uint8_t rxpacket[6] = {0};
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = length+3;
txpacket[PKT_INSTRUCTION] = INST_WRITE;
txpacket[PKT_PARAMETER0] = (uint8_t)address;
for (uint8_t s = 0; s < length; s++)
txpacket[PKT_PARAMETER0+1+s] = data[s];
//memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
result = txRxPacket(port, txpacket, rxpacket, error);
free(txpacket);
//delete[] txpacket;
return result;
}
int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data)
{
uint8_t data_write[1] = { data };
return writeTxOnly(port, id, address, 1, data_write);
}
int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error)
{
uint8_t data_write[1] = { data };
return writeTxRx(port, id, address, 1, data_write, error);
}
int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data)
{
uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
return writeTxOnly(port, id, address, 2, data_write);
}
int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error)
{
uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
return writeTxRx(port, id, address, 2, data_write, error);
}
int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
{
int result = COMM_TX_FAIL;
uint8_t *txpacket = (uint8_t *)malloc(length+6);
//uint8_t *txpacket = new uint8_t[length+6];
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = length+3;
txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
txpacket[PKT_PARAMETER0] = (uint8_t)address;
for (uint8_t s = 0; s < length; s++)
txpacket[PKT_PARAMETER0+1+s] = data[s];
//memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
result = txPacket(port, txpacket);
port->is_using_ = false;
free(txpacket);
//delete[] txpacket;
return result;
}
int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
{
int result = COMM_TX_FAIL;
uint8_t *txpacket = (uint8_t *)malloc(length+6);
//uint8_t *txpacket = new uint8_t[length+6];
uint8_t rxpacket[6] = {0};
txpacket[PKT_ID] = id;
txpacket[PKT_LENGTH] = length+3;
txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
txpacket[PKT_PARAMETER0] = (uint8_t)address;
for (uint8_t s = 0; s < length; s++)
txpacket[PKT_PARAMETER0+1+s] = data[s];
//memcpy(&txpacket[PKT_PARAMETER0+1], data, length);
result = txRxPacket(port, txpacket, rxpacket, error);
free(txpacket);
//delete[] txpacket;
return result;
}
int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
{
return COMM_NOT_AVAILABLE;
}
int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
{
int result = COMM_TX_FAIL;
uint8_t *txpacket = (uint8_t *)malloc(param_length+8);
// 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM
//uint8_t *txpacket = new uint8_t[param_length + 8];
txpacket[PKT_ID] = BROADCAST_ID;
txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM
txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
txpacket[PKT_PARAMETER0+0] = start_address;
txpacket[PKT_PARAMETER0+1] = data_length;
for (uint8_t s = 0; s < param_length; s++)
txpacket[PKT_PARAMETER0+2+s] = param[s];
//memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length);
result = txRxPacket(port, txpacket, 0, 0);
free(txpacket);
//delete[] txpacket;
return result;
}
int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)
{
int result = COMM_TX_FAIL;
uint8_t *txpacket = (uint8_t *)malloc(param_length+7);
// 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM
//uint8_t *txpacket = new uint8_t[param_length + 7];
txpacket[PKT_ID] = BROADCAST_ID;
txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM
txpacket[PKT_INSTRUCTION] = INST_BULK_READ;
txpacket[PKT_PARAMETER0+0] = 0x00;
for (uint8_t s = 0; s < param_length; s++)
txpacket[PKT_PARAMETER0+1+s] = param[s];
//memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length);
result = txPacket(port, txpacket);
if (result == COMM_SUCCESS)
{
int wait_length = 0;
for (int i = 0; i < param_length; i += 3)
wait_length += param[i] + 7;
port->setPacketTimeout((uint16_t)wait_length);
}
free(txpacket);
//delete[] txpacket;
return result;
}
int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length)
{
return COMM_NOT_AVAILABLE;
}

1036
dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp
View File

File diff suppressed because it is too large Load Diff

276
dynamixel_sdk/src/dynamixel_sdk_linux/port_handler_linux.cpp
View File

@ -1,276 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: zerom, Leon Ryu Woon Jung */
/*
* port_handler_linux.cpp
*
* Created on: 2016. 1. 26.
*/
#include <stdio.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <termios.h>
#include <time.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <linux/serial.h>
#include "dynamixel_sdk_linux/port_handler_linux.h"
#define LATENCY_TIMER 4 // msec (USB latency timer)
using namespace dynamixel;
PortHandlerLinux::PortHandlerLinux(const char *port_name)
: socket_fd_(-1),
baudrate_(DEFAULT_BAUDRATE_),
packet_start_time_(0.0),
packet_timeout_(0.0),
tx_time_per_byte(0.0)
{
is_using_ = false;
setPortName(port_name);
}
bool PortHandlerLinux::openPort()
{
return setBaudRate(baudrate_);
}
void PortHandlerLinux::closePort()
{
if(socket_fd_ != -1)
close(socket_fd_);
socket_fd_ = -1;
}
void PortHandlerLinux::clearPort()
{
tcflush(socket_fd_, TCIOFLUSH);
}
void PortHandlerLinux::setPortName(const char *port_name)
{
strcpy(port_name_, port_name);
}
char *PortHandlerLinux::getPortName()
{
return port_name_;
}
// TODO: baud number ??
bool PortHandlerLinux::setBaudRate(const int baudrate)
{
int baud = getCFlagBaud(baudrate);
closePort();
if(baud <= 0) // custom baudrate
{
setupPort(B38400);
baudrate_ = baudrate;
return setCustomBaudrate(baudrate);
}
else
{
baudrate_ = baudrate;
return setupPort(baud);
}
}
int PortHandlerLinux::getBaudRate()
{
return baudrate_;
}
int PortHandlerLinux::getBytesAvailable()
{
int bytes_available;
ioctl(socket_fd_, FIONREAD, &bytes_available);
return bytes_available;
}
int PortHandlerLinux::readPort(uint8_t *packet, int length)
{
return read(socket_fd_, packet, length);
}
int PortHandlerLinux::writePort(uint8_t *packet, int length)
{
return write(socket_fd_, packet, length);
}
void PortHandlerLinux::setPacketTimeout(uint16_t packet_length)
{
packet_start_time_ = getCurrentTime();
packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0;
}
void PortHandlerLinux::setPacketTimeout(double msec)
{
packet_start_time_ = getCurrentTime();
packet_timeout_ = msec;
}
bool PortHandlerLinux::isPacketTimeout()
{
if(getTimeSinceStart() > packet_timeout_)
{
packet_timeout_ = 0;
return true;
}
return false;
}
double PortHandlerLinux::getCurrentTime()
{
struct timespec tv;
clock_gettime( CLOCK_REALTIME, &tv);
return ((double)tv.tv_sec*1000.0 + (double)tv.tv_nsec*0.001*0.001);
}
double PortHandlerLinux::getTimeSinceStart()
{
double time;
time = getCurrentTime() - packet_start_time_;
if(time < 0.0)
packet_start_time_ = getCurrentTime();
return time;
}
bool PortHandlerLinux::setupPort(int cflag_baud)
{
struct termios newtio;
socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK);
if(socket_fd_ < 0)
{
printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n");
return false;
}
bzero(&newtio, sizeof(newtio)); // clear struct for new port settings
newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 0;
// clean the buffer and activate the settings for the port
tcflush(socket_fd_, TCIFLUSH);
tcsetattr(socket_fd_, TCSANOW, &newtio);
tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0;
return true;
}
bool PortHandlerLinux::setCustomBaudrate(int speed)
{
// try to set a custom divisor
struct serial_struct ss;
if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0)
{
printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n");
return false;
}
ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST;
ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed;
int closest_speed = ss.baud_base / ss.custom_divisor;
if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100)
{
printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed);
return false;
}
if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0)
{
printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n");
return false;
}
tx_time_per_byte = (1000.0 / (double)speed) * 10.0;
return true;
}
int PortHandlerLinux::getCFlagBaud(int baudrate)
{
switch(baudrate)
{
case 9600:
return B9600;
case 19200:
return B19200;
case 38400:
return B38400;
case 57600:
return B57600;
case 115200:
return B115200;
case 230400:
return B230400;
case 460800:
return B460800;
case 500000:
return B500000;
case 576000:
return B576000;
case 921600:
return B921600;
case 1000000:
return B1000000;
case 1152000:
return B1152000;
case 1500000:
return B1500000;
case 2000000:
return B2000000;
case 2500000:
return B2500000;
case 3000000:
return B3000000;
case 3500000:
return B3500000;
case 4000000:
return B4000000;
default:
return -1;
}
}

245
dynamixel_sdk/src/dynamixel_sdk_windows/port_handler_windows.cpp
View File

@ -1,245 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: Leon Ryu Woon Jung */
/*
* PortHandlerWindows.cpp
*
* Created on: 2016. 4. 06.
*/
#if defined(_WIN32) || defined(_WIN64)
#define WINDLLEXPORT
#endif
#include "dynamixel_sdk_windows/port_handler_windows.h"
#include <stdio.h>
#include <string.h>
#include <time.h>
#define LATENCY_TIMER 16 // msec (USB latency timer)
using namespace dynamixel;
PortHandlerWindows::PortHandlerWindows(const char *port_name)
: serial_handle_(INVALID_HANDLE_VALUE),
baudrate_(DEFAULT_BAUDRATE_),
packet_start_time_(0.0),
packet_timeout_(0.0),
tx_time_per_byte_(0.0)
{
is_using_ = false;
char buffer[15];
sprintf_s(buffer, sizeof(buffer), "\\\\.\\", port_name);
setPortName(port_name);
}
bool PortHandlerWindows::openPort()
{
return setBaudRate(baudrate_);
}
void PortHandlerWindows::closePort()
{
if (serial_handle_ != INVALID_HANDLE_VALUE)
{
CloseHandle(serial_handle_);
serial_handle_ = INVALID_HANDLE_VALUE;
}
}
void PortHandlerWindows::clearPort()
{
PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR);
}
void PortHandlerWindows::setPortName(const char *port_name)
{
strcpy_s(port_name_, sizeof(port_name_), port_name);
}
char *PortHandlerWindows::getPortName()
{
return port_name_;
}
bool PortHandlerWindows::setBaudRate(const int baudrate)
{
closePort();
baudrate_ = baudrate;
return setupPort(baudrate);
}
int PortHandlerWindows::getBaudRate()
{
return baudrate_;
}
int PortHandlerWindows::getBytesAvailable()
{
DWORD retbyte = 2;
BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL);
printf("%d", (int)res);
return (int)retbyte;
}
int PortHandlerWindows::readPort(uint8_t *packet, int length)
{
DWORD dwRead = 0;
if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE)
return -1;
return (int)dwRead;
}
int PortHandlerWindows::writePort(uint8_t *packet, int length)
{
DWORD dwWrite = 0;
if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE)
return -1;
return (int)dwWrite;
}
void PortHandlerWindows::setPacketTimeout(uint16_t packet_length)
{
packet_start_time_ = getCurrentTime();
packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0;
}
void PortHandlerWindows::setPacketTimeout(double msec)
{
packet_start_time_ = getCurrentTime();
packet_timeout_ = msec;
}
bool PortHandlerWindows::isPacketTimeout()
{
if (getTimeSinceStart() > packet_timeout_)
{
packet_timeout_ = 0;
return true;
}
return false;
}
double PortHandlerWindows::getCurrentTime()
{
QueryPerformanceCounter(&counter_);
QueryPerformanceFrequency(&freq_);
return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0;
}
double PortHandlerWindows::getTimeSinceStart()
{
double time;
time = getCurrentTime() - packet_start_time_;
if (time < 0.0) packet_start_time_ = getCurrentTime();
return time;
}
bool PortHandlerWindows::setupPort(int baudrate)
{
DCB dcb;
COMMTIMEOUTS timeouts;
DWORD dwError;
closePort();
serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);
if (serial_handle_ == INVALID_HANDLE_VALUE)
{
printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n");
return false;
}
dcb.DCBlength = sizeof(DCB);
if (GetCommState(serial_handle_, &dcb) == FALSE)
goto DXL_HAL_OPEN_ERROR;
// Set baudrate
dcb.BaudRate = (DWORD)baudrate;
dcb.ByteSize = 8; // Data bit = 8bit
dcb.Parity = NOPARITY; // No parity
dcb.StopBits = ONESTOPBIT; // Stop bit = 1
dcb.fParity = NOPARITY; // No Parity check
dcb.fBinary = 1; // Binary mode
dcb.fNull = 0; // Get Null byte
dcb.fAbortOnError = 0;
dcb.fErrorChar = 0;
// Not using XOn/XOff
dcb.fOutX = 0;
dcb.fInX = 0;
// Not using H/W flow control
dcb.fDtrControl = DTR_CONTROL_DISABLE;
dcb.fRtsControl = RTS_CONTROL_DISABLE;
dcb.fDsrSensitivity = 0;
dcb.fOutxDsrFlow = 0;
dcb.fOutxCtsFlow = 0;
if (SetCommState(serial_handle_, &dcb) == FALSE)
goto DXL_HAL_OPEN_ERROR;
if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event
goto DXL_HAL_OPEN_ERROR;
if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx)
goto DXL_HAL_OPEN_ERROR;
if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer
goto DXL_HAL_OPEN_ERROR;
if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE)
goto DXL_HAL_OPEN_ERROR;
if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE)
goto DXL_HAL_OPEN_ERROR;
// Timeout (Not using timeout)
// Immediatly return
timeouts.ReadIntervalTimeout = 0;
timeouts.ReadTotalTimeoutMultiplier = 0;
timeouts.ReadTotalTimeoutConstant = 1; // must not be zero.
timeouts.WriteTotalTimeoutMultiplier = 0;
timeouts.WriteTotalTimeoutConstant = 0;
if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE)
goto DXL_HAL_OPEN_ERROR;
tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0;
return true;
DXL_HAL_OPEN_ERROR:
closePort();
return false;
}