Diff: main.cpp

Revision:

1:66ff8f8e65f7

Parent:

0:bcd01ae054eb

Child:

2:7afc501b8283

diff -r bcd01ae054eb -r 66ff8f8e65f7 main.cpp
--- a/main.cpp	Sat May 04 18:46:43 2019 +0000
+++ b/main.cpp	Sat Apr 17 17:49:36 2021 +0000
@@ -1,27 +1,80 @@
 #include "mbed.h"
 #include "crc.h"
 
-Serial pc(SERIAL_TX, SERIAL_RX);
+Serial pc(USBTX, USBRX);
 
-Serial uart(PA_11, PA_12);
+Serial uart(PA_0, PA_1);
 DigitalOut RTS(D12);
 
+//DigitalOut led(LED1);
+
 DigitalOut dbg(D2);
 DigitalOut flip(D3);
 
-volatile int rx_count = 0;
+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];
 
-void rxCallback() {
-    while (uart.readable()) {
-        rx_buffer[rx_count] = uart.getc();
-        rx_count++;
+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) {
+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;    
+//    rx_count = 0;    
 
     tx_buffer[0] = 0xff;
     tx_buffer[1] = 0xff;
@@ -39,39 +92,307 @@
     tx_buffer[12] = crc & 0xff;
     tx_buffer[13] = (crc >> 8) & 0xff;
     
-    printf("0x%x 0x%x\n", tx_buffer[12], tx_buffer[13]);
-    
+//    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 (rx_count < 15) {
+    while (waitForReceive == 1) {
     }
-    flip = 0;
+        
+//    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);
+    }
     
-    for (int i = 0; i < 15; i++) {
-        pc.printf("0x%x ", rx_buffer[i]);
+
+}
+
+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);
     }
-    pc.printf("\n");
+}
+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(115200);
-    uart.attach(rxCallback);
+    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");
     
-    pc.printf("Program is running!\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;
-        write_get_position(0x01);
+//        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;
-        wait(1);
     }
 }