Skip to content

Commit

Permalink
Merge pull request #129 from ROBOTIS-GIT/develop
Browse files Browse the repository at this point in the history 
Develop
  • Loading branch information
@honghyun79
honghyun79 authored Jul 28, 2023
2 parents 7f4400a + 5dac7b1 commit 84b29f1
Show file tree
Hide file tree
Showing 10 changed files with 1,482 additions and 1,141 deletions.
250 changes: 250 additions & 0 deletions examples/advanced/fast_bulk_read/fast_bulk_read.ino
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,250 @@
/*******************************************************************************
* 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.
*******************************************************************************/

// Author: Honghyun Kim

#include <Dynamixel2Arduino.h>

// Please modify it to suit your hardware.
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
#define DXL_SERIAL Serial
#define DEBUG_SERIAL soft_serial
const int 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 int 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 int 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 int 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 int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
#elif defined(ARDUINO_OpenRB) // When using OpenRB-150
//OpenRB does not require the DIR control pin.
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = -1;
#else // Other boards when using DynamixelShield
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#endif


/* fastBulkRead
DYNAMIXEL PROTOCOL 1.0 does NOT support Fast Bulk Read feature.
Structures containing the necessary information to process the 'fastBulkRead' packet.
struct XELInfoBulkRead_t {
uint16_t addr;
uint16_t addr_length;
uint8_t *p_recv_buf;
uint8_t id;
uint8_t error;
} __attribute__((packed));
struct InfoBulkReadInst_t {
XELInfoBulkRead_t* p_xels;
uint8_t xel_count;
bool is_info_changed;
InfoSyncBulkBuffer_t packet;
} __attribute__((packed));
*/

/* bulkWrite
DYNAMIXEL PROTOCOL 1.0 does NOT support Bulk Write feature.
Structures containing the necessary information to process the 'bulkWrite' packet.
struct XELInfoBulkWrite_t {
uint16_t addr;
uint16_t addr_length;
uint8_t* p_data;
uint8_t id;
} __attribute__((packed));
struct InfoBulkWriteInst_t {
XELInfoBulkWrite_t* p_xels;
uint8_t xel_count;
bool is_info_changed;
InfoSyncBulkBuffer_t packet;
} __attribute__((packed));
*/


const uint8_t DXL_1_ID = 1;
const uint8_t DXL_2_ID = 2;
const uint8_t DXL_ID_CNT = 2;
const uint16_t BUF_SIZE = 128;
uint8_t user_pkt_buf[BUF_SIZE];

struct BrDataXel1 {
int16_t present_current;
int32_t present_velocity;
} __attribute__((packed));

struct BrDataXel2 {
int32_t present_position;
} __attribute__((packed));

struct BwDataXel1 {
int32_t goal_velocity;
} __attribute__((packed));

struct BwDataXel2 {
int32_t goal_position;
} __attribute__((packed));


BrDataXel1 br_data_xel_1;
BrDataXel2 br_data_xel_2;
DYNAMIXEL::InfoBulkReadInst_t br_infos;
DYNAMIXEL::XELInfoBulkRead_t info_xels_br[DXL_ID_CNT];

BwDataXel1 bw_data_xel_1;
BwDataXel2 bw_data_xel_2;
DYNAMIXEL::InfoBulkWriteInst_t bw_infos;
DYNAMIXEL::XELInfoBulkWrite_t info_xels_bw[DXL_ID_CNT];

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use Control table item names
using namespace ControlTableItem;

