2016年3月6日 星期日

使用 Raspberry Pi 操作伺服馬達機械手臂


重點:

1.PCA9685 模組,有I2C介面的 16 channel PWM 控制模組。

2. 無線遊戲機搖桿模組  使用PS2 和無限接收器




說明:

市面上可以輕易買到的伺服馬達,其控制方式很單純,就是輸入一個 PWM 訊號給它,然後藉由調整 PWM 訊號的脈衝寬度  來控制馬達轉軸應該轉到哪個角度並且固定維持在該角度。

主要的特性參數大致上有兩個,一個是脈衝寬度,一個是週期時間: 每發脈衝的間隔時間。伺服馬達會接收一個固定週期約 20ms ( 頻率50Hz ) 的週期性方形脈衝波 (50Hz PWM),並且會依照輸入的PWM訊號的脈衝寬度大小來調整轉軸應該轉到幾度的位置。一般來說 1.5 ms 的 PWM 訊號脈衝寬度大都是該馬達轉軸的中心位置,1.0 和 2.0 ms 的脈衝寬度對應到馬達轉軸順逆時針轉動的極限位置 ( -90 度和 +90 度)。 但是通常來說都會有許些的誤差,對於控制角度上有精確要求的話,就要想辦法報做校正的動作了。

最上面的端點是由PWM訊號產生裝置(在這裡是PCA9685)輸入 PWM 訊號給馬達用的,要注意的是馬達的主要 Power 來源並非由這裡來,這裡就只是單純的給控制訊號。最下面的端點是接地,而夾在中間的是馬達的電源輸入,看馬達的規格要小心輸入的電壓和吃電流的大小。一般來說馬達吃的電流都不小,而且在馬達運轉的時候會產生大量的雜訊,所以這個 5V 端點並不會是從 Raspberry Pi 板上直接拉出來,而是另外接一個可提供適當功率的 DC 直流電源。

I2C 控制 PCA9685


這個實驗之中至少需要 6 個 channel 的 PWM 輸出來控制伺服機械手臂, 但 Pi 的硬體 PWM 輸出數目不足。而利用軟體控制 GPIO 所實作出來的 PWM 訊號並不穩定,使用在機械手臂上的伺服馬達時,可能會讓整個手臂會不時的抖抖抖...。一般來說不會想要一支會自行抖抖抖的機械手臂。

因應這個原因才會採用外接 PWM 控制器 PCA9685 ,可以同時設定16 個 channel 各自的脈衝寬度,所以使用在伺服馬達的控制上正好合適。 特別是使用 I2C 通訊介面,使得Raspberry Pi 可以輕易地將其整合進來整個控制系統之中。

當然也要考慮到這樣的控制方案是否在反應速度上是否足夠。已知道控制伺服馬達的 PWM 訊號是 50 Hz ,也就是說伺服馬達對訊號的反應時間最快也就 20 ms。而Raspberry Pi 上的 I2C 訊號傳送速度據說預設是 100 kbits/sec,粗估傳送一個 byte 約只需要 0.01 ms,而和 PCA9685 進行一次操作設定也就最多 32 byte 的來回傳送。換句話說,Pi 和 PCA9685 一次指令的來回會在 1 ms 以下,故在反應速度上來說來得及。


USB 搖桿模組

插上在 Pi 的 USB port 之後,Linux 系統會自動認出是一個標準的 USB Joystick 模組並自動掛載,然後就可以透過檔案系統直接讀取搖桿傳過來的訊息。而網路上已經有人寫好用 Python 來和 USB Joystick 溝通的範例程式,可以參考一下。下面是範例程式所在的 Github:

https://gist.github.com/rdb/8864666
一般來說,會再把這份Code包裝成一個 class 以利使用。這樣一來,整個硬體系統架設上少了外接Arduino之後,整個系統變得非常的簡潔有力。


主程式如下:

#!/usr/bin/env python
from multiprocessing import Process, Queue
from time import sleep
import signal, os
from PCA9685 import PCA9685
from Joystick_usb import joystick_usb
from Arm import Arm
from LCM import LCM1602
pwm = ''
system_stop = False
## signal handler
def sigint_handler(signum, frame):
global system_stop
system_stop = True
## main program START
if __name__ == '__main__':
##########################
## Program initialization
##########################
signal.signal(signal.SIGINT, sigint_handler)
## START PCA9685 module
pwm = PCA9685()
pwm.reset()
pwm.wakeup()
## setup the mech arm pwm channel
motor_set_dic = {
"stage": 15,
"shoulder": 14,
"elbow": 13,
"wrist": 12,
"wrist_twist" : 11,
"finger": 10,
}
arm = Arm(pwm, motor_set_dic)
## setup the Joystick
joystick = joystick_usb()
## setup the LCM module to print the motor position
mylcd = LCM1602(debug_mode = False)
########################
## Program Main Loop
########################
while(not system_stop):
motor_pos = []
# this delay time control the response time of the arm
sleep(0.02)
if(arm.control_mode == 0):
arm.moveByJoystickPS2_mode0(joystick.read())
elif(arm.control_mode == 1):
arm.moveByJoystickPS2_mode1(joystick.read())
# send motor position information to LCM module
for i in range(6):
motor_pos.append(arm.pwm.getValChOff(15-i))
mylcd.the_queue.put(motor_pos)
######################################
# Program STOP when accept ctrl+c
######################################
joystick.close()
mylcd.close()



