2019年9月9日 星期一

{PCA9685}如何以緩動函數(Easing Functions)實現機器人伺服馬達的動作控制 #Arduino #ESP8266


網頁最後修改時間:2019/09/09


會認識緩動函數(Easing Functions)這個東西,是我寫完上一篇 "Q8Robot 機器人操作展示與頁面說明(R010 版本)" 之後找資料時看到的,如果有在做動畫的人,應該會對它很熟悉。

後來深入瞭解之後,看到了我想看到的東西,「怎麼用函數來模擬物體的動作方式?」簡單說,就是網頁開頭的那張圖所要表達的,「兩點位置之間的移動,可以怎麼呈現?」這也是此篇網頁要討論的重點。

接下來的內容,會一步步地說明,怎麼把方程式轉換成緩動函數?怎麼應用緩動函數控制伺服馬達?瞭解之後,不但可以應用到機器人步態動作中,也同樣能用於任何需要非常速動作的場合裡;像是呼吸燈、機器手臂動作、爬行運動、動畫呈現等...。

本篇網頁中,伺服馬達的驅動使用 PCA9685 模組。

【緩動函數之我見】

如果不懂什麼是緩動函數,可先 Google 找找資料先看看。說到底,它就是很多方程式的集合,拆解成函數之後,可以應用在很多地方。

舉個簡單的例子:呼吸燈,你會怎麼去實現?會用下面的第一種,還是第二種方法?

brightness = pwm(0 ... 255 ... 0), step 1;

brightness = pwm(255 * sin(degree)), step 1, degree 0 ... 180 ... 0;

一個是由 0 至 255,再由 255 至 0,每次增加 1;另一個是用正弦函數,由 0 至 180 度,再由 180 至 0 度,每次增減數就跟正弦函數有關係。轉換上面的虛擬碼成可執行的程式碼,就會看到兩者呈現呼吸燈效果的不同。

再舉個例子:螺桿滑塊(馬達轉動帶動滑塊做直線位移運動)由 A 位置移至 B 位置,就是加速 → 等速 → 減速。加減速的時候,就可用線性函數、三角函數或是多項式函數,這些也都算是緩動函數。

所以可以這樣理解:
  • 緩進(Ease In);
    A 位置開始使用函數處理,中間段之後等速一直至 B 位置。
  • 緩出(Ease Out);
    A 位置開始等速至中間段,之後一直至 B 位置使用函數處理。
  • 緩進+緩出(Ease In + Ease Out);
    A 位置至 B 位置都使用函數處理。
看過上面文字說明之後,應該有了淡薄仔了解。現在為了加深各位的了解,請到這個網站,玩玩它上面的 Flash;試著改變使用的函數並切換開始點與結束點的處理方式,看看小球在行進的軌跡點有何差異?

Easing Equations Flash
應該不難發現!只要開始與結束的距離是固定的,那麼不管哪一個函數處理後的兩位置間軌跡點的數目就是相等的,只差異在軌跡裡個別兩點之間的距離;這些距離的變化就是造成亮度忽亮忽滅,或是速度忽快忽滅變化的關鍵。

真實生活中的物體,不會從一個點到一個點時瞬間開始和結束,而且也幾乎從不以恆定的速度移動,物體動作時會加速與減速。例如,剛打開窗戶時動作會快一點,當它要完全開啟時動作會慢下來;又例如,球從高處往下丟,一開始會加速往下,撞擊到地面之後會反彈再掉落地面,一直反覆,這些都是真實生活中的物體的自然動作。

所以可以說:緩動會讓物體的動作感覺更自然;緩動函數就是在函數裡指定一個參數隨時間做變化,利用這個變化率來模擬物體在真實生活的動作。

緩動函數最常用在動畫的呈現上面,但在這篇網頁裡,我要把它應用到 RC 伺服馬達的旋轉控制,但不使用 Arduino 內建的 Servo 函式庫,而是改用 PCA9685 的函式庫 Adafruit PWM Servo Driver Library;因為它同時適用於 ESP8266 或其他 Arduino UNO/Nano(AVR 晶片系列)開發板,同樣的程式碼可以同時相容於兩種晶片開發。

【參考接線圖】

