---
# System prepended metadata

title: 5-4 CyberPi控制手臂位置
tags: [Dobot進階]

---

先前我們已經經打通了用CyberPi控制Dobot手臂，但僅限於讓手臂歸零這個簡單的指令，這樣並沒有辦法實際作出甚麼應用，其最大的難點在於算出對應的UART指令內容，至少要能夠控制手臂到各個指定位置，這個方案才算是有用的

https://jacquesjohnston.wordpress.com/2019/01/29/communicating-with-the-dobot-magician-using-raw-protocol/
在這篇文章中給出一個範例，讓手臂可以移動到(X:250,Y:150,Z:50,R:150)這個座標，傳輸的UART訊號內容如下
![image](https://hackmd.io/_uploads/HkwhyRbhT.png)

用microPython的實際程式碼如下：
``` python=1
from machine import UART
import cyberpi,time

uart1 = UART(1, baudrate=115200, tx=15, rx=21)

# homing
data1 = [170,170, 6, 31,3,0,0,0,0,222]
uart1.write(bytes(data1))
c=uart1.read()
cyberpi.console.println('homing')

# move to (250,150,50,150)
data = [170,170, 19, 84,3,1,0,0,122,67,0,0,22,67,0,0,72,66,0,0,22,67,175]
uart1.write(bytes(data))
```
### 十進位轉換

整個指令中Header跟ID等設定之前有介紹過了，我們把重點擺在x=250這個設定，跟他相對應的數字串[0,0,122,67]之間的轉換，以及最後的Checksum的計算。

首先250的資料型態是一個浮點數，在電腦中浮點數會以單精度或雙精度儲存，這裡設定的是單精度32位元，也可以看作是由4個位元組所組成，而每個位元組是由8個0/1組成，例如10000110，位元組的內容也可以用兩個16進位(Hex)的數表示，還可以用一個0~255的十進位(Dec)表示。
將250這個浮點數拆解成4個十進位就會得到[67,122,0,0]，想詳細了解轉換過程可以到下列網頁工具作測試。
 https://gregstoll.com/~gregstoll/floattohex/
 
 ![image](https://hackmd.io/_uploads/H1eg5A-2a.png)
 接著由於Dobot採用的是大序端(big endian)的架構，將數列順序顛倒之後就會得到範例中所寫的[0,0,122,67]。
 
### Checksum計算
 
而UART指令最後的Checksum，僅僅只是為了怕數據傳錯所設計的數學遊戲，CyberPi這一端將Payload中所有資料經過一套公式算出一個數，也就是最後的CheckSum，跟著指令一起傳給Dobot，Dobot也用同一套公式驗算一次，答案是對的才會執行指令。

一樣以上面的指令為例，Checksum的計算公式如下:
1.先將plyload中所有整數作加總，並轉成二進位，[84,3,1,0,0,122,67,0,0,22,67,0,0,72,66,0,0,22,67]的和為593，轉成二進位是1001010001
2.取二進位的8LSB並轉成整數，8LSB即最靠右邊的8碼，此處為01010001，轉成整數為81。
3.最後用256減去前一步得出的整數，256-81=175，這樣就得出範例中最後的Checksum了。

### 用程式生成UART指令
了解上述將參數轉十進位數據及Checksum兩段計算，如果每次要將手臂移動到新位置就要重新自己計算一次UART指令未免太麻煩了，已經公式化大量重複的任務就是要交由程式來完成，在下列範例程式已經可以再程式碼當中設定(x,y,z,r)座標，執行程式後自動計算出相對應的UART指令，並下達給Dobot開始動作。
``` python=1
from machine import UART
import struct
import cyberpi,time

uart1 = UART(1, baudrate=115200, tx=15, rx=21)

UartCommand=[170,170]
length=19
ID=84
Ctrl=3
ptpmode=1

# Home 260,0,-8.5,0
# 座標參數轉成十進制
x = 260
Dx=list(struct.pack("!f",x)) # 32float to 4 decimel list
print(Dx[::-1]) # to reverse

y = 0
Dy=list(struct.pack("!f",y))
print(Dy[::-1])

z = -8.5
Dz=list(struct.pack("!f",z))
print(Dz[::-1])

r= 180
Dr=list(struct.pack("!f",r))
print(Dx[::-1]) 

# 組成payload內容
payload=[]
payload.append(ID)
payload.append(Ctrl)
payload.append(ptpmode)
payload.extend(Dx[::-1])
payload.extend(Dy[::-1])
payload.extend(Dz[::-1])
payload.extend(Dr[::-1])
print(payload)

#計算Checksum
Checksum=256 - sum(payload) & 0xff
print(Checksum)

# 組成完整指令並發送
UartCommand.append(length)
UartCommand.extend(payload)
UartCommand.append(Checksum)
print(UartCommand)

uart1.write(bytes(UartCommand))
```

雖然前面講解計算公式很複雜，但寫成程式只要一行就能解決，

`Dx=list(struct.pack("!f",x))`
將座標x的數值轉換成十進制，其中`!f`表示小端序的浮點數，`struct.pack()`用於將資料轉換為二進制，`list()`將其包裝成串列。

`Checksum=256 - sum(payload) & 0xff`
從payload串列計算出最後的Checksum，其中`& 0xff`的計算等同`& 0x11111111`可取出整數的最末位8碼，即8LSP。

其他如`append()`用於在串列後補上一個質，`extend()`則是在串列後再連上另一個串列。


### CyberPi搖桿控制手臂
最後我們想要實現出來的功能是，能用CyberPi上的按鈕加搖桿控制手臂移動，並能在螢幕上即時顯示訊息，等於是每按下一次按鈕就要重新發送一次控制位置的指令，因此我們將前一個範例中設定(x,y,z,r)座標到發送UART指令的流程，包裝成函式MoveToPoint(x,y,z,r)。
``` python=1
from machine import UART
import cyberpi,time,struct

uart1 = UART(1, baudrate=115200, tx=15, rx=21)

Sx=260;Sy=0;Sz=-8.5;Sr=0


def MoveToPoint(x,y,z,r):
    UartCommand=[170,170]
    length=19
    ID=84
    Ctrl=3
    ptpmode=1
    
    # Home 260,0,-8.5,0
    
    # x = 200
    Dx=list(struct.pack("!f",x)) # 32float to 4 decimel list
    print(Dx[::-1]) # to reverse
    
    # y = 0
    Dy=list(struct.pack("!f",y))
    print(Dy[::-1])
    
    # z = -8.5
    Dz=list(struct.pack("!f",z))
    print(Dz[::-1])
    
    # r= 0 
    Dr=list(struct.pack("!f",r))
    print(Dx[::-1]) 
    
    payload=[]
    payload.append(ID)
    payload.append(Ctrl)
    payload.append(ptpmode)
    payload.extend(Dx[::-1])
    payload.extend(Dy[::-1])
    payload.extend(Dz[::-1])
    payload.extend(Dr[::-1])
    # print(payload)
    
    Checksum=256 - sum(payload) & 0xff
    # print(Checksum)
    
    UartCommand.append(length)
    UartCommand.extend(payload)
    UartCommand.append(Checksum)
    # print(UartCommand)
    
    uart1.write(bytes(UartCommand))

while True :
    if cyberpi.controller.is_press('a'):
        cyberpi.console.print('a')
        Sz+=5
        
    if cyberpi.controller.is_press('b'):
        cyberpi.console.print('b')
        # homing commend
        data1 = [170,170, 6, 31,3,0,0,0,0,222]  
        uart1.write(bytes(data1))
        Sx=260;Sy=0;Sz=-8.5;Sr=0
        time.sleep(1)
        
    if cyberpi.controller.is_press('up'): # 檢測是否按下按鍵
        cyberpi.console.print('^')
        Sx+=5
        
    if cyberpi.controller.is_press('down'):
        cyberpi.console.print('v')
        Sx-=5
        
    if cyberpi.controller.is_press('left'):
        cyberpi.console.print('<')
        Sy+=5
        
    if cyberpi.controller.is_press('right'):
        cyberpi.console.print('>')
        Sy-=5
        
    if cyberpi.controller.is_press('middle'):
        cyberpi.console.print('o')
        Sz-=5
        
    MoveToPoint(Sx,Sy,Sz,Sr)
    time.sleep(.1)
```
接著用while True做無限環圈，每隔0.1秒判斷CyberPi上的每個按鍵是否被按下，搖桿前後左右控制手臂的XY軸，按下搖桿Z軸往下、按下A鍵Z軸往上，而按下B鍵手臂會歸零，設計CyberPi的功能時不要忘記一定要把歸零的指令加進去。
當中使用到的CyberPi函數，`cyberpi.controller.is_press('up')`回傳一按鍵是否被按下，`cyberpi.console.print()`在螢幕上輸出字串。
