diff --git a/c/include/dynamixel_sdk/packet_handler.h b/c/include/dynamixel_sdk/packet_handler.h index 66bb29e2..287dd460 100644 --- a/c/include/dynamixel_sdk/packet_handler.h +++ b/c/include/dynamixel_sdk/packet_handler.h @@ -79,8 +79,6 @@ typedef struct uint8_t *broadcast_ping_id_list; }PacketData; -PacketData *packetData; - WINDECLSPEC void packetHandler (); WINDECLSPEC const char *getTxRxResult (int protocol_version, int result); diff --git a/c/include/dynamixel_sdk/port_handler.h b/c/include/dynamixel_sdk/port_handler.h index 77462484..5186dfac 100644 --- a/c/include/dynamixel_sdk/port_handler.h +++ b/c/include/dynamixel_sdk/port_handler.h @@ -42,11 +42,6 @@ #include "robotis_def.h" -static const int DEFAULT_BAUDRATE = 57600; - -int g_used_port_num; -uint8_t *g_is_using; - WINDECLSPEC int portHandler (const char *port_name); WINDECLSPEC uint8_t openPort (int port_num); diff --git a/c/src/dynamixel_sdk/group_bulk_read.c b/c/src/dynamixel_sdk/group_bulk_read.c index 900e0b94..48f926c8 100644 --- a/c/src/dynamixel_sdk/group_bulk_read.c +++ b/c/src/dynamixel_sdk/group_bulk_read.c @@ -28,6 +28,8 @@ #include "group_bulk_read.h" #endif +extern PacketData *packetData; // defined in packet_handler.c + typedef struct { uint8_t id; diff --git a/c/src/dynamixel_sdk/group_bulk_write.c b/c/src/dynamixel_sdk/group_bulk_write.c index d602cc85..eac1c034 100644 --- a/c/src/dynamixel_sdk/group_bulk_write.c +++ b/c/src/dynamixel_sdk/group_bulk_write.c @@ -27,6 +27,8 @@ #include "group_bulk_write.h" #endif +extern PacketData *packetData; // defined in packet_handler.c + typedef struct { uint8_t id; diff --git a/c/src/dynamixel_sdk/group_sync_read.c b/c/src/dynamixel_sdk/group_sync_read.c index 9b0dbc3c..feb16c61 100644 --- a/c/src/dynamixel_sdk/group_sync_read.c +++ b/c/src/dynamixel_sdk/group_sync_read.c @@ -27,6 +27,8 @@ #include "group_sync_read.h" #endif +extern PacketData *packetData; // defined in packet_handler.c + typedef struct { uint8_t id; diff --git a/c/src/dynamixel_sdk/group_sync_write.c b/c/src/dynamixel_sdk/group_sync_write.c index df7cfa62..26cc1522 100644 --- a/c/src/dynamixel_sdk/group_sync_write.c +++ b/c/src/dynamixel_sdk/group_sync_write.c @@ -27,6 +27,8 @@ #include "group_sync_write.h" #endif +extern PacketData *packetData; // defined in packet_handler.c + typedef struct { uint8_t id; diff --git a/c/src/dynamixel_sdk/packet_handler.c b/c/src/dynamixel_sdk/packet_handler.c index fe4c0083..49de9395 100644 --- a/c/src/dynamixel_sdk/packet_handler.c +++ b/c/src/dynamixel_sdk/packet_handler.c @@ -33,6 +33,9 @@ #include "protocol2_packet_handler.h" #endif +PacketData *packetData; +extern int g_used_port_num; // defined in port_handler.c + void packetHandler() { int port_num; diff --git a/c/src/dynamixel_sdk/port_handler.c b/c/src/dynamixel_sdk/port_handler.c index 7f46ade2..45c39861 100644 --- a/c/src/dynamixel_sdk/port_handler.c +++ b/c/src/dynamixel_sdk/port_handler.c @@ -91,3 +91,8 @@ void setPacketTimeoutMSec(int port_num, double msec) { setPacketTimeoutMSecWi uint8_t isPacketTimeout (int port_num) { return isPacketTimeoutWindows(port_num); } #endif + +static const int DEFAULT_BAUDRATE = 57600; + +int g_used_port_num; +uint8_t *g_is_using; diff --git a/c/src/dynamixel_sdk/port_handler_linux.c b/c/src/dynamixel_sdk/port_handler_linux.c index 965e64d1..4997eb31 100644 --- a/c/src/dynamixel_sdk/port_handler_linux.c +++ b/c/src/dynamixel_sdk/port_handler_linux.c @@ -89,6 +89,9 @@ typedef struct }PortData; static PortData *portData; +extern int DEFAULT_BAUDRATE; // defined in port_handler.c +extern int g_used_port_num; // defined in port_handler.c +extern uint8_t *g_is_using; // defined in port_handler.c int portHandlerLinux(const char *port_name) { diff --git a/c/src/dynamixel_sdk/port_handler_mac.c b/c/src/dynamixel_sdk/port_handler_mac.c index 458dd4f7..cc22d653 100644 --- a/c/src/dynamixel_sdk/port_handler_mac.c +++ b/c/src/dynamixel_sdk/port_handler_mac.c @@ -56,6 +56,9 @@ typedef struct }PortData; static PortData *portData; +extern int DEFAULT_BAUDRATE; // defined in port_handler.c +extern int g_used_port_num; // defined in port_handler.c +extern uint8_t *g_is_using; // defined in port_handler.c int portHandlerMac(const char *port_name) { diff --git a/c/src/dynamixel_sdk/port_handler_windows.c b/c/src/dynamixel_sdk/port_handler_windows.c index e8d52adc..700a9f94 100644 --- a/c/src/dynamixel_sdk/port_handler_windows.c +++ b/c/src/dynamixel_sdk/port_handler_windows.c @@ -48,6 +48,9 @@ typedef struct }PortData; static PortData *portData; +extern int DEFAULT_BAUDRATE; // defined in port_handler.c +extern int g_used_port_num; // defined in port_handler.c +extern uint8_t *g_is_using; // defined in port_handler.c int portHandlerWindows(const char *port_name) { diff --git a/c/src/dynamixel_sdk/protocol1_packet_handler.c b/c/src/dynamixel_sdk/protocol1_packet_handler.c index 5e1eee76..74c89bee 100644 --- a/c/src/dynamixel_sdk/protocol1_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol1_packet_handler.c @@ -49,6 +49,9 @@ #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. +extern PacketData *packetData; // defined in packet_handler.c +extern uint8_t *g_is_using; // defined in port_handler.c + const char *getTxRxResult1(int result) { switch (result) @@ -701,7 +704,7 @@ void regWriteTxOnly1(int port_num, uint8_t id, uint16_t address, uint16_t length } txPacket1(port_num); - + g_is_using[port_num] = False; } diff --git a/c/src/dynamixel_sdk/protocol2_packet_handler.c b/c/src/dynamixel_sdk/protocol2_packet_handler.c index 3f403482..1387db44 100644 --- a/c/src/dynamixel_sdk/protocol2_packet_handler.c +++ b/c/src/dynamixel_sdk/protocol2_packet_handler.c @@ -58,6 +58,9 @@ #define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. +extern PacketData *packetData; // defined in packet_handler.c +extern uint8_t *g_is_using; // defined in port_handler.c + const char *getTxRxResult2(int result) { switch (result) @@ -251,13 +254,13 @@ void addStuffing(uint8_t *packet) uint16_t i; uint16_t packet_length_before_crc; uint16_t out_index, in_index; - + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); int packet_length_out = packet_length_in; - + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD return; - + packet_length_before_crc = packet_length_in - 2; for (i = 3; i < packet_length_before_crc; i++) { @@ -265,10 +268,10 @@ void addStuffing(uint8_t *packet) if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) packet_length_out++; } - + if (packet_length_in == packet_length_out) // no stuffing required return; - + out_index = packet_length_out + 6 - 2; // last index before crc in_index = packet_length_in + 6 - 2; // last index before crc while (out_index != in_index) @@ -548,7 +551,7 @@ uint16_t pingGetModelNum2(int port_num, uint8_t id) printf("[PingGetModeNum] memory allocation failed.. \n"); return COMM_TX_FAIL; } - + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -749,7 +752,7 @@ void clearMultiTurn2(int port_num, uint8_t id) printf("[ClearMultiTurn] memory allocation failed..\n"); return; } - + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 8; packetData[port_num].tx_packet[PKT_LENGTH_H] = 0; @@ -772,7 +775,7 @@ void factoryReset2(int port_num, uint8_t id, uint8_t option) printf("[FactoryReset] memory allocation failed..\n"); return; } - + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = 4; packetData[port_num].tx_packet[PKT_LENGTH_H] = 0; @@ -792,7 +795,7 @@ void readTx2(int port_num, uint8_t id, uint16_t address, uint16_t length) printf("[ReadTx] memory allocation failed..\n"); return; } - + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -828,7 +831,7 @@ void readRx2(int port_num, uint16_t length) printf("[ReadRx] memory allocation failed..\n"); return; } - + rxPacket2(port_num); if (packetData[port_num].communication_result == COMM_SUCCESS) { @@ -854,7 +857,7 @@ void readTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) printf("[ReadTxRx] memory allocation failed..\n"); return; } - + if (id >= BROADCAST_ID) { packetData[port_num].communication_result = COMM_NOT_AVAILABLE; @@ -1032,7 +1035,7 @@ void writeTxRx2(int port_num, uint8_t id, uint16_t address, uint16_t length) printf("[WriteTxRx] memory allocation failed..\n"); return; } - + packetData[port_num].tx_packet[PKT_ID] = id; packetData[port_num].tx_packet[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); packetData[port_num].tx_packet[PKT_LENGTH_H] = DXL_HIBYTE(length + 5);