Issue ROBOTIS-GIT#115 - allow override of hard-coded delay

Signed-off-by: John Auld <[email protected]>
johnauld · Aug 8, 2023 · 21788c0 · 21788c0
1 parent 5dac7b1
commit 21788c0
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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_ */
+#endif /* DYNAMIXEL_PORT_HANDLER_HPP_ */