PCA9685:

--------------
Python & I2C:
--------------


Python 操作 I2C 通訊介面需要用到 smbus 這個模組,所以沒有安裝的話記得用apt-get 指令安裝 python-smbus。

> sudo apt-get install python-smbus

下面的 Python 範例 code 主要是透過使用 


read_i2c_block_data(addr, cmd, len) 

和 

write_i2c_block_data(addr,cmd,[data,...])

這兩個 API 來讀寫 PCA9685 裡面的暫存器數值控制 16 個 PWM。
=============
控制 PCA9685:
=============

從官方 Datasheet 所述,PCA9685 的 I2C 位置預設為 0x40。在知道 I2C 位置之後,控制 PCA9685就只剩下三個重點:

  設定PWM訊號頻率 (PWM Frequency)

  進入與離開睡眠模式

  分別設定 16 個 PWM 頻道

----------------
設定PWM訊號頻率
:
----------------

PCA9685 的 16 個 PWM 
頻道的 PWM 頻率是一樣的,設定 PWM 頻率(PWM Frequency) 的方法為直接設定 PCA9685 的 PRESCALE 暫存器。 

PRESCALE 暫存器位置為 0xFE

PRESCALE 暫存器設定值和 PWM 頻率 (Hz) 的換算公式為:

PRESCALE_value = 25000000 / ( 4096 * PWM_Frequency - 1.0)

所以這裡我們使用 PWM 頻率為 50Hz (用來控制伺服馬達),換算 PRESCALE 值約為 121。所以利用smbus提供的API的設定方法如下:

bus.write_i2c_block_data(addr , 0xFE, [121])

---------------
進入與離開睡眠模式
---------------

PCA9685 接上電之後的狀態會是處在
睡眠模式(SLEEP)之下。SLEEP模式之下,所有的PWM頻道輸出都會被關掉。所以一開始在設定好PRESCALE的值之後,必順離開SLEEP模式 PCA9685 才會有辦法輸出PWM訊號。

PCA9685 進入睡眠模式的方法為設 MODE1 暫存器(暫存器位置:0x00)的 SLEEP bit (第4個 bit)為 1 ,將其設為 0 的話就醒來

code :

def set_sleep(addr): reg_mode1 = 0x00 sleep_bit = 0x01 << 4 old_mode1_val = bus.read_i2c_block_data(addr, reg_mode1, 1) bus.write_i2c_block_data(addr, reg_mode1, [old_mode1_val | sleep_bit]) def unset_sleep(addr): reg_mode1 = 0x00 sleep_bit = 0x01 << 4 old_mode1_val = bus.read_i2c_block_data(addr, reg_mode1, 1) bus.write_i2c_block_data(addr, reg_mode1, [old_mode1_val &~(sleep_bit)])

-------------------
設定 16 個 PWM 頻道

-------------------

PCA9685 控制每個 PWM 訊號需要設定兩個值: 訊號電壓上升(ON: 由0V變5V)時間點、訊號電壓下降(OFF: 由5V變0V)時間點。



PWM 為一個固定週期的方形電波,而在PCA9685模組裡面透過PRESCALE設定時間週期的長短,而一個週期會被分成 4096 等份。所以 
PWM 訊號電壓上升(ON)和下降(OFF)兩個時間點設定值的範圍均為 0 ~ 4095。然而,要儲存這樣的值需要 12 個 bit 的記憶體空間

所以一個時間點的設定值需要用到兩個暫存器,一個用來存放值的第 0 ~ 7 bit 稱之為 low byte (L)暫存器,另一個用來存放值的第 8 ~ 11 bit 稱之為 high byte (H)暫存器。而 high byte 暫存器的第0~3 bit 被拿來存放值的第 8 ~ 11 bit,所以high byte 暫存器還剩下第 4 ~ 7 bit 其中第 4 bit 如果被設為1的話該頻道的PWM輸出會被關掉,剩下的第 5 ~ 7 bit 則棄之不用。

所以每個PWM訊號的暫存器名稱(定義在datasheet中):

LEDx_ON_L : PWM訊號電壓上升時間點的low byte暫存器。x值(0~15)為PWM頻道的號碼。
LEDx_ON_H : PWM訊號電壓上升時間點的high byte暫存器。x值(0~15)為PWM頻道的號碼。
LEDx_OFF_L: PWM訊號電壓下降時間點的low byte 暫存器。x值(0~15)為PWM頻道的號碼。 
LEDx_OFF_H: PWM訊號電壓下降時間點的high byte暫存器。x值(0~15)為PWM頻道的號碼。

