File content as of revision 3:ab8084bfcb53:

//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;
    */
    
    
    
    
    
}