File content as of revision 1:66ff8f8e65f7:

#include "mbed.h"
#include "crc.h"

Serial pc(USBTX, USBRX);

Serial uart(PA_0, PA_1);
DigitalOut RTS(D12);

//DigitalOut led(LED1);

DigitalOut dbg(D2);
DigitalOut flip(D3);

AnalogIn GainPot(A2);
DigitalIn Calibrate(D7);
DigitalIn GainUpdate(D6);

DigitalOut led(D8);

volatile uint8_t waitForReceive = 0;
volatile uint8_t nextReload = 15;

uint8_t rx_buffer[100];

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];
    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_us(200);         // fix this!!!
        RTS = 0;       // Disable Tx / Enable Rx
    }

}
void write_get_position(uint8_t id, uint8_t id2) {
    for(int i = 0; i < 100; i++) rx_buffer[i] = 0;
    
    uint8_t tx_buffer[14];
//    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;
    tx_buffer[10] = 0x04; 
    tx_buffer[11] = 0x00; 
    uint16_t 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(200);
    RTS = 0;

    
    flip = 1;
    while (waitForReceive == 1) {
    }
        
//    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);
    }
    

}

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; //torque enable
        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_us(200);        // 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(1);         // 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;
    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] = 0x50; //Kp
        packetBuffer[9] = 0x00;
        packetBuffer[10] = kd1;
        packetBuffer[11] = kd2;
        
        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(1);         // 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);
    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] = 0x54; //Kp
        packetBuffer[9] = 0x00;
        packetBuffer[10] = kp1;
        packetBuffer[11] = kp2;
        
        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(1);         // 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);
    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] = 0x58; //Feed forward acceleration
        packetBuffer[9] = 0x00;
        packetBuffer[10] = kp1;
        packetBuffer[11] = kp2;
        
        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(1);         // 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);
    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] = 0x5A; //Feed forward velocity
        packetBuffer[9] = 0x00;
        packetBuffer[10] = kp1;
        packetBuffer[11] = kp2;
        
        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(1);         // fix this!!!
        RTS = 0;       // Disable Tx / Enable Rx
        wait_ms(10);
    }
}
int main()
{
    wait_ms(300);
    RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;
    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");
    pc.printf("Program is running!\r\n");
    
   
    
    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);
    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;
    }
}