Aditya Mehrotra
/
DMA_Dynamixel_Communications
Teleop Demo Code
Diff: main.cpp
- Revision:
- 2:7afc501b8283
- Parent:
- 1:66ff8f8e65f7
- Child:
- 3:ab8084bfcb53
--- a/main.cpp Sat Apr 17 17:49:36 2021 +0000 +++ b/main.cpp Mon Sep 27 02:53:32 2021 +0000 @@ -1,41 +1,22 @@ #include "mbed.h" #include "crc.h" +#define WAIT_TIME_MS 5 +#define LEN 100 + Serial pc(USBTX, USBRX); -Serial uart(PA_0, PA_1); -DigitalOut RTS(D12); - -//DigitalOut led(LED1); +RawSerial uart(D1, D0); +DigitalInOut RTS(D3); -DigitalOut dbg(D2); -DigitalOut flip(D3); - -AnalogIn GainPot(A2); -DigitalIn Calibrate(D7); -DigitalIn GainUpdate(D6); - -DigitalOut led(D8); +DigitalOut dbg(LED1); +DigitalOut flip(D2); volatile uint8_t waitForReceive = 0; volatile uint8_t nextReload = 15; -uint8_t rx_buffer[100]; +uint8_t rx_buffer[LEN]; -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]; @@ -65,17 +46,19 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_us(200); // fix this!!! + wait_ms(10); // fix this!!! RTS = 0; // Disable Tx / Enable Rx } } -void write_get_position(uint8_t id, uint8_t id2) { + +void getPosition(uint8_t id/*, uint8_t id2*/) { for(int i = 0; i < 100; i++) rx_buffer[i] = 0; - uint8_t tx_buffer[14]; + uint8_t elements = 14; + unsigned char tx_buffer[elements]; // rx_count = 0; - + tx_buffer[0] = 0xff; tx_buffer[1] = 0xff; tx_buffer[2] = 0xfd; @@ -85,44 +68,64 @@ 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[9] = 0x00; //SECOND BYTE OF IDENTIFIER (command) + tx_buffer[10] = 0x04; //p1 --> means we want 4 bytes back + tx_buffer[11] = 0x00; //p2 + + unsigned short crc = update_crc(0, tx_buffer, 12); + tx_buffer[12] = crc & 0xff; tx_buffer[13] = (crc >> 8) & 0xff; // printf("0x%x 0x%x\r\n", tx_buffer[12], tx_buffer[13]); - nextReload = 11; + //nextReload = 11; RTS = 1; waitForReceive = 1; wait_us(100); for (int i = 0; i < 14; i++) { uart.putc(tx_buffer[i]); } - wait_us(200); + wait_us(350); RTS = 0; - + - flip = 1; - while (waitForReceive == 1) { + for (int i = 0; i<=14; i++){ + rx_buffer[i] = uart.getc(); } + +} + + +void setGoalCurrent(unsigned char ID, unsigned char p1, unsigned char p2){ + 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] = 0x66; //Position + packetBuffer[9] = 0x00; + packetBuffer[10] = p1; + packetBuffer[11] = p2; -// 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); + 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(10); // fix this!!! + RTS = 0; // Disable Tx / Enable Rx } - } @@ -139,7 +142,7 @@ packetBuffer[5] = 0x06; //length packetBuffer[6] = 0x00; //length2 packetBuffer[7] = 0x03; //write command - packetBuffer[8] = 0x40; //torque enable + packetBuffer[8] = 0x40; //enable current mode packetBuffer[9] = 0x00; packetBuffer[10] = enable; @@ -153,7 +156,7 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_us(200); // fix this!!! + wait_ms(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } @@ -185,17 +188,15 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(1); // fix this!!! + wait_ms(WAIT_TIME_MS); // 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; +void ledOff(unsigned char ID) +{ + uint8_t elements = 13; unsigned char packetBuffer[elements]; if (uart.writeable() ) { packetBuffer[0] = 0xff; @@ -203,34 +204,33 @@ packetBuffer[2] = 0xfd; packetBuffer[3] = 0x00; packetBuffer[4] = ID; //ID - packetBuffer[5] = 0x07; //length + packetBuffer[5] = 0x06; //length packetBuffer[6] = 0x00; //length2 packetBuffer[7] = 0x03; //write command - packetBuffer[8] = 0x50; //Kp + packetBuffer[8] = 0x41; //led packetBuffer[9] = 0x00; - packetBuffer[10] = kd1; - packetBuffer[11] = kd2; + packetBuffer[10] = 0x00; //off - unsigned short crc = update_crc(0,packetBuffer,12); + unsigned short crc = update_crc(0,packetBuffer,11); - packetBuffer[12] = crc&0x00ff; //CRC1 - packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + 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!!! + wait_ms(5); // 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); + + +void enPosMode(unsigned char ID) +{ + uint8_t elements = 13; unsigned char packetBuffer[elements]; if (uart.writeable() ) { packetBuffer[0] = 0xff; @@ -238,34 +238,32 @@ packetBuffer[2] = 0xfd; packetBuffer[3] = 0x00; packetBuffer[4] = ID; //ID - packetBuffer[5] = 0x07; //length + packetBuffer[5] = 0x06; //length packetBuffer[6] = 0x00; //length2 packetBuffer[7] = 0x03; //write command - packetBuffer[8] = 0x54; //Kp + packetBuffer[8] = 0x0B; //mode packetBuffer[9] = 0x00; - packetBuffer[10] = kp1; - packetBuffer[11] = kp2; + packetBuffer[10] = 0x03; //on - unsigned short crc = update_crc(0,packetBuffer,12); + unsigned short crc = update_crc(0,packetBuffer,11); - packetBuffer[12] = crc&0x00ff; //CRC1 - packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + 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!!! + wait_ms(WAIT_TIME_MS); // 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); + +void enCurMode(unsigned char ID) +{ + uint8_t elements = 13; unsigned char packetBuffer[elements]; if (uart.writeable() ) { packetBuffer[0] = 0xff; @@ -273,34 +271,33 @@ packetBuffer[2] = 0xfd; packetBuffer[3] = 0x00; packetBuffer[4] = ID; //ID - packetBuffer[5] = 0x07; //length + packetBuffer[5] = 0x06; //length packetBuffer[6] = 0x00; //length2 packetBuffer[7] = 0x03; //write command - packetBuffer[8] = 0x58; //Feed forward acceleration + packetBuffer[8] = 0x0B; //mode packetBuffer[9] = 0x00; - packetBuffer[10] = kp1; - packetBuffer[11] = kp2; + packetBuffer[10] = 0x00; //on - unsigned short crc = update_crc(0,packetBuffer,12); + unsigned short crc = update_crc(0,packetBuffer,11); - packetBuffer[12] = crc&0x00ff; //CRC1 - packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + 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!!! + wait_ms(WAIT_TIME_MS); // 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); + + +void setBaud(unsigned char ID, unsigned char RATE) +{ + uint8_t elements = 13; unsigned char packetBuffer[elements]; if (uart.writeable() ) { packetBuffer[0] = 0xff; @@ -308,91 +305,87 @@ packetBuffer[2] = 0xfd; packetBuffer[3] = 0x00; packetBuffer[4] = ID; //ID - packetBuffer[5] = 0x07; //length + packetBuffer[5] = 0x06; //length packetBuffer[6] = 0x00; //length2 packetBuffer[7] = 0x03; //write command - packetBuffer[8] = 0x5A; //Feed forward velocity + packetBuffer[8] = 0x08; //baud rate packetBuffer[9] = 0x00; - packetBuffer[10] = kp1; - packetBuffer[11] = kp2; + packetBuffer[10] = RATE; //0x01 - 57600, 0x02 - 115,200, 0x03 - 1000000 - unsigned short crc = update_crc(0,packetBuffer,12); + unsigned short crc = update_crc(0,packetBuffer,11); - packetBuffer[12] = crc&0x00ff; //CRC1 - packetBuffer[13] = (crc>>8)&0x00ff; //CRC2 + 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!!! + wait_ms(5); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } } + + + + + int main() { wait_ms(300); RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN; + RTS.mode(OpenDrainNoPull); + RTS.output(); pc.baud(115200); - 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"); + uart.baud(57600); + RTS = 0; + wait(1); + + //setBaud(0x01, 0x01); + //wait(1); + pc.printf("Program is running!\r\n"); - + //torqueEnable(0x01,0x01); + //wait(1); - 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); + getPosition(0x01); + wait(1); + + //torqueEnable(0x01,0x00); + //wait(1); + + for (int i = 0; i<=14; i++) { + pc.printf("%hx ", rx_buffer[i]); //USE %HX FOR HEX!!! + } + pc.printf("\n"); + pc.printf("done!n"); + + + + + /* + ledOff(0x01); + torqueEnable(0x01,0x00); + enPosMode(0x01); + //enCurMode(0x01); + wait(1); + ledOn(0x01); + //setP(0x01,100); 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; -// 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; - } + //setGoalCurrent(0x01, 0xF0, 0x00); + setGoalPosition(0x01,0x00,0x0F,0x00,0x00); + wait(5); + ledOff(0x01); + torqueEnable(0x01,0x00); + //led = 1; + */ + + + + }