Aditya Mehrotra / Mbed OS DMA_Dynamixel_Communications

Teleop Demo Code

Files at this revision

API Documentation at this revision

Revision 3:ab8084bfcb53, committed 2021-09-27

Comitter:
adimmit
Date:
Mon Sep 27 04:13:15 2021 +0000
Parent:
2:7afc501b8283
Commit message:
1Mbaud;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
main_115200_archive.txt Show annotated file Show diff for this revision Revisions of this file
main_57600_archive.txt Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Sep 27 02:53:32 2021 +0000
+++ b/main.cpp	Mon Sep 27 04:13:15 2021 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "crc.h"
 
-#define WAIT_TIME_MS 5
+#define WAIT_TIME_MS 1
 #define LEN 100
 
 Serial pc(USBTX, USBRX);
@@ -46,7 +46,7 @@
         for (int i = 0; i< elements; i++) {
             uart.putc(packetBuffer[i]);
         }
-        wait_ms(10);         // fix this!!!
+        wait_ms(WAIT_TIME_MS);         // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
     }
 
@@ -81,11 +81,11 @@
     //nextReload = 11;
     RTS = 1;
     waitForReceive = 1;
-    wait_us(100);
+    wait_us(15);
     for (int i = 0; i < 14; i++) {
         uart.putc(tx_buffer[i]);
     }
-    wait_us(350);
+    wait_us(25);
     RTS = 0;
  
     
@@ -123,7 +123,7 @@
         for (int i = 0; i< elements; i++) {
             uart.putc(packetBuffer[i]);
         }
-        wait_ms(10);         // fix this!!!
+        wait_us(25);         // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
     }
 
@@ -156,10 +156,11 @@
         for (int i = 0; i< elements; i++) {
             uart.putc(packetBuffer[i]);
         }
-        wait_ms(WAIT_TIME_MS);        // fix this!!!
+        wait_ms(5);        // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
-        wait_ms(10);
+        //wait_ms(10);
     }
+    wait_ms(10);
 }
 void ledOn(unsigned char ID)
 {
@@ -190,8 +191,8 @@
         }
         wait_ms(WAIT_TIME_MS);         // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
-        wait_ms(10);
     }
+    wait_ms(10);
 }
 
 void ledOff(unsigned char ID)
@@ -221,10 +222,10 @@
         for (int i = 0; i< elements; i++) {
             uart.putc(packetBuffer[i]);
         }
-        wait_ms(5);         // fix this!!!
+        wait_ms(WAIT_TIME_MS);         // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
-        wait_ms(10);
     }
+    wait_ms(10);
 }
 
 
@@ -255,10 +256,10 @@
         for (int i = 0; i< elements; i++) {
             uart.putc(packetBuffer[i]);
         }
-        wait_ms(WAIT_TIME_MS);         // fix this!!!
+        wait_ms(5);         // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
-        wait_ms(10);
     }
+    wait_ms(10);
 }
 
 void enCurMode(unsigned char ID)
@@ -288,7 +289,7 @@
         for (int i = 0; i< elements; i++) {
             uart.putc(packetBuffer[i]);
         }
-        wait_ms(WAIT_TIME_MS);         // fix this!!!
+        wait_ms(5);         // fix this!!!
         RTS = 0;       // Disable Tx / Enable Rx
         wait_ms(10);
     }
@@ -339,11 +340,11 @@
     RTS.mode(OpenDrainNoPull);
     RTS.output();
     pc.baud(115200);
-    uart.baud(57600);
+    uart.baud(1000000);
     RTS = 0;
     wait(1);
     
-    //setBaud(0x01, 0x01);
+    //setBaud(0x01, 0x03);
     //wait(1);
     
     pc.printf("Program is running!\r\n");
@@ -352,6 +353,7 @@
     //wait(1);
     
     
+    /*
     getPosition(0x01);
     wait(1);
 
@@ -363,27 +365,36 @@
     }
     pc.printf("\n");
     pc.printf("done!n");
+    */
     
+    /*
+    ledOn(0x01);
+    wait(1);
+    ledOff(0x01);
+    wait(1);
+    
+    pc.printf("done!n");
+    */
     
     
     
-    /*
     ledOff(0x01);
     torqueEnable(0x01,0x00);
-    enPosMode(0x01);
-    //enCurMode(0x01);
+    //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);
+    setGoalCurrent(0x01, 0xF0, 0x00);
+    //setGoalPosition(0x01,0x00,0x0F,0x00,0x00);
+    wait(1);
     ledOff(0x01);
     torqueEnable(0x01,0x00);
     //led = 1;
-    */
+    
+    
     
     
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_115200_archive.txt	Mon Sep 27 04:13:15 2021 +0000
@@ -0,0 +1,396 @@
+//ARCHIVING THIS CODE FOR LATER --> SO WE DON'T RUIN THE 115200 WORKING VERSION
+
+#include "mbed.h"
+#include "crc.h"
+
+#define WAIT_TIME_MS 2.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(WAIT_TIME_MS);         // 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(175);
+    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(WAIT_TIME_MS);         // 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(5);        // fix this!!!
+        RTS = 0;       // Disable Tx / Enable Rx
+        //wait_ms(10);
+    }
+    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(WAIT_TIME_MS);         // 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(5);         // 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(5);         // 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(115200);
+    RTS = 0;
+    wait(1);
+    
+    //setBaud(0x01, 0x02);
+    //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(1);
+    ledOff(0x01);
+    torqueEnable(0x01,0x00);
+    //led = 1;
+    */
+    
+    
+    
+    
+    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_57600_archive.txt	Mon Sep 27 04:13:15 2021 +0000
@@ -0,0 +1,393 @@
+//ARCHIVING THIS CODE FOR LATER --> SO WE DON'T RUIN THE 57600 WORKING VERSION
+
+#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;
+    */
+    
+    
+    
+    
+}