Aditya Mehrotra / Mbed OS DMA_Dynamixel_Communications

Teleop Demo Code

Diff: main_57600_archive.txt

Revision:
3:ab8084bfcb53
--- /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;
+    */
+    
+    
+    
+    
+}