這一系列的暫存器是按照 LEDx_ON_L, LEDx_ON_H, LEDx_OFF_L, LEDx_OFF_H 的順序照PWM頻道號碼依 0 ~ 15 的順序排列下去。 而 LED0_ON_L 暫存器位置為 0x06,所以剩下的暫存器位置可以用下面四個簡單的公式算出來:

LEDx_ON_L  = 0x06 + 4 * x
LEDx_ON_H  = 0x07 + 4 * x
LEDx_OFF_L = 0x08 + 4 * x
LEDx_OFF_H = 0x09 + 4 * x

code :

ef set_PWM_ON(addr , ch, value): low_byte_val = value & 0x00FF high_byte_val = ( value & 0x0F00 ) >> 8 reg_low_byte = 0x06 + 4 * ch bus.write_i2c_block_data(addr, reg_low_byte , [low_byte_val ]) bus.write_i2c_block_data(addr, reg_low_byte + 1, [high_byte_val]) def set_PWM_OFF(addr, ch, value): low_byte_val = value & 0x00FF high_byte_val = ( value & 0x0F00 ) >> 8 reg_low_byte = 0x08 + 4 * ch bus.write_i2c_block_data(addr, reg_low_byte , [low_byte_val ]) bus.write_i2c_block_data(addr, reg_low_byte + 1, [high_byte_val])

總之,如果只是想要用PWM訊號控制馬達之類的,那可以不用管所有的 LEDx_ON_L 和 LEDx_ON_H 暫存器,都給它用預設值0就好,也就是每一個週期(20ms)一開就會是ON了,接下來是什麼時間點要OFF而已,也就是只要去控制 LEDx_OFF_L 和 LEDx_OFF_H這兩個暫存器就好了。



所以,PCA9685上電後,設好PRESCALE,離開睡眠模式之後就可以開始使用了。

配合上面的範例code 使用如下:

--code--
import smbus pca9685_addr = 0x40 bus = smbus.SMBus(1) def set_PWM_ON(addr , ch, value): low_byte_val = value & 0x00FF high_byte_val = ( value & 0x0F00 ) >> 8 reg_low_byte = 0x06 + 4 * ch bus.write_i2c_block_data(addr, reg_low_byte , [low_byte_val ]) bus.write_i2c_block_data(addr, reg_low_byte + 1, [high_byte_val]) def set_PWM_OFF(addr, ch, value): low_byte_val = value & 0x00FF high_byte_val = ( value & 0x0F00 ) >> 8 reg_low_byte = 0x08 + 4 * ch bus.write_i2c_block_data(addr, reg_low_byte , [low_byte_val ]) bus.write_i2c_block_data(addr, reg_low_byte + 1, [high_byte_val]) # 設定PRESCALE PWM frequency = 50Hz bus.i2c_write_i2c_block_data(pca9685_addr, 0xFE, [121]) # 離開睡眠模式 bus.i2c_write_i2c_block_data(pca9685_addr, 0x00, [0x01]) # 設定第 0 個 PWM 頻道輸出 dutycycle = 1024/4096 set_PWM_OFF(pca9685_addr, 0, 1024) --------

還是覺得太麻煩的話就下載下面這個 github 的專案。

https://github.com/onionys/python_code

裡面有一個寫好的 PCA9685.py 可以在命令列下這樣使用:

    重設並啟動 PCA9685 (i2c 位置設定為 0x40)
> sudo PCA9685.py reset
    顯示目前所有PWM頻道的狀況
> sudo PCA9685.py info
    設定 PWM 頻道 1 其 duty-cycle 為 996/4096
> sudo PCA9685.py ch 1 996


相關文件:
1) PCA9685 DataSheet

2) 關於伺服馬達控制

0°~180° Servo 
利用PWM控制角度

 Most servo motor will work well on 50 Hz of PWM frequency;
this mean the PWM signal should have a period of 20ms.
 
 
Servo Angle Schematic 180 3.8 
 
 
This servo can operate 180° when given a pulse signal ranging from 600usec to 2400usec. 
 
#define MIN_PULSE_WIDTH       600     // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
--------------
360° Servo 
利用PWM控制轉的方向與轉速

360° servo 是藉由0.5~2.5ms  HIGH  PULSE
50Hz的脈波訊號做控制霢1.5ms  HIGH  PULSE是
位於停止的狀態;小於1.5ms  時順時霢轉動霢愈
小愈快;大於1.5ms 時霢時霢轉動霢愈大愈快。


Reference : http://blog.ittraining.com.tw/2015/04/raspberry-pi.html

Reference : http://blog.ittraining.com.tw/2015/07/raspberry-pi2-python-i2c-16-pwm-pca9685.html

沒有留言:

張貼留言