這裡的接線圖同時畫上了 Arduino Nano(AVR 晶片系列開發板) 和 ESP8266(NodeMCU)開發板,但實際接線的時候選擇其中一個接就好;會這樣做的原因只是為了節省頁面而已。

另外,伺服馬達用的電流比較大(SG90 至少要以 375 mA 左右來算),要使用能提供較大電流的電源模組,否則很容易造成開發板重置。怎麼看呢?看串列埠視窗的輸出,重置就會重頭開始,或是整個就不再輸出了!

PCA9685 範例程式參考接線圖
【方程式 ➜ 緩動函數】

從開始(A)位置 startPosb)移動到終點(B)位置 targetPos,彼此間距離(或稱夾角) _included_angle = targetPos - startPosc)。

選定一個方程式(EQ())作為緩動函式,設定切分時間(多久執行一次函數計算)為 SLICE_TIME,兩點間移動執行時間為 SERVOMOVE[][1],可得到總執行次數 _tick_count = SERVOMOVE[][1] / SLICE_TIMEd)。

設定 _tick = 0 ... _tick_count-1 代入 EQ() 後,再加入其他變數計算後,可得到下一個動作的位置,看起來類似下面的數學式

nextPos = startPos + { _included_angle * EQ( _tick, _tick_count) };
= b + { c * EQ(t, d) };

EQ(t, d)得到的值,範圍介在 [0.0 ~ 1.0) 之間,轉換成緩動函數時就能得到 startPos 至 targetPos 之間的值。

舉個線性移動的例子:

開始位置 startPos = 30 度,目標位置 targetPos = 130 度,兩者間距 _included_angle = 100 度;設定執行時間 SERVOMOVE[][1] = 1 秒,切分時間設定為 SLICE_TIME = 10 ms,那麼總執行次數就是 SERVOMOVE[][1] / SLICE_TIMEd= _tick_count = 100 次。

有了這些基本的資料之後,接著就要選擇方程式,這裡選擇線性方程式 y = x。

程式的執行是由 _tick 一直循環執行到 _tick_count - 1 ,因此可得到

x' = _tick / _tick_count;// [ 0.0 ... 1.0 )

接著,為了要讓 y 輸出的範圍能直接輸出在 [ 0 ... _included_angle ) ,那麼 x 就可修改為

y = x = {_included_angle * ( _tick / _tick_count ) };// [ 0.0 ... _included_angle]

緩動函數就可以寫成

function easingLinear( _tick, startPos, _included_angle, _tick_count ) {
   return startPos + _included_angle * _tick / _tick_count;
}

由這個緩動函數,依次由 _tick = 0 ... _tick_count -1 做循環求解來得到一系列的角度值,將這些角度值再轉換成 PWM 的脈衝計數,就可用來讓 PCA9685 驅動伺服馬達。

** 其他緩動函式的實現,可以參考這篇網頁,各緩動函數的參數(t, b, c, d),解釋就在上面。

【PCA9685 驅動 RC 伺服馬達說明】

有了能夠產生緩動效果的函數後,接著下來,來講講怎麼使用 PCA9685 來處理這些函數值。

PCA9685 要用於驅動 RC 伺服馬達,首先要先來了解 RC 伺服馬達驅動方式。

以下面 SG90 RC 伺服馬達為例:

SG90 伺服馬達規格表
它可以被週期性(20ms 或 50Hz)的訊號配以適當的佔空比(Duty Ratio)控制其轉動的角度,一般來說就是 (1 ms ~ 2 ms ) / 20ms,但也有些是 (0.5ms ~ 2.5ms) / 20ms,詳細就要看其資料手冊上的定義,畫成圖就如下所示。

RC 伺服馬達
PCA9685 可設定工作在 24 Hz ~ 1526 Hz 的固定頻率,有 16 個通道可產生 12-bit (4096 步)解析度的 PWM 訊號;為方便之後應用於程式碼中,下面的程式會直接套用函式庫 Adafruit PWM Servo Driver 。

基本上,要用函式庫去控制 RC 伺服馬達,首先就是要把 PWM 的頻率設成 50Hz,這樣就會產生週期為 20ms 的 PWM 訊號,符合基本頻率的要求。

Adafruit_PWMServoDriver SVDRIVER;
#define MIN       0
#define MAX       1

//...