void setup() {
pinMode(LED_BUILTIN, OUTPUT);
DEBUG_SERIAL.begin(115200);
dxl.begin(57600);

dxl.torqueOff(DXL_1_ID);
dxl.torqueOff(DXL_2_ID);
dxl.setOperatingMode(DXL_1_ID, OP_VELOCITY);
dxl.setOperatingMode(DXL_2_ID, OP_POSITION);
dxl.torqueOn(DXL_1_ID);
dxl.torqueOn(DXL_2_ID);

// Fill the members of structure to fastSyncRead using external user packet buffer
br_infos.packet.p_buf = user_pkt_buf;
br_infos.packet.buf_capacity = BUF_SIZE;
br_infos.packet.is_completed = false;
br_infos.p_xels = info_xels_br;

info_xels_br[0].id = DXL_1_ID;
info_xels_br[0].addr = 126; // Present Current of X serise.
info_xels_br[0].addr_length = 2 + 4; // Present Current + Present Velocity
info_xels_br[0].p_recv_buf = (uint8_t*)&br_data_xel_1;

info_xels_br[1].id = DXL_2_ID;
info_xels_br[1].addr = 132; // Present Position of X serise.
info_xels_br[1].addr_length = 4; // Present Position + Present Velocity
info_xels_br[1].p_recv_buf = (uint8_t*)&br_data_xel_2;

br_infos.xel_count = DXL_ID_CNT;
br_infos.is_info_changed = true;

// Fill the members of structure to syncWrite using internal packet buffer
bw_infos.packet.p_buf = nullptr;
bw_infos.packet.is_completed = false;
bw_infos.p_xels = info_xels_bw;

bw_data_xel_1.goal_velocity = 0;
info_xels_bw[0].id = DXL_1_ID;
info_xels_bw[0].addr = 104; // Goal Velocity of X serise.
info_xels_bw[0].addr_length = 4; // Goal Velocity
info_xels_bw[0].p_data = (uint8_t*)&bw_data_xel_1;

bw_data_xel_2.goal_position = 0;
info_xels_bw[1].id = DXL_2_ID;
info_xels_bw[1].addr = 116; // Goal Position of X serise.
info_xels_bw[1].addr_length = 4; // Goal Position
info_xels_bw[1].p_data = (uint8_t*)&bw_data_xel_2;

bw_infos.xel_count = DXL_ID_CNT;
bw_infos.is_info_changed = true;
}

void loop() {
static uint32_t try_count = 0;

bw_data_xel_1.goal_velocity += 5;
if (bw_data_xel_1.goal_velocity >= 200)
bw_data_xel_1.goal_velocity = 0;
bw_data_xel_2.goal_position += 255;
if (bw_data_xel_2.goal_position >= 1023)
bw_data_xel_2.goal_position = 0;
bw_infos.is_info_changed = true;

DEBUG_SERIAL.print("\n>>>>>> Bulk Instruction Test : ");
DEBUG_SERIAL.println(try_count++);
if (true == dxl.bulkWrite(&bw_infos)) {
DEBUG_SERIAL.println("[BulkWrite] Success");
DEBUG_SERIAL.print(" ID: ");
DEBUG_SERIAL.println(bw_infos.p_xels[0].id);
DEBUG_SERIAL.print("\t Goal Velocity: ");
DEBUG_SERIAL.println(bw_data_xel_1.goal_velocity);
DEBUG_SERIAL.print(" ID: ");
DEBUG_SERIAL.println(bw_infos.p_xels[1].id);
DEBUG_SERIAL.print("\t Goal Position: ");
DEBUG_SERIAL.println(bw_data_xel_2.goal_position);
} else {
DEBUG_SERIAL.print("[BulkWrite] Fail, Lib error code: ");
DEBUG_SERIAL.print(dxl.getLastLibErrCode());
}
DEBUG_SERIAL.println();

delay(250);

// Transmit predefined fastBulkRead instruction packet
// and receive a status packet from each DYNAMIXEL
uint8_t recv_cnt = dxl.fastBulkRead(&br_infos);
if (recv_cnt > 0) {
DEBUG_SERIAL.print("[fastBulkRead] Success, Received ID Count: ");
DEBUG_SERIAL.println(recv_cnt);

DEBUG_SERIAL.print(" ID: ");
DEBUG_SERIAL.print(br_infos.p_xels[0].id);
DEBUG_SERIAL.print(", Error: ");
DEBUG_SERIAL.println(br_infos.p_xels[0].error);
DEBUG_SERIAL.print("\t Present Current: ");
DEBUG_SERIAL.println(br_data_xel_1.present_current);
DEBUG_SERIAL.print("\t Present Velocity: ");
DEBUG_SERIAL.println(br_data_xel_1.present_velocity);

DEBUG_SERIAL.print(" ID: ");
DEBUG_SERIAL.print(br_infos.p_xels[1].id);
DEBUG_SERIAL.print(", Error: ");
DEBUG_SERIAL.println(br_infos.p_xels[1].error);
DEBUG_SERIAL.print("\t Present Position: ");
DEBUG_SERIAL.println(br_data_xel_2.present_position);
} else {
DEBUG_SERIAL.print("[fastBulkRead] Fail, Lib error code: ");
DEBUG_SERIAL.println(dxl.getLastLibErrCode());
}
DEBUG_SERIAL.println("=======================================================");

digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
delay(750);
}
46 changes: 24 additions & 22 deletions examples/advanced/fast_sync_read/fast_sync_read.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,39 +59,39 @@
DYNAMIXEL PROTOCOL 1.0 does NOT support Sync Read feature.
Structures containing the necessary information to process the 'fastSyncRead' packet.
typedef struct XELInfoFastSyncRead{
uint8_t *p_recv_buf;
uint8_t id;
uint8_t error;
} __attribute__((packed)) XELInfoFastSyncRead_t;
typedef struct InfoFastSyncReadInst{
uint16_t addr;
uint16_t addr_length;
XELInfoFastSyncRead_t* p_xels;
uint8_t xel_count;
bool is_info_changed;
InfoSyncBulkBuffer_t packet;
} __attribute__((packed)) InfoFastSyncReadInst_t;
struct XELInfoSyncRead_t {
uint8_t *p_recv_buf;
uint8_t id;
uint8_t error;
} __attribute__((packed));
struct InfoSyncReadInst_t {
uint16_t addr;
uint16_t addr_length;
XELInfoSyncRead_t* p_xels;
uint8_t xel_count;
bool is_info_changed;
InfoSyncBulkBuffer_t packet;
} __attribute__((packed));
*/

