紀大帥的奇妙旅程: 使用 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.

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

紀大帥的奇妙旅程

2016年3月6日星期日

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

I2C 控制 PCA9685

USB 搖桿模組

沒有留言:

張貼留言

2016年3月6日 星期日

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

I2C 控制 PCA9685

USB 搖桿模組

沒有留言:

張貼留言

2016年3月6日星期日