Aditya Mehrotra
/
DMA_Dynamixel_Communications
Teleop Demo Code
main.cpp
- Committer:
- adimmit
- Date:
- 2021-09-27
- Revision:
- 2:7afc501b8283
- Parent:
- 1:66ff8f8e65f7
- Child:
- 3:ab8084bfcb53
File content as of revision 2:7afc501b8283:
#include "mbed.h" #include "crc.h" #define WAIT_TIME_MS 5 #define LEN 100 Serial pc(USBTX, USBRX); RawSerial uart(D1, D0); DigitalInOut RTS(D3); DigitalOut dbg(LED1); DigitalOut flip(D2); volatile uint8_t waitForReceive = 0; volatile uint8_t nextReload = 15; uint8_t rx_buffer[LEN]; 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_ms(10); // fix this!!! RTS = 0; // Disable Tx / Enable Rx } } void getPosition(uint8_t id/*, uint8_t id2*/) { for(int i = 0; i < 100; i++) rx_buffer[i] = 0; uint8_t elements = 14; unsigned char tx_buffer[elements]; // 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; //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; RTS = 1; waitForReceive = 1; wait_us(100); for (int i = 0; i < 14; i++) { uart.putc(tx_buffer[i]); } wait_us(350); RTS = 0; 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; 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 } } 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; //enable current mode 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_ms(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } } 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(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } } void ledOff(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] = 0x00; //off 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(5); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } } void enPosMode(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] = 0x0B; //mode packetBuffer[9] = 0x00; packetBuffer[10] = 0x03; //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(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } } void enCurMode(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] = 0x0B; //mode packetBuffer[9] = 0x00; packetBuffer[10] = 0x00; //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(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } } void setBaud(unsigned char ID, unsigned char RATE) { 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] = 0x08; //baud rate packetBuffer[9] = 0x00; packetBuffer[10] = RATE; //0x01 - 57600, 0x02 - 115,200, 0x03 - 1000000 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(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(57600); RTS = 0; wait(1); //setBaud(0x01, 0x01); //wait(1); pc.printf("Program is running!\r\n"); //torqueEnable(0x01,0x01); //wait(1); 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); wait(1); //setGoalCurrent(0x01, 0xF0, 0x00); setGoalPosition(0x01,0x00,0x0F,0x00,0x00); wait(5); ledOff(0x01); torqueEnable(0x01,0x00); //led = 1; */ }