19 #if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) 24 #include "../../include/dynamixel_sdk/port_handler_arduino.h" 26 #if defined (__OPENCR__) 27 #define DYNAMIXEL_SERIAL Serial3 30 #define LATENCY_TIMER 4 // msec (USB latency timer) 35 : baudrate_(DEFAULT_BAUDRATE_),
36 packet_start_time_(0.0),
43 #if defined(__OPENCR__) 44 pinMode(BDPIN_DXL_PWR_EN, OUTPUT);
47 #elif defined(__OPENCM904__) 48 if (port_name[0] ==
'1')
51 p_dxl_serial = &Serial1;
53 else if (port_name[0] ==
'2')
56 p_dxl_serial = &Serial2;
58 else if (port_name[0] ==
'3')
61 p_dxl_serial = &Serial3;
66 p_dxl_serial = &Serial1;
69 drv_dxl_begin(socket_fd_);
77 #if defined(__OPENCR__) 78 pinMode(BDPIN_DXL_PWR_EN, OUTPUT);
94 int temp __attribute__((unused));
95 #if defined(__OPENCR__) 96 while (DYNAMIXEL_SERIAL.available())
98 temp = DYNAMIXEL_SERIAL.read();
100 #elif defined(__OPENCM904__) 101 while (p_dxl_serial->available())
103 temp = p_dxl_serial->read();
110 strcpy(port_name_, port_name);
120 baudrate_ = checkBaudrateAvailable(baudrate);
125 setupPort(baudrate_);
139 #if defined(__OPENCR__) 140 bytes_available = DYNAMIXEL_SERIAL.available();
141 #elif defined(__OPENCM904__) 142 bytes_available = p_dxl_serial->available();
145 return bytes_available;
152 #if defined(__OPENCR__) 153 rx_length = DYNAMIXEL_SERIAL.available();
154 #elif defined(__OPENCM904__) 155 rx_length = p_dxl_serial->available();
158 if (rx_length > length)
161 for (
int i = 0; i < rx_length; i++)
163 #if defined(__OPENCR__) 164 packet[i] = DYNAMIXEL_SERIAL.read();
165 #elif defined(__OPENCM904__) 166 packet[i] = p_dxl_serial->read();
179 #if defined(__OPENCR__) 180 length_written = DYNAMIXEL_SERIAL.write(packet, length);
181 #elif defined(__OPENCM904__) 182 length_written = p_dxl_serial->write(packet, length);
187 return length_written;
192 packet_start_time_ = getCurrentTime();
193 packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (
LATENCY_TIMER * 2.0) + 2.0;
198 packet_start_time_ = getCurrentTime();
199 packet_timeout_ = msec;
204 if (getTimeSinceStart() > packet_timeout_)
215 return (
double)millis();
222 elapsed_time = getCurrentTime() - packet_start_time_;
223 if (elapsed_time < 0.0)
224 packet_start_time_ = getCurrentTime();
231 #if defined(__OPENCR__) 232 DYNAMIXEL_SERIAL.begin(baudrate);
233 #elif defined(__OPENCM904__) 234 p_dxl_serial->setDxlMode(
true);
235 p_dxl_serial->begin(baudrate);
240 tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0;
271 #if defined(__OPENCR__) 272 digitalWrite(BDPIN_DXL_PWR_EN, HIGH);
278 #if defined(__OPENCR__) 279 digitalWrite(BDPIN_DXL_PWR_EN, LOW);
285 #if defined(__OPENCR__) 286 drv_dxl_tx_enable(TRUE);
287 #elif defined(__OPENCM904__) 288 drv_dxl_tx_enable(socket_fd_, TRUE);
294 #if defined(__OPENCR__) 295 #ifdef SERIAL_WRITES_NON_BLOCKING 296 DYNAMIXEL_SERIAL.flush();
298 drv_dxl_tx_enable(FALSE);
300 #elif defined(__OPENCM904__) 301 #ifdef SERIAL_WRITES_NON_BLOCKING 302 p_dxl_serial->flush();
304 drv_dxl_tx_enable(socket_fd_, FALSE);
int writePort(uint8_t *packet, int length)
The function that writes bytes on the port buffer The function writes bytes on the port buffer...
bool setupPort(const int cflag_baud)
void setPacketTimeout(uint16_t packet_length)
The function that sets and starts stopwatch for watching packet timeout The function sets the stopwa...
int checkBaudrateAvailable(int baudrate)
int readPort(uint8_t *packet, int length)
The function that reads bytes from the port buffer The function gets bytes from the port buffer...
double getTimeSinceStart()
int getBaudRate()
The function that returns current baudrate set into the port handler The function returns current ba...
void closePort()
The function that closes the port The function closes the port.
bool setBaudRate(const int baudrate)
The function that sets baudrate into the port handler The function sets baudrate into the port handl...
PortHandlerArduino(const char *port_name)
The function that initializes instance of PortHandler and gets port_name The function initializes in...
bool openPort()
The function that opens the port The function calls PortHandlerArduino::setBaudRate() to open the po...
int getBytesAvailable()
The function that checks how much bytes are able to be read from the port buffer The function checks...
void clearPort()
The function that clears the port The function clears the port.
char * getPortName()
The function that returns port name set into the port handler The function returns current port name...
void setPortName(const char *port_name)
The function that sets port name into the port handler The function sets port name into the port han...
bool is_using_
shows whether the port is in use
virtual bool setBaudRate(const int baudrate)=0
The function that sets baudrate into the port handler The function sets baudrate into the port handl...
bool isPacketTimeout()
The function that checks whether packet timeout is occurred The function checks whether current time...
virtual void setPortName(const char *port_name)=0
The function that sets port name into the port handler The function sets port name into the port han...