Aditya Mehrotra
/
DMA_Dynamixel_Communications
Teleop Demo Code
Diff: main.cpp
- Revision:
- 1:66ff8f8e65f7
- Parent:
- 0:bcd01ae054eb
- Child:
- 2:7afc501b8283
--- a/main.cpp Sat May 04 18:46:43 2019 +0000 +++ b/main.cpp Sat Apr 17 17:49:36 2021 +0000 @@ -1,27 +1,80 @@ #include "mbed.h" #include "crc.h" -Serial pc(SERIAL_TX, SERIAL_RX); +Serial pc(USBTX, USBRX); -Serial uart(PA_11, PA_12); +Serial uart(PA_0, PA_1); DigitalOut RTS(D12); +//DigitalOut led(LED1); + DigitalOut dbg(D2); DigitalOut flip(D3); -volatile int rx_count = 0; +AnalogIn GainPot(A2); +DigitalIn Calibrate(D7); +DigitalIn GainUpdate(D6); + +DigitalOut led(D8); + +volatile uint8_t waitForReceive = 0; +volatile uint8_t nextReload = 15; + uint8_t rx_buffer[100]; -void rxCallback() { - while (uart.readable()) { - rx_buffer[rx_count] = uart.getc(); - rx_count++; +extern "C" void DMA1_Stream2_IRQHandler() { + DMA1->LIFCR |= (1 << 5); // clear the transfer interrupt flag + // there is some sort of DMA error generated on each transfer + // it needs to be cleared before we can re-enable + DMA1->LIFCR = 0xffffffff; // clear other flags which are confusing and bad + + DMA1_Stream2->CR &= ~1; // disable interrupt + // start the next transfer + DMA1_Stream2->NDTR = nextReload; + DMA1_Stream2->M0AR = (uint32_t)rx_buffer; + DMA1_Stream2->CR |= 1; // enable + flip = 0; + waitForReceive = 0; +} +void setGoalPosition(unsigned char ID, unsigned char p1, unsigned char p2, unsigned char p3, unsigned char p4){ + uint8_t elements = 16; + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x09; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x74; //Position + packetBuffer[9] = 0x00; + packetBuffer[10] = p1; + packetBuffer[11] = p2; + packetBuffer[12] = p3; + packetBuffer[13] = p4; + + unsigned short crc = update_crc(0,packetBuffer,14); + + packetBuffer[14] = crc&0x00ff; //CRC1 + packetBuffer[15] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_us(200); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx } + } - -void write_get_position(uint8_t id) { +void write_get_position(uint8_t id, uint8_t id2) { + for(int i = 0; i < 100; i++) rx_buffer[i] = 0; + uint8_t tx_buffer[14]; - rx_count = 0; +// rx_count = 0; tx_buffer[0] = 0xff; tx_buffer[1] = 0xff; @@ -39,39 +92,307 @@ tx_buffer[12] = crc & 0xff; tx_buffer[13] = (crc >> 8) & 0xff; - printf("0x%x 0x%x\n", tx_buffer[12], tx_buffer[13]); - +// printf("0x%x 0x%x\r\n", tx_buffer[12], tx_buffer[13]); + nextReload = 11; RTS = 1; + waitForReceive = 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) { + while (waitForReceive == 1) { } - flip = 0; + +// for (int i = 0; i < 15; i++) { +// pc.printf("0x%x ", rx_buffer[i]); +// } +// pc.printf("\r\n"); + + nextReload = 15; + //pc.printf("NDTR before send2: %d\r\n", DMA1_Stream2->NDTR); + waitForReceive = 1; + setGoalPosition(id2, rx_buffer[9], rx_buffer[10], rx_buffer[11], rx_buffer[12]); + + while (waitForReceive == 1) { + //wait_ms(100); + //printf("NDTR: %d wfr: %d\r\n", DMA1_Stream2->NDTR, waitForReceive); + } - for (int i = 0; i < 15; i++) { - pc.printf("0x%x ", rx_buffer[i]); + +} + +void torqueEnable(unsigned char ID, unsigned char enable){ + + uint8_t elements = 13; + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x06; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x40; //torque enable + packetBuffer[9] = 0x00; + packetBuffer[10] = enable; + + unsigned short crc = update_crc(0,packetBuffer,11); + + packetBuffer[11] = crc&0x00ff; //CRC1 + packetBuffer[12] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_us(200); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx + wait_ms(10); } - pc.printf("\n"); +} +void ledOn(unsigned char ID) +{ + uint8_t elements = 13; + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x06; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x41; //led + packetBuffer[9] = 0x00; + packetBuffer[10] = 0x01; //on + + unsigned short crc = update_crc(0,packetBuffer,11); + + packetBuffer[11] = crc&0x00ff; //CRC1 + packetBuffer[12] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_ms(1); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx + wait_ms(10); + } } +void setD(unsigned char ID, uint16_t kd) { + + uint8_t kd2 = kd >> 8; + uint8_t kd1 = kd & 0xff; + uint8_t elements = 14; + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x07; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x50; //Kp + packetBuffer[9] = 0x00; + packetBuffer[10] = kd1; + packetBuffer[11] = kd2; + + unsigned short crc = update_crc(0,packetBuffer,12); + + packetBuffer[12] = crc&0x00ff; //CRC1 + packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_ms(1); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx + wait_ms(10); + } +} +void setP(unsigned char ID, uint16_t kp) { + uint8_t kp2 = kp >> 8; + uint8_t kp1 = kp & 0xff; + uint8_t elements = 14; +// printf("0x%x 0x%x\r\n", kp1, kp2); + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x07; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x54; //Kp + packetBuffer[9] = 0x00; + packetBuffer[10] = kp1; + packetBuffer[11] = kp2; + + unsigned short crc = update_crc(0,packetBuffer,12); + + packetBuffer[12] = crc&0x00ff; //CRC1 + packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_ms(1); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx + wait_ms(10); + } +} +void setFFA(unsigned char ID, uint16_t k) { + uint8_t kp2 = k >> 8; + uint8_t kp1 = k & 0xff; + uint8_t elements = 14; +// printf("0x%x 0x%x\r\n", kp1, kp2); + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x07; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x58; //Feed forward acceleration + packetBuffer[9] = 0x00; + packetBuffer[10] = kp1; + packetBuffer[11] = kp2; + + unsigned short crc = update_crc(0,packetBuffer,12); + + packetBuffer[12] = crc&0x00ff; //CRC1 + packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_ms(1); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx + wait_ms(10); + } +} +void setFFV(unsigned char ID, uint16_t k) { + uint8_t kp2 = k >> 8; + uint8_t kp1 = k & 0xff; + uint8_t elements = 14; +// printf("0x%x 0x%x\r\n", kp1, kp2); + unsigned char packetBuffer[elements]; + if (uart.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = 0xfd; + packetBuffer[3] = 0x00; + packetBuffer[4] = ID; //ID + packetBuffer[5] = 0x07; //length + packetBuffer[6] = 0x00; //length2 + packetBuffer[7] = 0x03; //write command + packetBuffer[8] = 0x5A; //Feed forward velocity + packetBuffer[9] = 0x00; + packetBuffer[10] = kp1; + packetBuffer[11] = kp2; + + unsigned short crc = update_crc(0,packetBuffer,12); + + packetBuffer[12] = crc&0x00ff; //CRC1 + packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + + // Send instruction packet + RTS = 1; // Enable Tx / Disable Rx + for (int i = 0; i< elements; i++) { + uart.putc(packetBuffer[i]); + } + wait_ms(1); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx + wait_ms(10); + } +} int main() { + wait_ms(300); + RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN; pc.baud(115200); - uart.baud(115200); - uart.attach(rxCallback); + uart.baud(1000000); +// uart.attach(rxCallback); + UART4->CR3 |= USART_CR3_DMAR; + DMA1_Stream2->PAR = (uint32_t)&UART4->DR; + DMA1_Stream2->M0AR = (uint32_t)rx_buffer; + DMA1_Stream2->CR |= (4<<25); + DMA1_Stream2->NDTR = 16; +// DMA1_Stream2->CR |= DMA_SxCR_PFCTRL; + DMA1_Stream2->CR |= DMA_SxCR_MINC; + NVIC_EnableIRQ(DMA1_Stream2_IRQn); + DMA1_Stream2->CR |= DMA_SxCR_TCIE; + pc.printf("Enabling DMA\r\n"); + //DMA1_Stream2->CR |= DMA_SxCR_EN; + pc.printf("DMA Enabled\r\n"); + pc.printf("Program is running!\r\n"); - pc.printf("Program is running!\n"); + + + ledOn(0x01); + setD(0x01, 120); + setP(0x01,200); + setD(0x02, 120); + setP(0x02, 200); + setD(0x03, 0); + setD(0x04, 0); + setD(0x05, 0); + setD(0x06, 0); + setP(0x03, 200); + setP(0x04, 200); + setP(0x05, 200); + setP(0x06, 200); +// setFFA(0x03, 800); +// setFFA(0x05, 800); + //setGoalPosition(0x04, 0xD0, 0x07, 0x00, 0x00); +// wait_ms(10); + torqueEnable(0x02,0x01); + torqueEnable(0x01,0x01); + torqueEnable(0x03,0x01); + torqueEnable(0x04,0x01); + torqueEnable(0x05,0x01); + torqueEnable(0x06,0x01); + wait(1); + led = 1; + while(uart.readable()) {uart.getc();} + DMA1_Stream2->CR |= DMA_SxCR_EN; for (;;) { + dbg = 1; - write_get_position(0x01); +// ledOn(0x03); + write_get_position(0x01,0x02); + write_get_position(0x02,0x01); + write_get_position(0x03,0x04); + write_get_position(0x04,0x03); + write_get_position(0x05,0x06); + write_get_position(0x06,0x05); + //wait_ms(10); dbg = 0; - wait(1); } }