First thing : remove any long blocking operation. So the delay in blink_once, all the prints that are not part of the communication itself.
There is no chance for that to work while the delay between each byte is bigger than the servo internal byte-to-byte timeout, which is 100ms
( http://support.robotis.com/en/product/dynamixel/dxl_communication.htm talks about that at the very bottom).
And the BT serial might be useful (even though a little slow), but you’d better not interleave writing to it and to the dxl port.
If the serial libs are implemented with interrupts, it might not matter in most cases (that will depend on the size if the corresponding tx buffer, the quantity of data you try to send and the way the lib handles the buffer being full), but you could be safer by just removing it.
Now that I told you to basically remove all the debug features, you might ask how to debug things then…
If you have a logic analyzer, that’s the way to do it.
Start by monitoring the DATA line to see what actually goes on on the bus.
To get more info about what is happening in your code, set a few GPIOs as output, and switch them high or low at key points in your code.
You can use pulses (set high then low immediately afterwards) to show events happening, potentially using multiple of them to get more info (example: one pulse when doing this, two pluses when doing that, … Or as many pulses as the state you are in in a finite state machine, or as many pulses as the number of time you pass a certain point… ).
You can also use these gpios to delimitate zones: set high when you get in a function and low before you return, you have the exact time it took to run it. I use that all the time to benchmark performance without slowing down the execution or requiring additional memory.
The logic analyzer software will show you precise timing of all the things happening on these debug pins. It is not as explicit as text printed on the screen, but it’s so much more powerful to debug time-sensitive code like communications.
With the arduino-like code, the pulses will not be super-super narrow, it might take dozens of cycles to do one, but compared to the rest of what is happening it should not impact too much. I usually write directly in the register to do it in 2 cycles per change, but it’s not really arduino-like and probably overkill here.
Not too important anymore as it would be solved by removing all debug prints, there is the problem of you writing debug info to SerialUSB. These additional characters are mixed within the data coming from the Dxl bus… Pypot will see it all and will never be able to parse any packet.
Edit: when talking at 1Mbps like it’s the case here, one character takes 0.01ms to be transmitted on the bus (8 bits + 1 start bit + 1 stop bit, so 10 bits per transmitted byte), so think wisely before adding any delay() or print.