Aditya Mehrotra
/
DMA_Dynamixel_Communications
Teleop Demo Code
main.cpp@0:bcd01ae054eb, 2019-05-04 (annotated)
- Committer:
- bwang
- Date:
- Sat May 04 18:46:43 2019 +0000
- Revision:
- 0:bcd01ae054eb
- Child:
- 1:66ff8f8e65f7
first commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 0:bcd01ae054eb | 1 | #include "mbed.h" |
bwang | 0:bcd01ae054eb | 2 | #include "crc.h" |
bwang | 0:bcd01ae054eb | 3 | |
bwang | 0:bcd01ae054eb | 4 | Serial pc(SERIAL_TX, SERIAL_RX); |
bwang | 0:bcd01ae054eb | 5 | |
bwang | 0:bcd01ae054eb | 6 | Serial uart(PA_11, PA_12); |
bwang | 0:bcd01ae054eb | 7 | DigitalOut RTS(D12); |
bwang | 0:bcd01ae054eb | 8 | |
bwang | 0:bcd01ae054eb | 9 | DigitalOut dbg(D2); |
bwang | 0:bcd01ae054eb | 10 | DigitalOut flip(D3); |
bwang | 0:bcd01ae054eb | 11 | |
bwang | 0:bcd01ae054eb | 12 | volatile int rx_count = 0; |
bwang | 0:bcd01ae054eb | 13 | uint8_t rx_buffer[100]; |
bwang | 0:bcd01ae054eb | 14 | |
bwang | 0:bcd01ae054eb | 15 | void rxCallback() { |
bwang | 0:bcd01ae054eb | 16 | while (uart.readable()) { |
bwang | 0:bcd01ae054eb | 17 | rx_buffer[rx_count] = uart.getc(); |
bwang | 0:bcd01ae054eb | 18 | rx_count++; |
bwang | 0:bcd01ae054eb | 19 | } |
bwang | 0:bcd01ae054eb | 20 | } |
bwang | 0:bcd01ae054eb | 21 | |
bwang | 0:bcd01ae054eb | 22 | void write_get_position(uint8_t id) { |
bwang | 0:bcd01ae054eb | 23 | uint8_t tx_buffer[14]; |
bwang | 0:bcd01ae054eb | 24 | rx_count = 0; |
bwang | 0:bcd01ae054eb | 25 | |
bwang | 0:bcd01ae054eb | 26 | tx_buffer[0] = 0xff; |
bwang | 0:bcd01ae054eb | 27 | tx_buffer[1] = 0xff; |
bwang | 0:bcd01ae054eb | 28 | tx_buffer[2] = 0xfd; |
bwang | 0:bcd01ae054eb | 29 | tx_buffer[3] = 0x00; |
bwang | 0:bcd01ae054eb | 30 | tx_buffer[4] = id; //ID |
bwang | 0:bcd01ae054eb | 31 | tx_buffer[5] = 0x07; //length |
bwang | 0:bcd01ae054eb | 32 | tx_buffer[6] = 0x00; //length2 |
bwang | 0:bcd01ae054eb | 33 | tx_buffer[7] = 0x02; //read command |
bwang | 0:bcd01ae054eb | 34 | tx_buffer[8] = 0x84; //position identifier |
bwang | 0:bcd01ae054eb | 35 | tx_buffer[9] = 0x00; |
bwang | 0:bcd01ae054eb | 36 | tx_buffer[10] = 0x04; |
bwang | 0:bcd01ae054eb | 37 | tx_buffer[11] = 0x00; |
bwang | 0:bcd01ae054eb | 38 | uint16_t crc = update_crc(0, tx_buffer, 12); |
bwang | 0:bcd01ae054eb | 39 | tx_buffer[12] = crc & 0xff; |
bwang | 0:bcd01ae054eb | 40 | tx_buffer[13] = (crc >> 8) & 0xff; |
bwang | 0:bcd01ae054eb | 41 | |
bwang | 0:bcd01ae054eb | 42 | printf("0x%x 0x%x\n", tx_buffer[12], tx_buffer[13]); |
bwang | 0:bcd01ae054eb | 43 | |
bwang | 0:bcd01ae054eb | 44 | RTS = 1; |
bwang | 0:bcd01ae054eb | 45 | wait_us(100); |
bwang | 0:bcd01ae054eb | 46 | for (int i = 0; i < 14; i++) { |
bwang | 0:bcd01ae054eb | 47 | uart.putc(tx_buffer[i]); |
bwang | 0:bcd01ae054eb | 48 | } |
bwang | 0:bcd01ae054eb | 49 | wait_us(200); |
bwang | 0:bcd01ae054eb | 50 | RTS = 0; |
bwang | 0:bcd01ae054eb | 51 | |
bwang | 0:bcd01ae054eb | 52 | flip = 1; |
bwang | 0:bcd01ae054eb | 53 | while (rx_count < 15) { |
bwang | 0:bcd01ae054eb | 54 | } |
bwang | 0:bcd01ae054eb | 55 | flip = 0; |
bwang | 0:bcd01ae054eb | 56 | |
bwang | 0:bcd01ae054eb | 57 | for (int i = 0; i < 15; i++) { |
bwang | 0:bcd01ae054eb | 58 | pc.printf("0x%x ", rx_buffer[i]); |
bwang | 0:bcd01ae054eb | 59 | } |
bwang | 0:bcd01ae054eb | 60 | pc.printf("\n"); |
bwang | 0:bcd01ae054eb | 61 | } |
bwang | 0:bcd01ae054eb | 62 | |
bwang | 0:bcd01ae054eb | 63 | int main() |
bwang | 0:bcd01ae054eb | 64 | { |
bwang | 0:bcd01ae054eb | 65 | pc.baud(115200); |
bwang | 0:bcd01ae054eb | 66 | uart.baud(115200); |
bwang | 0:bcd01ae054eb | 67 | uart.attach(rxCallback); |
bwang | 0:bcd01ae054eb | 68 | |
bwang | 0:bcd01ae054eb | 69 | pc.printf("Program is running!\n"); |
bwang | 0:bcd01ae054eb | 70 | |
bwang | 0:bcd01ae054eb | 71 | for (;;) { |
bwang | 0:bcd01ae054eb | 72 | dbg = 1; |
bwang | 0:bcd01ae054eb | 73 | write_get_position(0x01); |
bwang | 0:bcd01ae054eb | 74 | dbg = 0; |
bwang | 0:bcd01ae054eb | 75 | wait(1); |
bwang | 0:bcd01ae054eb | 76 | } |
bwang | 0:bcd01ae054eb | 77 | } |