void setup() {
   SVDRIVER.begin();
   SVDRIVER.setPWMFreq( 50 );
   //...
}

//...

再來就是需要進行一些計算,計算角度與 PWM 佔空比(或脈衝寬度)之間的關係。

這部分要先來定義幾個基本的伺服馬達參數;最大 / 最小可用的 PWM 脈衝寬度(us)和其對應的最小 / 最大的轉動角度。

int SERVO_PULSE_WIDTH_US[]       = { 700, 2400 };     // min ~ max = 700 ~ 2400 us
int SERVO_ROTATE_ANGLE_DEGREE[]  = {   0,  180 };     // 對應上面的 min ~ max = 0 ~ 180 度

中間角度對應的脈衝寬度值(inter_pwm_us)的部分,可用 map() 函式來取得

inter_pwm_us = map( degree, SERVO_ROTATE_ANGLE_DEGREE[MIN], SERVO_ROTATE_ANGLE_DEGREE[MAX],
                            SERVO_PULSE_WIDTH_US[MIN], SERVO_PULSE_WIDTH_US[MAX]);

不過取得的脈衝寬度並不能直接給 PCA9685 用,因為它所能接受的是代表脈衝寬度的 0 ~ 4095 的數字(pwm_count),剛好代表所設定頻率(50Hz)的單一週期範圍 0 ~ 20ms;所以還要將 us 轉換成 pwm_count

pwm_count = inter_pwm_us / 1000000 * 50 * 4096;

計算一下最小與最大角度的 pwm_count 範圍,就可以得到下面的式子:

143.36 143 ≤ pwm_count ≤ 491.52 491

90 度就是 1550 us ≅ 317(pwm_count)。

取得的 pwm_count 可作為下面函式的第三個引數;其中,第二個參數不要改,設為 0 就可以。

SVDRIVER.setPWM( SERVOS_PIN[0], 0, pwm_count );

安ㄋㄟ,不知道有沒看懂!

到這裡,講解的就是 Q8Robot 機器人步態單一動作單一顆伺服馬達處理的方法,有很多緩動函數可以做選擇,也可以自己創造一個,只要符合你(妳)專案 / 專題出來的效果的,沒什麼不能用。

例如,Q8Robot 的緩動函數就是選擇正弦函數,但不是選擇 0 ~ 180 度而是 90 ~ 270 度,效果就是它網頁影片上面所呈現的。

** 在寫這篇網頁的時候,Q8Robot 程式早就已經都弄好了,以目前 R010 的程式架構,並沒有能隨意變更緩動函式的功能,或許之後的改版會把這部分新增進去。

【PCA9685 - 伺服馬達的緩動函數控制範例】

Q8Robot 現在還沒有的功能,現在彌補在下面,也順便做個緩動函數的展示程式。

/*-/--*-*/*/*/*/***//-*-*-**-*/*-*-/*/*/*-*-/-////--/**/**--**/--///--//**----**//--**//**----***//*-**//*
下面程式碼已通過 Arduino Nano、ESP8266(NodeMCU)的測試,Arduino UNO 或是其他 ESP8266 模組應該也沒有問題,其他的就要自己做測試了!
/*-/--*-*/*/*/*/***//-*-*-**-*/*-*-/*/*/*-*-/-////--/**/**--**/--///--//**----**//--**//**----***//*-**//*


下面的程式碼已預先在裡面設置了三個緩動函數(以函數指標定義在 EASINGEQ[]),伺服馬達會循環這三種緩動函數(SERVOMOVE[][]),每一個循環有四個設定的角度,運行的時候記得打開 Serial Monitor。

程式碼下載

緩動函數以及移動的角度和時間都可以在程式裡自行新增以及修改,需要的資料都已提供在此網頁之中。有興趣的,可以自己再新增進去,然後再運行看看效果。

** 影片就不附了,自己動手就看的到。

【結論】

在真的了解緩動函數的每個參數的意義之後,實際上的應用並不會太複雜,反而會延伸出,想要尋找更多的方程式或是動作曲線,來做成不同的動作效果,這就是我會特意寫這篇網頁的目的。

希望此篇網頁的內容,能讓大家多熟悉一樣東西,增加寫作時,多一種的想法!


<< 部落格相關文章 >>



沒有留言:

張貼留言