|
27 | 27 | #define DYNAMIXEL_SERIAL Serial3 |
28 | 28 | #endif |
29 | 29 |
|
30 | | -#define LATENCY_TIMER 16 // msec (USB latency timer) |
| 30 | +#define LATENCY_TIMER 4 // msec (USB latency timer) |
31 | 31 |
|
32 | 32 | using namespace dynamixel; |
33 | 33 |
|
@@ -91,10 +91,17 @@ void PortHandlerArduino::closePort() |
91 | 91 |
|
92 | 92 | void PortHandlerArduino::clearPort() |
93 | 93 | { |
| 94 | + int temp __attribute__((unused)); |
94 | 95 | #if defined(__OPENCR__) |
95 | | - DYNAMIXEL_SERIAL.flush(); |
| 96 | + while (DYNAMIXEL_SERIAL.available()) |
| 97 | + { |
| 98 | + temp = DYNAMIXEL_SERIAL.read(); |
| 99 | + } |
96 | 100 | #elif defined(__OPENCM904__) |
97 | | - p_dxl_serial->flush(); |
| 101 | + while (p_dxl_serial->available()) |
| 102 | + { |
| 103 | + temp = p_dxl_serial->read(); |
| 104 | + } |
98 | 105 | #endif |
99 | 106 | } |
100 | 107 |
|
@@ -205,7 +212,7 @@ bool PortHandlerArduino::isPacketTimeout() |
205 | 212 |
|
206 | 213 | double PortHandlerArduino::getCurrentTime() |
207 | 214 | { |
208 | | - return (double)millis(); |
| 215 | + return (double)millis(); |
209 | 216 | } |
210 | 217 |
|
211 | 218 | double PortHandlerArduino::getTimeSinceStart() |
@@ -285,8 +292,15 @@ void PortHandlerArduino::setTxEnable() |
285 | 292 | void PortHandlerArduino::setTxDisable() |
286 | 293 | { |
287 | 294 | #if defined(__OPENCR__) |
| 295 | +#ifdef SERIAL_WRITES_NON_BLOCKING |
| 296 | + DYNAMIXEL_SERIAL.flush(); // make sure it completes before we disable... |
| 297 | +#endif |
288 | 298 | drv_dxl_tx_enable(FALSE); |
| 299 | + |
289 | 300 | #elif defined(__OPENCM904__) |
| 301 | +#ifdef SERIAL_WRITES_NON_BLOCKING |
| 302 | + p_dxl_serial->flush(); |
| 303 | +#endif |
290 | 304 | drv_dxl_tx_enable(socket_fd_, FALSE); |
291 | 305 | #endif |
292 | 306 | } |
|
0 commit comments