Add some overloading function & example for creating custom

SerialPortHandler (#23)

Author: Kei

examples/advanced/add_custom_SerialPortHandler/add_custom_SerialPortHandler.ino

@@ -0,0 +1,114 @@
+/*******************************************************************************
+* 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 <Dynamixel2Arduino.h>
+
+
+#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
+  #include <SoftwareSerial.h>
+  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
+#else
+  #define DXL_SERIAL   Serial1
+  #define DEBUG_SERIAL Serial
+  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
+#endif
+
+class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
+{
+  public:
+    NewSerialPortHandler(HardwareSerial& port, const int dir_pin = -1)
+    : SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin)
+    {}
+
+    virtual size_t write(uint8_t c) override
+    {
+      size_t ret = 0;
+      digitalWrite(DXL_DIR_PIN, HIGH);
+      while(digitalRead(DXL_DIR_PIN) != HIGH);
+      
+      ret = port_.write(c);
+
+      port_.flush();
+      digitalWrite(DXL_DIR_PIN, LOW);
+      while(digitalRead(DXL_DIR_PIN) != LOW);
+      
+      return ret;
+    }
+
+    virtual size_t write(uint8_t *buf, size_t len) override
+    {
+      size_t ret;
+      digitalWrite(DXL_DIR_PIN, HIGH);
+      while(digitalRead(DXL_DIR_PIN) != HIGH);
+
+      ret = port_.write(buf, len);
+
+      port_.flush();
+      digitalWrite(DXL_DIR_PIN, LOW);
+      while(digitalRead(DXL_DIR_PIN) != LOW);
+
+      return ret;     
+    }
+
+  private:
+    HardwareSerial& port_;
+    const int dir_pin_;
+};
+
+const uint8_t DXL_ID = 1;
+const float DXL_PROTOCOL_VERSION = 2.0;
+
+Dynamixel2Arduino dxl;
+NewSerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
+
+
+void setup() {
+  // put your setup code here, to run once:
+
+  // Use Serial to debug.
+  DEBUG_SERIAL.begin(115200);
+
+  // Set Port instance
+  dxl.setPort(dxl_port);
+  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
+  dxl.begin(57600);
+  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
+  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+
+  DEBUG_SERIAL.print("PROTOCOL ");
+  DEBUG_SERIAL.print(DXL_PROTOCOL_VERSION, 1);
+  DEBUG_SERIAL.print(", ID ");
+  DEBUG_SERIAL.print(DXL_ID);
+  DEBUG_SERIAL.print(": ");
+  if(dxl.ping(DXL_ID) == true){
+    DEBUG_SERIAL.print("ping succeeded!");
+    DEBUG_SERIAL.print(", Model Number: ");
+    DEBUG_SERIAL.println(dxl.getModelNumber(DXL_ID));
+  }else{
+    DEBUG_SERIAL.println("ping failed!");
+  }
+  delay(500);
+}
+
+
+

examples/advanced/read_write_ControlTableItem/read_write_ControlTableItem.ino

@@ -116,24 +116,16 @@
 */
 
 
-#ifdef ARDUINO_AVR_UNO
+#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
   #include <SoftwareSerial.h>
-  SoftwareSerial soft_serial(7, 8); //RX,TX
+  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
-#elif ARDUINO_AVR_MEGA2560
-  #define DXL_SERIAL   Serial
-  #define DEBUG_SERIAL Serial1
-  const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield
-#elif BOARD_OpenCM904
-  #define DXL_SERIAL   Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (To use the DXL port on the OpenCM 9.04 board, you must use Serial1 for Serial. And because of the OpenCM 9.04 driver code, you must call Serial1.setDxlMode(true); before dxl.begin();.)
-  #define DEBUG_SERIAL Serial
-  const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (To use the DXL port on the OpenCM 9.04 board, you must use 28 for DIR PIN.)  
+  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
 #else
   #define DXL_SERIAL   Serial1
   #define DEBUG_SERIAL Serial
-  const uint8_t DXL_DIR_PIN = 2; //DYNAMIXEL Shield
+  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
 #endif
 
 const uint8_t DXL_ID = 1;

keywords.txt

@@ -43,6 +43,7 @@ writeControlTableItem	KEYWORD2
 
 # Master Class
 setPortProtocolVersion	KEYWORD2
+setPortProtocolVersionUsingIndex	KEYWORD2
 getPortProtocolVersion	KEYWORD2
 setPort	KEYWORD2
 ping	KEYWORD2

src/Dynamixel2Arduino.cpp

@@ -67,21 +67,36 @@ static bool checkAndconvertWriteData(float in_data, int32_t &out_data, uint8_t u
 static bool checkAndconvertReadData(int32_t in_data, float &out_data, uint8_t unit, ItemAndRangeInfo_t &item_info);
 
 
+Dynamixel2Arduino::Dynamixel2Arduino()
+  : Master()
+{}
+
 Dynamixel2Arduino::Dynamixel2Arduino(HardwareSerial& port, int dir_pin)
-  : Master(), dxl_port_(port, dir_pin)
+  : Master()
 {
-  setPort(dxl_port_);
+  p_dxl_port_ = new SerialPortHandler(port, dir_pin);
+  setPort(p_dxl_port_);
 }
 
 /* For Master configuration */
 void Dynamixel2Arduino::begin(unsigned long baud)
 {
-  dxl_port_.begin(baud);
+  p_dxl_port_ = (SerialPortHandler*)getPort();
+
+  if(p_dxl_port_ == nullptr)
+    return;
+
+  p_dxl_port_->begin(baud);
 }
 
-unsigned long Dynamixel2Arduino::getPortBaud() const
+unsigned long Dynamixel2Arduino::getPortBaud()
 {
-  return dxl_port_.getBaud();
+  p_dxl_port_ = (SerialPortHandler*)getPort();
+
+  if(p_dxl_port_ == nullptr)
+    return 0;
+
+  return p_dxl_port_->getBaud();
 }
 
 bool Dynamixel2Arduino::scan()
@@ -95,6 +110,9 @@ bool Dynamixel2Arduino::scan()
 
 bool Dynamixel2Arduino::ping(uint8_t id)
 {
+  if(getPort() == nullptr)
+    return false;
+
   bool ret = false;
   uint8_t i;
 
@@ -146,6 +164,9 @@ bool Dynamixel2Arduino::ping(uint8_t id)
 
 uint16_t Dynamixel2Arduino::getModelNumber(uint8_t id)
 {
+  if(getPort() == nullptr)
+    return false;
+
   uint16_t model_num = 0xFFFF;
 
   (void) read(id, COMMON_MODEL_NUMBER_ADDR, COMMON_MODEL_NUMBER_ADDR_LENGTH,
@@ -177,6 +198,9 @@ bool Dynamixel2Arduino::setProtocol(uint8_t id, float version)
 //TODO: Simplify the code by grouping model numbers.
 bool Dynamixel2Arduino::setBaudrate(uint8_t id, uint32_t baudrate)
 {
+  if(getPort() == nullptr)
+    return false;
+
   uint16_t model_num = getModelNumberFromTable(id);
   uint8_t baud_idx = 0;
 
@@ -420,6 +444,9 @@ bool Dynamixel2Arduino::ledOff(uint8_t id)
 
 bool Dynamixel2Arduino::setLedState(uint8_t id, bool state)
 {
+  if(getPort() == nullptr)
+    return false;
+
   bool ret = false;
   uint16_t model_num = getModelNumberFromTable(id);
 
@@ -463,6 +490,9 @@ bool Dynamixel2Arduino::setLedState(uint8_t id, bool state)
 //TODO: Simplify the code by grouping model numbers.
 bool Dynamixel2Arduino::setOperatingMode(uint8_t id, uint8_t mode)
 {
+  if(getPort() == nullptr)
+    return false;
+
   bool ret = false;
   uint16_t model_num = getModelNumberFromTable(id);
 
@@ -707,13 +737,19 @@ float Dynamixel2Arduino::getPresentCurrent(uint8_t id, uint8_t unit)
 
 int32_t Dynamixel2Arduino::readControlTableItem(uint8_t item_idx, uint8_t id, uint32_t timeout)
 {
+  if(getPort() == nullptr)
+    return 0;
+
   uint16_t model_num = getModelNumberFromTable(id);
 
   return readControlTableItem(model_num, item_idx, id, timeout);
 }
 
 bool Dynamixel2Arduino::writeControlTableItem(uint8_t item_idx, uint8_t id, int32_t data, uint32_t timeout)
 {
+  if(getPort() == nullptr)
+    return false;
+
   uint16_t model_num = getModelNumberFromTable(id);
 
   return writeControlTableItem(model_num, item_idx, id, data, timeout);
@@ -726,6 +762,9 @@ bool Dynamixel2Arduino::writeControlTableItem(uint8_t item_idx, uint8_t id, int3
 
 int32_t Dynamixel2Arduino::readControlTableItem(uint16_t model_num, uint8_t item_idx, uint8_t id, uint32_t timeout)
 {
+  if(getPort() == nullptr)
+    return 0;
+
   int32_t recv_len, ret = 0;
   ControlTableItemInfo_t item_info;
 
@@ -749,6 +788,9 @@ int32_t Dynamixel2Arduino::readControlTableItem(uint16_t model_num, uint8_t item
 
 bool Dynamixel2Arduino::writeControlTableItem(uint16_t model_num, uint8_t item_idx, uint8_t id, int32_t data, uint32_t timeout)
 {
+  if(getPort() == nullptr)
+    return false;
+  
   ControlTableItemInfo_t item_info;
 
   item_info = getControlTableItemInfo(model_num, item_idx);
@@ -790,6 +832,11 @@ uint16_t Dynamixel2Arduino::getModelNumberFromTable(uint8_t id)
 
 float Dynamixel2Arduino::readForRangeDependencyFunc(uint8_t func_idx, uint8_t id, uint8_t unit)
 {
+  p_dxl_port_ = (SerialPortHandler*)getPort();
+
+  if(p_dxl_port_ == nullptr)
+    return 0.0;
+
   float ret = 0;
   int32_t ret_data = 0;
   uint16_t model_num = getModelNumberFromTable(id);
@@ -807,6 +854,11 @@ float Dynamixel2Arduino::readForRangeDependencyFunc(uint8_t func_idx, uint8_t id
 
 bool Dynamixel2Arduino::writeForRangeDependencyFunc(uint8_t func_idx, uint8_t id, float value, uint8_t unit)
 {
+  p_dxl_port_ = (SerialPortHandler*)getPort();
+
+  if(p_dxl_port_ == nullptr)
+    return false;
+
   bool ret = false;
   int32_t data = 0;
   uint16_t model_num = getModelNumberFromTable(id);

src/Dynamixel2Arduino.h

@@ -45,6 +45,14 @@ enum ParamUnit{
 class Dynamixel2Arduino : public DYNAMIXEL::Master
 {
   public:
+    /**
+     * @brief The constructor.
+     * @code
+     * Dynamixel2Arduino dxl;
+     * @endcode
+     */   
+    Dynamixel2Arduino();
+
     /**
      * @brief The constructor.
      * @code
@@ -79,7 +87,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master
      * @endcode
      * @return It returns serial baudrate of board port.
      */   
-    unsigned long getPortBaud() const;
+    unsigned long getPortBaud();
 
     /**
      * @brief It is API for To checking the communication connection status of DYNAMIXEL.
@@ -409,7 +417,7 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master
       uint8_t id;
     } IdAndModelNum_t;
 
-    DYNAMIXEL::SerialPortHandler dxl_port_;
+    DYNAMIXEL::SerialPortHandler *p_dxl_port_;
 
     IdAndModelNum_t registered_dxl_[DXL_MAX_NODE];
     uint8_t         registered_dxl_cnt_;