From 59c93046802ceafc2a5c9c5986d85fd322022da7 Mon Sep 17 00:00:00 2001 From: Kei Date: Thu, 26 Dec 2019 17:14:12 +0900 Subject: [PATCH] Updated syncRead/Write class for testing. (#23) --- .../bulk_read_write_raw.ino | 2 +- .../sync_read_write/sync_read_write.ino | 163 +++++++++ .../sync_read_write_raw.ino | 2 +- src/utility/master.h | 315 ++++++++++++++---- 4 files changed, 421 insertions(+), 61 deletions(-) create mode 100644 examples/advanced/sync_read_write/sync_read_write.ino diff --git a/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino b/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino index d5797e9..a4b1453 100644 --- a/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino +++ b/examples/advanced/bulk_read_write_raw/bulk_read_write_raw.ino @@ -222,7 +222,7 @@ void loop() { DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(br_data_xel_2.present_position); }else{ DEBUG_SERIAL.print("[BulkRead] Fail, Lib error code: "); - DEBUG_SERIAL.print(dxl.getLastLibErrCode()); + DEBUG_SERIAL.println(dxl.getLastLibErrCode()); } DEBUG_SERIAL.println("======================================================="); diff --git a/examples/advanced/sync_read_write/sync_read_write.ino b/examples/advanced/sync_read_write/sync_read_write.ino new file mode 100644 index 0000000..b612d61 --- /dev/null +++ b/examples/advanced/sync_read_write/sync_read_write.ino @@ -0,0 +1,163 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +#include + +// Please modify it to suit your hardware. +#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield + #include + SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX + #define DXL_SERIAL Serial + #define DEBUG_SERIAL soft_serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield + #define DXL_SERIAL Serial + #define DEBUG_SERIAL SerialUSB + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL SerialUSB + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit. + #define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board) + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board) +#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit. + // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. + // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 + #define DXL_SERIAL Serial3 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. +#else // Other boards when using DynamixelShield + #define DXL_SERIAL Serial1 + #define DEBUG_SERIAL Serial + const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN +#endif + + + +const uint8_t DXL_ID_CNT = 2; +const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2}; +const uint16_t user_pkt_buf_cap = 128; +uint8_t user_pkt_buf[user_pkt_buf_cap]; + +const uint16_t SR_START_ADDR = 126; +const uint16_t SR_ADDR_LEN = 10; //2+4+4 +const uint16_t SW_START_ADDR = 104; //Goal velocity +const uint16_t SW_ADDR_LEN = 4; +typedef struct sr_data{ + int16_t present_current; + int32_t present_velocity; + int32_t present_position; +} __attribute__((packed)) sr_data_t; +typedef struct sw_data{ + int32_t goal_velocity; +} __attribute__((packed)) sw_data_t; + + +sr_data_t sr_data[DXL_ID_CNT]; +sw_data_t sw_data[DXL_ID_CNT]; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); +DYNAMIXEL::SyncRead dxl_sr(dxl); +DYNAMIXEL::SyncWrite dxl_sw(dxl); + + +void setup() { + // put your setup code here, to run once: + uint8_t i; + pinMode(LED_BUILTIN, OUTPUT); + DEBUG_SERIAL.begin(115200); + dxl.begin(57600); + + for(i=0; i= 200){ + sw_data[i].goal_velocity = 0; + } + } + dxl_sw.updateParamData(); + + DEBUG_SERIAL.print("\n>>>>>> Sync Instruction Test : "); + DEBUG_SERIAL.println(try_count++); + // Send syncWrite + if(dxl_sw.sendPacket() == true){ + DEBUG_SERIAL.println("[SyncWrite] Success"); + for(i=0; igoal_velocity); + } + }else{ + DEBUG_SERIAL.print("[SyncWrite] Fail, Lib error code: "); + DEBUG_SERIAL.print(dxl.getLastLibErrCode()); + } + DEBUG_SERIAL.println(); + + delay(250); + + // Send syncRead & Receive data + recv_cnt = dxl_sr.sendPacket(); + if(recv_cnt > 0){ + DEBUG_SERIAL.print("[SyncRead] Success, Received ID Count: "); + DEBUG_SERIAL.println(recv_cnt); + for(i=0; ipresent_current); + DEBUG_SERIAL.print("\t Present Velocity: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_velocity); + DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(dxl_sr.getDataPtr(id)->present_position); + } + }else{ + DEBUG_SERIAL.print("[SyncRead] Fail, Lib error code: "); + DEBUG_SERIAL.println(dxl.getLastLibErrCode()); + } + DEBUG_SERIAL.println("======================================================="); + + digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); + delay(750); +} + + + + diff --git a/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino b/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino index fb56d4a..253b431 100644 --- a/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino +++ b/examples/advanced/sync_read_write_raw/sync_read_write_raw.ino @@ -204,7 +204,7 @@ void loop() { } }else{ DEBUG_SERIAL.print("[SyncRead] Fail, Lib error code: "); - DEBUG_SERIAL.print(dxl.getLastLibErrCode()); + DEBUG_SERIAL.println(dxl.getLastLibErrCode()); } DEBUG_SERIAL.println("======================================================="); diff --git a/src/utility/master.h b/src/utility/master.h index 1a13317..788047e 100644 --- a/src/utility/master.h +++ b/src/utility/master.h @@ -36,8 +36,6 @@ typedef struct InfoFromPing{ uint8_t firmware_version; } InfoFromPing_t; - -#if 1 typedef struct InfoSyncBulkBuffer{ uint8_t* p_buf; uint16_t buf_capacity; @@ -105,9 +103,6 @@ typedef struct InfoBulkWriteInst{ InfoSyncBulkBuffer_t packet; } __attribute__((packed)) InfoBulkWriteInst_t; -#endif - - class Master @@ -167,40 +162,10 @@ class Master bool factoryReset(uint8_t id, uint8_t option, uint32_t timeout_ms = 10); bool reboot(uint8_t id, uint32_t timeout_ms = 10); bool clear(uint8_t id, uint8_t option, uint32_t ex_option, uint32_t timeout_ms = 10); - - -//TODO -#if 1 uint8_t syncRead(InfoSyncReadInst_t* p_info, uint32_t timeout_ms = 10); bool syncWrite(InfoSyncWriteInst_t* p_info); - uint8_t bulkRead(InfoBulkReadInst_t* p_info, uint32_t timeout_ms = 10); bool bulkWrite(InfoBulkWriteInst_t* p_info); -#else - /* Easy functions for Sync Read */ - bool beginSetupSyncRead(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info = nullptr); - bool addSyncReadID(uint8_t id, InfoSyncBulkInst_t *p_sync_info = nullptr); - bool endSetupSyncRead(InfoSyncBulkInst_t *p_sync_info = nullptr); - uint8_t doSyncRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint8_t *p_err_list, uint8_t err_list_size, InfoSyncBulkInst_t *p_sync_info = nullptr); - - /* Easy functions for Sync Write */ - bool beginSetupSyncWrite(uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_sync_info = nullptr); - bool addSyncWriteData(uint8_t id, uint8_t *p_data, InfoSyncBulkInst_t *p_sync_info = nullptr); - bool endSetupSyncWrite(InfoSyncBulkInst_t *p_sync_info = nullptr); - bool doSyncWrite(InfoSyncBulkInst_t *p_sync_info = nullptr); - - /* Easy functions for Bulk Read */ - bool beginSetupBulkRead(InfoSyncBulkInst_t *p_bulk_info = nullptr); - bool addBulkReadID(uint8_t id, uint16_t addr, uint16_t addr_len, InfoSyncBulkInst_t *p_bulk_info = nullptr); - bool endSetupBulkRead(InfoSyncBulkInst_t *p_bulk_info = nullptr); - uint8_t doBulkRead(uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint8_t *p_err_list, uint8_t err_list_size, InfoSyncBulkInst_t *p_bulk_info = nullptr); - - /* Easy functions for Bulk Write */ - bool beginSetupBulkWrite(InfoSyncBulkInst_t *p_bulk_info = nullptr); - bool addBulkWriteData(uint8_t id, uint16_t addr, uint16_t addr_len, uint8_t *p_data, InfoSyncBulkInst_t *p_bulk_info = nullptr); - bool endSetupBulkWrite(InfoSyncBulkInst_t *p_bulk_info = nullptr); - bool doBulkWrite(InfoSyncBulkInst_t *p_bulk_info = nullptr); -#endif uint8_t getLastStatusPacketError() const; @@ -223,40 +188,272 @@ class Master InfoToParseDXLPacket_t info_rx_packet_; DXLLibErrorCode_t last_lib_err_; - }; -} +}; + + + +template +class SyncWrite +{ + public: + SyncWrite(Master &dxl_master) + : dxl_master_(dxl_master) + { + setPacketBuffer(dxl_master_.getPacketBuffer(), dxl_master_.getPacketBufferCapacity()); + memset(&sw_info_, 0, sizeof(sw_info_)); + memset(xel_infos_, 0, sizeof(xel_infos_)); + sw_info_.addr = START_ADDR; + sw_info_.addr_length = sizeof(T); + sw_info_.p_xels = xel_infos_; + } + + bool setPacketBuffer(uint8_t* p_buf, uint16_t buf_capacity) + { + bool ret = false; + if(p_buf != nullptr && buf_capacity > 0){ + sw_info_.packet.p_buf = p_buf; + sw_info_.packet.buf_capacity = buf_capacity; + ret = true; + } + return ret; + } + + uint8_t* getPacketBuffer() const + { + return sw_info_.packet.p_buf; + } + + bool addParam(uint8_t id, T &data) + { + bool ret = false; + uint8_t i; + + for(i=0; i= XEL_CNT_MAX){ + return 0xFF; + } + return xel_infos_[index].id; + } + + T* getDataPtr(uint8_t id) + { + T* p_ret = nullptr; + uint8_t i; + + for(i=0; i -// class SyncWriteMaster -// { -// public: +template +class SyncRead +{ + public: + SyncRead(Master &dxl_master) + : dxl_master_(dxl_master) + { + setPacketBuffer(dxl_master_.getPacketBuffer(), dxl_master_.getPacketBufferCapacity()); + memset(&sr_info_, 0, sizeof(sr_info_)); + memset(xel_infos_, 0, sizeof(xel_infos_)); + sr_info_.addr = START_ADDR; + sr_info_.addr_length = sizeof(T); + sr_info_.p_xels = xel_infos_; + } + + bool setPacketBuffer(uint8_t* p_buf, uint16_t buf_capacity) + { + bool ret = false; + if(p_buf != nullptr && buf_capacity > 0){ + sr_info_.packet.p_buf = p_buf; + sr_info_.packet.buf_capacity = buf_capacity; + ret = true; + } + return ret; + } + + uint8_t* getPacketBuffer() const + { + return sr_info_.packet.p_buf; + } + + bool addParam(uint8_t id, T &recv_data) + { + bool ret = false; + uint8_t i; + + for(i=0; i= XEL_CNT_MAX){ + return 0xFF; + } + return xel_infos_[index].id; + } + + uint8_t getError(uint8_t id) const + { + uint8_t i, ret = 0; + + for(i=0; i