網頁最後修改時間:2019/09/09
後來深入瞭解之後,看到了我想看到的東西,「怎麼用函數來模擬物體的動作方式?」簡單說,就是網頁開頭的那張圖所要表達的,「兩點位置之間的移動,可以怎麼呈現?」這也是此篇網頁要討論的重點。
接下來的內容,會一步步地說明,怎麼把方程式轉換成緩動函數?怎麼應用緩動函數控制伺服馬達?瞭解之後,不但可以應用到機器人步態動作中,也同樣能用於任何需要非常速動作的場合裡;像是呼吸燈、機器手臂動作、爬行運動、動畫呈現等...。
本篇網頁中,伺服馬達的驅動使用 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 位置都使用函數處理。
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 伺服馬達規格表 |
RC 伺服馬達 |
基本上,要用函式庫去控制 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 範圍,就可以得到下面的式子:
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。
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/********************************************************************** | |
* pca9685_servo_easing_function_test.ino | |
* PCA9685 - 伺服馬達的緩動函數控制範例 | |
* | |
* Author: Ruten.Proteus | |
* Date: 2019/09/06 | |
* | |
* Written by Ruten.Proteus | |
* BSD license, all text above must be included in any redistribution | |
* ******************************************************************** | |
* | |
* {Hardware} | |
* 1. NodeMCU (ESP8266) + PCA9685 Servo Driver Board | |
* 2. Arduino Nano (ATmega328) + PCA9685 Servo Driver Board | |
* | |
* {Wiring} | |
* NodeMCU PCA9685 | Arduino UNO/Nano Power Supply | |
* -------------------------------------------------------------- | |
* Vin | +5V | |
* V+ | +5V | |
* VCC | +5V | |
* | 5V +5V | |
* GND GND | GND GND | |
* D1(5) SCL | A5 | |
* D2(4) SDA | A4 | |
* -------------------------------------------------------------- | |
* SERVO PCA9685 | |
* ---------------------- | |
* 棕 GND | |
* 紅 V+ | |
* 橙 PWM | |
* ----------------------- | |
* | |
* {Result} Work !!! | |
* + 程式功能實作新增的部分,?: 待測試;o: 已過測試;-: 功能移除;x: 未過測試 | |
* [o] Arduino Nano test OK! | |
* [o] ESP8266, NodeMCU test OK! | |
* [o] 可處理執行時間小於 SLICE_TIME 的需求,讓其全速旋轉至指定的角度。 | |
* [o] 可隨意指定或加入 Easing Function 作為轉動時的動作依據。 | |
* | |
*/ | |
#include <Adafruit_PWMServoDriver.h> | |
#define MIN 0 | |
#define MAX 1 | |
Adafruit_PWMServoDriver SVDRIVER; | |
/** easingEQ 定義緩動方程式 */ | |
typedef float (*EasingEQ)( float tick, float start_pos, float next_pos, float tick_count ); | |
// cubic easing in/out - acceleration until halfway, then deceleration | |
float easeInOutCubic(float t, float b, float c, float d) { | |
t /= d/2; | |
if (t < 1) return c/2*t*t*t + b; | |
t -= 2; | |
return c/2*(t*t*t + 2) + b; | |
}; | |
// sinusoidal easing in/out - accelerating until halfway, then decelerating | |
float easeInOutSine(float t, float b, float c, float d) { | |
return -c/2 * (cos(PI*t/d) - 1) + b; | |
}; | |
//simple linear tweening - no easing, no acceleration | |
float linearTween(float t, float b, float c, float d) { | |
return c*t/d + b; | |
}; | |
const uint8_t SERVOMOVENO = 4; | |
const float SERVOMOVE[][2] = { | |
// deg, ms } | |
{ 90, 500 }, | |
{ 180, 500 }, | |
{ 0, 1000 }, | |
{ 180, 1000 }, | |
}; | |
const uint8_t EASINGEQNO = 3; | |
EasingEQ EASINGEQ[] = { | |
linearTween, | |
easeInOutSine, | |
easeInOutCubic, | |
}; | |
char* EASINGEQNAME[] = { | |
"linearTween", | |
"easeInOutSine", | |
"easeInOutCubic", | |
}; | |
// 伺服馬達脈衝寬度與角度設定 | |
int SERVO_PULSE_WIDTH_US[] = { 700, 2400 }; // min ~ max = 700 ~ 2400 us | |
int SERVO_ROTATE_ANGLE_DEGREE[] = { 0, 180 }; // 對應上面的為 min ~ max = 0 ~ 180 度 | |
// 儲存伺服馬達使用的 PCA9685 腳位 | |
const int SERVOS_PIN[8] = { 11, 10, 9, 8, 7, 6, 5, 4 }; | |
// 儲存伺服馬達位置 | |
int CURRENT_SERVOS_POSITION[8] = {90}; | |
// 切分最小時間(ms) | |
const int SLICE_TIME = 10; // 10 ms | |
unsigned long PREVIOUS_MILLIS; | |
// EASINGEQ 計算需要的參數 | |
int _tick, _tick_count, _start_angle, _included_angle; | |
// index for SERVOMOVE | |
uint8_t _idx_servomove, _idx_easingeq; | |
// 回傳給 PCA9685 的 pwm 數值 | |
int degree_to_pwm_count( float degree ) { | |
return int( map( degree, SERVO_ROTATE_ANGLE_DEGREE[MIN], SERVO_ROTATE_ANGLE_DEGREE[MAX], | |
SERVO_PULSE_WIDTH_US[MIN], SERVO_PULSE_WIDTH_US[MAX]) * 0.2048 ); | |
} | |
void setup() { | |
Serial.begin( 115200 ); | |
Serial.print("\r\n\r\n"); | |
SVDRIVER.begin(); | |
SVDRIVER.setPWMFreq( 50 ); // 50Hz = 20ms = 4096 (修改此處會影響 defree_to_pwm_count) | |
Wire.setClock(400000); | |
delay( 2000 ); | |
// 回到預設點 | |
Serial.println( F("Move to initial position: 90") ); | |
SVDRIVER.setPWM( SERVOS_PIN[0], 0, degree_to_pwm_count(90) ); | |
delay( 1000 ); | |
Serial.println( EASINGEQNAME[0] ); | |
// 相關參數初始化 | |
_tick = _tick_count = _idx_servomove = _included_angle = 0; | |
PREVIOUS_MILLIS = millis(); | |
} | |
void loop() { | |
if( (millis() - PREVIOUS_MILLIS) >= SLICE_TIME ) { | |
PREVIOUS_MILLIS = millis(); | |
if( _tick == 0 ) { | |
// 計算切分的數量 | |
if( SERVOMOVE[_idx_servomove][1] < SLICE_TIME ) { _tick = _tick_count = 1; } | |
else _tick_count = SERVOMOVE[_idx_servomove][1] / SLICE_TIME; | |
// 夾角 | |
_included_angle = SERVOMOVE[_idx_servomove][0] - CURRENT_SERVOS_POSITION[0]; | |
// 起始角度 | |
_start_angle = CURRENT_SERVOS_POSITION[0]; | |
Serial.print( "\tmove to "); Serial.print( SERVOMOVE[_idx_servomove][0] ); | |
Serial.print( "deg, " ); Serial.print( SERVOMOVE[_idx_servomove][1] ); Serial.println( " ms" ); | |
if( ++_idx_servomove == SERVOMOVENO ) { // 一個 SERVOMLOVE 指定的循環角度執行完畢 | |
_idx_servomove = 0; | |
if( ++_idx_easingeq == EASINGEQNO ) _idx_easingeq = 0; // 切換 EASINGEQ | |
Serial.println( EASINGEQNAME[_idx_easingeq] ); | |
} | |
} | |
// 切分執行 | |
CURRENT_SERVOS_POSITION[0] = constrain( EASINGEQ[_idx_easingeq]( _tick, _start_angle, _included_angle, _tick_count ), 0, 180 ); | |
SVDRIVER.setPWM( SERVOS_PIN[0], 0, degree_to_pwm_count(CURRENT_SERVOS_POSITION[0]) ); | |
// 角度到達後 | |
if( ++_tick >= _tick_count ) _tick = 0; | |
} | |
} |
緩動函數以及移動的角度和時間都可以在程式裡自行新增以及修改,需要的資料都已提供在此網頁之中。有興趣的,可以自己再新增進去,然後再運行看看效果。
** 影片就不附了,自己動手就看的到。
【結論】
在真的了解緩動函數的每個參數的意義之後,實際上的應用並不會太複雜,反而會延伸出,想要尋找更多的方程式或是動作曲線,來做成不同的動作效果,這就是我會特意寫這篇網頁的目的。
希望此篇網頁的內容,能讓大家多熟悉一樣東西,增加寫作時,多一種的想法!
<< 部落格相關文章 >>
沒有留言:
張貼留言
留言屬名為"Unknown"或"不明"的用戶,大多這樣的留言都會直接被刪除掉,不會得到任何回覆!
發問問題,請描述清楚你(妳)的問題,別人回答前不會想去 "猜" 問題是什麼?
不知道怎麼發問,請看 [公告] 部落格提問須知 - 如何問問題 !