ㄆㄜㄊㄧㄡㄙˋ: ｛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）位置 startPos（b）移動到終點（B）位置 targetPos，彼此間距離（或稱夾角） _included_angle = targetPos - startPos（c）。

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

設定 _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_TIME（d）= _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。

程式碼下載

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

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

【結論】

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

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

<< 部落格相關文章 >>

Q8Robot 機器人操作展示與頁面說明（R010 版本）

ㄆㄜㄊㄧㄡㄙˋ

2019年9月9日星期一

｛PCA9685｝如何以緩動函數（Easing Functions）實現機器人伺服馬達的動作控制 #Arduino #ESP8266

沒有留言:

張貼留言

2019年9月9日 星期一

｛PCA9685｝如何以緩動函數（Easing Functions）實現機器人伺服馬達的動作控制 #Arduino #ESP8266

沒有留言:

張貼留言

2019年9月9日星期一