/* syncWrite
DYNAMIXEL PROTOCOL 1.0 supports Control Table address up to 255.
Structures containing the necessary information to process the 'syncWrite' packet.
typedef struct XELInfoSyncWrite{
struct XELInfoSyncWrite_t {
uint8_t* p_data;
uint8_t id;
} __attribute__((packed)) XELInfoSyncWrite_t;
} __attribute__((packed));
typedef struct InfoSyncWriteInst{
struct InfoSyncWriteInst_t {
uint16_t addr;
uint16_t addr_length;
XELInfoSyncWrite_t* p_xels;
uint8_t xel_count;
bool is_info_changed;
InfoSyncBulkBuffer_t packet;
} __attribute__((packed)) InfoSyncWriteInst_t;
} __attribute__((packed));
*/

const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
Expand All @@ -115,8 +115,8 @@ typedef struct sw_data{


sr_data_t sr_data[DXL_ID_CNT];
DYNAMIXEL::InfoFastSyncReadInst_t sr_infos;
DYNAMIXEL::XELInfoFastSyncRead_t info_xels_sr[DXL_ID_CNT];
DYNAMIXEL::InfoSyncReadInst_t sr_infos;
DYNAMIXEL::XELInfoSyncRead_t info_xels_sr[DXL_ID_CNT];

sw_data_t sw_data[DXL_ID_CNT];
DYNAMIXEL::InfoSyncWriteInst_t sw_infos;
Expand Down Expand Up @@ -231,8 +231,10 @@ void loop() {
DEBUG_SERIAL.print("[fastSyncRead] Success, Received ID Count: ");
DEBUG_SERIAL.println(recv_cnt);
for(i=0; i<recv_cnt; i++) {
DEBUG_SERIAL.print(" ID: ");DEBUG_SERIAL.print(sr_infos.p_xels[i].id);
DEBUG_SERIAL.print("\t Present Position: ");DEBUG_SERIAL.println(sr_data[i].present_position);
DEBUG_SERIAL.print(" ID: ");
DEBUG_SERIAL.print(sr_infos.p_xels[i].id);
DEBUG_SERIAL.print("\t Present Position: ");
DEBUG_SERIAL.println(sr_data[i].present_position);
}
} else {
DEBUG_SERIAL.print("[fastSyncRead] Fail, Lib error code: ");
Expand Down
Loading

0 comments on commit 84b29f1

Please sign in to comment.