Aditya Mehrotra / Mbed OS 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?

UserRevisionLine numberNew 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 }