ELMO驅動器用arm闆子控制，并工作在 PVT(position-velocity-time)模式

Elmo驅動器 SimplIQ系列可以結合編碼器資訊，以多種模式控制伺服電機，常用的模式有速度控制模式，力矩控制模式，微步控制模式。實驗室使用的場景基本就是速度模式控制伺服電機的旋轉，對于四輪獨立驅動轉向的小車來說，通過改變PWM周期可以達到調速的目的。對于多軸機械臂來說，不僅需要速度控制，還需要以特定的速度模式工作，也就是PVT(position-velocity-time)模式，通過ROS規劃的機械臂路徑就是包含各軸運動過程的PVT資料。是以我們可以通過将ROS規劃的路徑經過三次樣條插補得到一條更加平滑的路徑(https://blog.csdn.net/qq_34935373/article/details/98482706)，然後将資料處理成elmo驅動器的PVT模式下的控制指令或者程式(多條指令特定集合)，更加精确的控制伺服電機。

首先介紹驅動器速度模式下的指令，如下所示是速度環回報過程，回報的參數在電機調試過程中确定，詳見之前部落格或者參考手冊。

速度模式下不可避免要設計到速度和加速度等資訊，特别在機械臂的運動過程中，對各軸的加速度是有限制的，ELMO驅動器中有特定的指令可以幫助我們設定最大加速度和減速度的限制，如下所示，JV是設定速度的指令，讓電機以特定的速度持續運動，PM和SF後面的PVT控制也需要用到，BG指令是指令開始執行，如果有新的程式或指令上傳到了驅動器，那麼開始執行程式。

PM上述描述說的很清楚了，推薦使用PM=1;最後一個指令SF是用來平滑加速度的，如下所示，分别使用SF=0;SF=10;SF=50檢視速度的曲線可以看出，SF較小的時候，速度是有抖動了，當SF較大的時候，速度非常平滑，但是到達指定速度所用時間更長，響應更慢。

接下來重點說明機械臂所使用的PVT控制模式，當然除了PVT模式，還有Jog持續運動模式，PTP點到點運動模式，PT位置時間模式，可以檢視相應手冊。此處使用的PVT控制方法是可以通過CAN總線來達到實時性的目的，目前使用的方式是通過序列槽将路徑的PVT資料一次性發送到驅動器，并不具有實時性。使用PV指令來選擇PVT模式。

對于PVT模式的運動，要確定第一個點的速度要是0，電機不處于運動狀态。下圖是驅動器PT和PVT模式的對比，重點關注的就是Motion buffer length 和Time between user data points兩個參數，第一個參數說明了PVT最大可以儲存的路徑點為64，超過64個路徑點就會報錯，第二個參數說明了每個路徑點之間的時間間隔範圍為【1-255】毫秒。

路徑點最多可以達到64個，規劃時間間隔在毫秒級别，每個點的PVT資料描述方式如下：

驅動器執行PVT模式的時候，内部相當于有一個指針依次讀取，是以驅動器需要知道一共有多少一個PVT路徑點和起始點，通過MP指令确定，如下所示，MP[1]定義開始的inxdex，一般設定為MP[1]=1；MP[2]定義結束的Index，例如有10個點，那就是MP[2]=10；MP[3]是兩個bit位控制，我選擇的是00，停止運動但是不結束PVT模式，是以MP[3]=0;。

注意上述指令讀取之前，需要先設定MO=0;UM=5；RM=0;PX=0;意思就是先停止電機，工作在特定模式，RM=0表示不使用feedback B，PX=0表示清零編碼器資料。如下程式使用了八個PVT點，運作的效果是先以10000cnt/s正轉到編碼器資料為1250脈沖的位置，然後反轉會原始點，#@AUTOEXEC表示上電自動執行的程式，将該程式在Elmo Studio裡面編譯上傳，然後點選紅色感歎号即可運作程式(或者斷電再上電，也會自動執行)。

#@AUTOEXEC

MO=0;UM=5;RM=0;PX=0;
MP[1]=1;MP[2]=8;MP[3]=0;

QP[1]=0;	QV[1]=0;     QT[1]=250;
QP[2]=1250; QV[2]=10000; QT[2]=250;
QP[3]=3750; QV[3]=10000; QT[3]=250;
QP[4]=5000; QV[4]=0;     QT[4]=250;
QP[5]=5000; QV[5]=0;     QT[5]=250;
QP[6]=3750; QV[6]=-10000;QT[6]=250;
QP[7]=1250; QV[7]=-10000;QT[7]=250;
QP[8]=0;    QV[8]=0;

MO=1;
PV=1;
BG
           

後面要通過序列槽發送指令的方式控制程式運作，是以不采用#@AUTOEXEC這種自動執行程式的方式，改用調用函數的方式，将開頭寫成##Func的形式，然後發送指令XQ##Func；即可調用該函數，不需要通過Elmo Studio點選感歎号執行了，和上面實作的效果是一樣的。

通過自帶的Elmo Studio 完成的PVT模式的程式設計和控制，我們的最終目的是通過闆子讀寫序列槽的方式控制驅動器，現在要解決的問題就是編譯問題，在軟體裡面點選的編譯按鈕是沒有對應的指令(可能有，我沒發現)，但是之前的手冊說了，程式也知識特定的指令按照特定的順序組合起來的東西，是以嘗試通過序列槽将上面的程式裡的指令直接發送看看效果，如下是通過序列槽調試助手的效果：

通過觀察電機的運動，發現果然可以，稍微有點不同的是，将指令按順序直接發送，因為這裡BG；指令就是執行程式的效果，如果想再調用一次，那麼可以再發送一個XQ##Func；即可。

接下來通過闆子往序列槽發送指令的方式，簡化程式和節約時間空間的角度考慮，将要發送的資料變成如下形式：

MO=0;UM=5;RM=0;PX=0;MP[1]=1;MP[2]=8;MP[3]=0;QP[1]=0;QV[1]=0;QT[1]=250;QP[2]=1250;QV[2]=10000;QT[2]=250;QP[3]=3750;QV[3]=10000;QT[3]=250;QP[4]=5000;QV[4]=0;QT[4]=250;QP[5]=5000;QV[5]=0;QT[5]=250;QP[6]=3750;QV[6]=-10000;QT[6]=250;QP[7]=1250;QV[7]=-10000;QT[7]=250;QP[8]=0;QV[8]=0;MO=1;PV=1;BG;
           

連接配接闆子，硬體參考https://blog.csdn.net/qq_34935373/article/details/103131748

#include<stdio.h>
#include<fcntl.h>
#include <string.h>
#include<unistd.h>
#include<termios.h> // using the termios.h library 

#define SLOTS   "/sys/devices/bone_capemgr.9/slots"

int main()
{
    int fd, count_r,count_t,i;
    struct termios opt;       //uart  confige structure
       
    // 加載裝置樹
    if ((fd = open(SLOTS, O_RDWR)) < 0)
    {
        perror("UART: Failed to open the UART device:ttyO1.\n");
        return -1;
    } 
    write(fd,"BB-UART2",8);
    close(fd);

    //open the UART1: read&write, No block and doesn't serve as an terminal
    if ((fd = open("/dev/ttyO2", O_RDWR | O_NOCTTY | O_NDELAY)) < 0)
    {
        perror("UART: Failed to open the UART device:ttyO1.\n");
        return -1;
    }

    tcgetattr(fd, &opt); // get the configuration of the UART

    // config UART
    opt.c_cflag = B19200 | CS8 | CREAD | CLOCAL;
    // 9600 baud, 8-bit, enable receiver, no modem control lines
    opt.c_iflag = IGNPAR | ICRNL;
    // ignore partity errors, CR -> newline
    opt.c_iflag &= ~(IXON | IXOFF | IXANY);
    //turn off software stream control
    opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    //關閉回顯功能,關閉經典輸入 改用原始輸入
    tcflush(fd,TCIOFLUSH);        // 清理輸入輸出緩沖區
    tcsetattr(fd, TCSANOW, &opt); // changes occur immmediately

    char str[] = "MO=0;UM=5;RM=0;PX=0;MP[1]=1;MP[2]=8;MP[3]=0;QP[1]=0;QV[1]=0;QT[1]=250;QP[2]=1250;QV[2]=10000;QT[2]=250;QP[3]=3750;QV[3]=10000;QT[3]=250;QP[4]=5000;QV[4]=0;QT[4]=250;QP[5]=5000;QV[5]=0;QT[5]=250;QP[6]=3750;QV[6]=-10000;QT[6]=250;QP[7]=1250;QV[7]=-10000;QT[7]=250;QP[8]=0;QV[8]=0;MO=1;PV=1;BG;";
    int len_str = strlen(str);
    unsigned char send_buff[] = {0};
    for(i=0;i<len_str;i++)
    {
        send_buff[i] = str[i];  
    }

    if ((count_t = write(fd, send_buff,len_str))<0)
    {
        perror("ERR:Failed to write to the Device:ttyO2\n");
        return -1;
    }
    usleep(5000000); // 延時 5s
    close(fd);
    return 0;
}