diff --git a/src/Dynamixel2Arduino.cpp b/src/Dynamixel2Arduino.cpp index dc53fdf..017fb08 100644 --- a/src/Dynamixel2Arduino.cpp +++ b/src/Dynamixel2Arduino.cpp @@ -128,7 +128,7 @@ Dynamixel2Arduino::Dynamixel2Arduino(HardwareSerial& port, int dir_pin, uint16_t } /* For Master configuration */ -void Dynamixel2Arduino::begin(unsigned long baud) +void Dynamixel2Arduino::begin(unsigned long baud, uint32_t delay) { p_dxl_port_ = (SerialPortHandler*)getPort(); @@ -137,7 +137,7 @@ void Dynamixel2Arduino::begin(unsigned long baud) return; } - p_dxl_port_->begin(baud); + p_dxl_port_->begin(baud, delay); } unsigned long Dynamixel2Arduino::getPortBaud() diff --git a/src/Dynamixel2Arduino.h b/src/Dynamixel2Arduino.h index 29a6b52..409c217 100644 --- a/src/Dynamixel2Arduino.h +++ b/src/Dynamixel2Arduino.h @@ -80,9 +80,10 @@ class Dynamixel2Arduino : public DYNAMIXEL::Master * dxl.begin(57600); * @endcode * @param baud The port speed you want on the board (the speed to communicate with DYNAMIXEL) (default : 57600) + * @param delay Delay in milliseconds after opening to allow hardware to settle (default: 500) * @return It returns true(1) on success, false(0) on failure. */ - void begin(unsigned long baud = 57600); + void begin(unsigned long baud = 57600, uint32_t delay = 500); /** * @brief It is API for getting serial baudrate of board port. diff --git a/src/utility/port_handler.cpp b/src/utility/port_handler.cpp index 690395c..543f9d8 100644 --- a/src/utility/port_handler.cpp +++ b/src/utility/port_handler.cpp @@ -29,7 +29,7 @@ void SerialPortHandler::begin() begin(baud_); } -void SerialPortHandler::begin(unsigned long baud) +void SerialPortHandler::begin(unsigned long baud, uint32_t delay) { #if defined(ARDUINO_OpenCM904) if(port_ == Serial1 && getOpenState() == false){ @@ -39,14 +39,16 @@ void SerialPortHandler::begin(unsigned long baud) if(port_ == Serial1 && getOpenState() == false){ pinMode(BDPIN_DXL_PWR_EN, OUTPUT); digitalWrite(BDPIN_DXL_PWR_EN, HIGH); - delay(500); // Wait for the FET to turn on. + if(delay) + delay(delay); // Wait for the FET to turn on. } #elif defined(ARDUINO_OpenCR) if(port_ == Serial3 && getOpenState() == false){ pinMode(BDPIN_DXL_PWR_EN, OUTPUT); digitalWrite(BDPIN_DXL_PWR_EN, HIGH); } - delay(500); // Wait for the DYNAMIXEL to power up normally. + if(delay) + delay(delay); // Wait for the DYNAMIXEL to power up normally. #endif baud_ = baud; diff --git a/src/utility/port_handler.h b/src/utility/port_handler.h index 2256d63..5fed551 100644 --- a/src/utility/port_handler.h +++ b/src/utility/port_handler.h @@ -54,7 +54,7 @@ class SerialPortHandler : public DXLPortHandler virtual size_t write(uint8_t) override; virtual size_t write(uint8_t *buf, size_t len) override; - virtual void begin(unsigned long baud); + virtual void begin(unsigned long baud, uint32_t delay=300); virtual unsigned long getBaud() const; private: @@ -101,4 +101,4 @@ class USBSerialPortHandler : public DXLPortHandler }//namespace DYNAMIXEL -#endif /* DYNAMIXEL_PORT_HANDLER_HPP_ */ \ No newline at end of file +#endif /* DYNAMIXEL_PORT_HANDLER_HPP_ */