Aditya Mehrotra
/
DMA_Dynamixel_Communications
Teleop Demo Code
Diff: main.cpp
- Revision:
- 0:bcd01ae054eb
- Child:
- 1:66ff8f8e65f7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat May 04 18:46:43 2019 +0000 @@ -0,0 +1,77 @@ +#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); + } +}