Bayley Wang
/
dynamixel_demo
sending and receiving data from a Dynamixel
main.cpp
- Committer:
- bwang
- Date:
- 2019-05-04
- Revision:
- 0:bcd01ae054eb
File content as of revision 0:bcd01ae054eb:
#include "mbed.h" #include "crc.h" Serial pc(SERIAL_TX, SERIAL_RX); Serial uart(PA_11, PA_12); DigitalOut RTS(D12); DigitalOut dbg(D2); DigitalOut flip(D3); volatile int rx_count = 0; uint8_t rx_buffer[100]; void rxCallback() { while (uart.readable()) { rx_buffer[rx_count] = uart.getc(); rx_count++; } } void write_get_position(uint8_t id) { uint8_t tx_buffer[14]; rx_count = 0; tx_buffer[0] = 0xff; tx_buffer[1] = 0xff; tx_buffer[2] = 0xfd; tx_buffer[3] = 0x00; tx_buffer[4] = id; //ID tx_buffer[5] = 0x07; //length tx_buffer[6] = 0x00; //length2 tx_buffer[7] = 0x02; //read command tx_buffer[8] = 0x84; //position identifier tx_buffer[9] = 0x00; tx_buffer[10] = 0x04; tx_buffer[11] = 0x00; uint16_t crc = update_crc(0, tx_buffer, 12); tx_buffer[12] = crc & 0xff; tx_buffer[13] = (crc >> 8) & 0xff; printf("0x%x 0x%x\n", tx_buffer[12], tx_buffer[13]); RTS = 1; wait_us(100); for (int i = 0; i < 14; i++) { uart.putc(tx_buffer[i]); } wait_us(200); RTS = 0; flip = 1; while (rx_count < 15) { } flip = 0; for (int i = 0; i < 15; i++) { pc.printf("0x%x ", rx_buffer[i]); } pc.printf("\n"); } int main() { pc.baud(115200); uart.baud(115200); uart.attach(rxCallback); pc.printf("Program is running!\n"); for (;;) { dbg = 1; write_get_position(0x01); dbg = 0; wait(1); } }