# 動機所-七軸手臂API說明
## 以下說明:example_20200106.py (comport2.py以及motor.py為函式庫)
### 我目前使用的軟體環境為:
* windows 10
* anaconda 1.9.2
* spyder 3.3.2
* python 3.6
### 要安裝的程式庫
####
* pyserial
* crcmod(https://anaconda.org/conda-forge/crcmod)
* struct

### 七軸馬達的datasheet,寄存器位置
#### 使用的馬達有兩種型號,分別為pro,mx。前面六顆馬達為pro系列,最後一顆馬達為mx系列。需要區分的原因為encoder換算不同,字串格式以及寄存器位置不同，以下為馬達從第一軸到第七軸採用的型號。如果沒有特殊需求，這一部分使用者不需要了解，但有助於對程式碼的了解。
* 1~2: H54-200-S500-R(A): http://emanual.robotis.com/docs/en/dxl/pro/h54-200-s500-ra/
* 3~5: H54-100-S500-R(A):http://emanual.robotis.com/docs/en/dxl/pro/h54-100-s500-ra/
* 6: H42-20-S300-R(A):http://emanual.robotis.com/docs/en/dxl/pro/h42-20-s300-ra/
* 7: MX-106R: http://emanual.robotis.com/docs/en/dxl/mx/mx-106-2/

### 整體使用的流程(由上至下依序執行每一段即可)

* 引用函式庫

import comport2
from motor import Dynamixel_Pro as pro

* 連接comport 做RS485的半雙工傳輸,baudrate為 2M。會依照每一台電腦讀取,port的名稱會不一樣! 除非USB有重新連接，不然只需要在開啟電源後，執行過一次即可。

com = comport2.RS485(port="COM4", baudrate=2000000)
com.openPort()

* 由於馬達是eeprom, 關電有些register設定的值會消失, 因此每次重新開電源都需要執行這一段,重新插拔USB則不用執行。這一段只需要執行就好，不需理解register的位置。除非電源有重新啟動，不然只需要在開啟電源後，執行過一次即可。

com.write(int(7),[578,2],64)
com.write(int(7),[580,2],108)
com.write(int(7),[582,2],109)
com.write(int(7),[584,2],110)
com.write(int(7),[586,2],111)
com.write(int(7),[588,2],112)
com.write(int(7),[590,2],113)
com.write(int(7),[592,2],114)
com.write(int(7),[594,2],115)
com.write(int(7),[596,2],116)
com.write(int(7),[598,2],117)
com.write(int(7),[600,2],118)
com.write(int(7),[602,2],119)
com.write(int(7),[604,2],128)
com.write(int(7),[606,2],129)
com.write(int(7),[608,2],130)
com.write(int(7),[610,2],131)
com.write(int(7),[612,2],132)
com.write(int(7),[614,2],133)
com.write(int(7),[616,2],134)
com.write(int(7),[618,2],135)

* 馬達激磁。馬達通電後,需要執行以下的指令,才會對收到的RS485串口命令進行馬達的轉動。除非電源有重新啟動，不然只需要在開啟電源後，執行過一次即可。

ID = [1,2,3,4,5,6,7]
data=[1,1,1,1,1,1,1]
com.syncWrite(ID,[634,1], data)

* 設定轉動速度。pro系列換算單位為: 0.01rpm; mx系列為: 0.229 rpm。因此用以下為例,pro系列馬達目前速度上限為每秒18度。mx為54.96度。也是我在11月25時的設定。除非電源有重新啟動，不然只需要在開啟電源後，執行過一次即可。

ID = [1,2,3,4,5,6,7]
data=[300,300,300,300,300,300,40]
com.syncWrite(ID,[639,4], data)

以上都完成後，就達成了馬達接上電源後的基本設定。上述的部分都不需要使用者做任何的更改，除了com的名稱以及需要更改速度時。

* 執行發送七軸角度的命令
首先將1~7軸的角度(浮點數，我一般都是輸入小數點下六位)依序填入jointAngle這個變數，如下。

jointAngle = [0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000]

然後執行以下三行，就會發送，不需要做任何更改。發送出去的命令有無完成，並無法知道，因為馬達本身的MCU並沒有內建flag的機制。但基本上只要給予足夠的時間，馬達都會走到指定的位置。推薦發送命令的頻率為100hz，每次命令間，馬達指定的移動間隔角度小於0.05度。

data_angle=[jointAngle[0],jointAngle[1],jointAngle[2],jointAngle[3],jointAngle[4],jointAngle[5],jointAngle[6]]
data_angle=[ int(pro.a2e(data_angle[0],54)) ,int(pro.a2e(data_angle[1],54)),  int(pro.a2e(data_angle[2],54)),int( pro.a2e(data_angle[3],54)) , int(pro.a2e(data_angle[4],54)), int(pro.a2e(data_angle[5],42)),int(data_angle[6]/360*4096+2048)]
com.syncWrite(ID,[643,4], data_angle)

* 執行接收七軸角度的命令
直接執行以下兩行，不需做任何更改

read_position = com.syncRead(ID,[651,4])
read_position = [pro.e2a(read_position[0],54),pro.e2a(read_position[1],54),pro.e2a(read_position[2],54),pro.e2a(read_position[3],54),pro.e2a(read_position[4],54),pro.e2a(read_position[5],42),((read_position[6]-2048)/4096*360)]

read_position即為讀取的七軸角度(浮點數)。

* 備註: 因為RS485為半雙工協定! 發送以及讀取請不要用multiprocessing 的方式同時執行! 因為半雙工協定一次只可以擇一執行發送或是讀取的命令。半雙工協定為馬達內部MCU整合的通訊晶片決定，因此無法改善為全雙工協定。