內容學自：趙虛左老師。學后總結！?
實現機器人移動的一種策略是：控制系統會先發布預期的車輛速度信息，然后驅動系統訂閱到該信息，不斷調整電機轉速直至達到預期速度，調速過程中還需要時時獲取實際速度并反饋給控制系統，控制系統會計算實際位移并生成里程計信息。
控制系統(ROS端)其實就是典型的發布和訂閱實現，而具體到驅動系統(Arduino)層面，需要解決的問題有如下幾點：帶著問題學更有目標性!
1.Arduino 如何訂閱控制系統發布的速度相關信息？
2.Arduino 如何發布實際速度相關信息到控制系統？
3.Arduino 如何驅動電機(正傳、反轉)？
4.Arduino 如何實現電機測速？
5.Arduino 如何實現電機調速？一、硬件部分
1.1?直流減速電機
需要簡單了解電機類型、機械結構以及各項參數，這些是和機器人的負載、極限速度、測速結果等休戚相關的。
????????電機主體通過輸入軸與減速箱相連接，通過減速箱的減速效果，最終外端的輸出軸會按照比例(取決于減速箱減速比)降低電機輸入軸的轉速，當然速度降低之后，將提升電機的力矩。
????????尾部是AB相霍爾編碼器，通過AB編碼器輸出的波形圖，可以判斷電機的轉向以及計算電機轉速。
需要關注的參數：
1.額定扭矩: 額定扭矩和機器人質量以及有效負荷相關，二者正比例相關，額定扭矩越大，可支持的機器人質量以及有效負荷越高；
2.減速比: 電機輸入軸與輸出軸的減速比例，比如: 減速比為90，意味著電機主體旋轉90圈，輸出軸旋轉1圈。3.減速后轉速: 與減速比相關，是電機減速箱輸出軸的轉速，單位是 rpm(轉/分)減速后轉速與減速前轉速存在轉換關系: 減速后轉速 = 減速前轉速 / 減速比。另外，可以根據官方給定的額定功率下的減速后轉速結合車輪參數來確定小車最大速度。4.編碼器精度:是指編碼器旋轉一圈單相(當前編碼器有AB兩相)輸出的脈沖數；5.注意:電機輸入軸旋轉一圈的同時，編碼器旋轉一圈.如果輸出軸旋轉一圈，那么編碼器的旋轉圈數和減速比一致(比如減速比是90，那么輸出軸旋轉一圈，編碼器旋轉90圈)。6.編碼器輸出的脈沖數計算公式則是: 輸出軸旋轉一圈產生的脈沖數 = 減速比 * 編碼器旋轉一圈發送的脈沖數(比如:減速比為90，編碼器旋轉一圈輸出11個脈沖，那么輸出軸旋轉一圈總共產生 11 * 90 也即990個脈沖)。1.2 電機編碼器?
M1: 電機電源+(和M2對調可以正反轉 )
GND: 編碼器電源-
C2: 信號線
C1: 信號線
VCC:編碼器電源+
M2: 電機電源-(和M1對調可以正反轉)?1.3?電機驅動板
電機驅動板可選型號較多，比如:TB6612、L298N、L298P....但是這些電機驅動板與電機相連時，需要使用杜邦線，接線會顯得凌亂，下面是一款基于L298P優化的電機驅動板，該驅動板可以使用端子線直接連接電機，接線更規整、美觀。
端子線母頭對應的引腳(自上而下)
母頭1: 4、地線、21、20、5V輸入、 5
母頭2: 7、地線、18、19、5V輸入、 6PS:電機驅動板使用時，需要打開USB接口處的電源開關。?二、電機基本控制
實現：控制單個電機轉動，先控制電機以某個速率正向轉動N秒，再讓電機停止N秒，再控制電機以某個速率逆向轉動N秒，最后讓電機停止N秒，如此循環。
接線：
左電機的M1與M2對應的是引腳4(DIRA)和引腳5(PWMA)，引腳4控制轉向，引腳5輸出PWM。
右電機的M1與M2對應的是引腳6(PWMB)和引腳7(DIRB)，引腳7控制轉向，引腳6輸出PWM。代碼：?
/** 電機轉動控制* 1.定義接線中電機對應的引腳* 2.setup 中設置引腳為輸出模式* 3.loop中控制電機轉動*/int DIRA = 4;
int PWMA = 5;void setup() {//兩個引腳都設置為 OUTPUTpinMode(DIRA,OUTPUT);pinMode(PWMA,OUTPUT);
}void loop() {//先正向轉動3秒digitalWrite(DIRA,HIGH);analogWrite(PWMA,100);delay(3000);//停止3秒digitalWrite(DIRA,HIGH);analogWrite(PWMA,0);delay(3000);//再反向轉動3秒digitalWrite(DIRA,LOW);analogWrite(PWMA,100);delay(3000);//停止3秒digitalWrite(DIRA,LOW);analogWrite(PWMA,0);delay(3000);/** 注意: * 1.可以通過將DIRA設置為HIGH或LOW來控制電機轉向，但是哪個標志位正轉或反轉需要根據需求判斷，轉向是相對的。* 2.PWM的取值為 [0,255],該值可自己設置。* */
}三、電機測速
3.1?編碼器測速原理
使用?AB相增量式編碼器測速。
原理：AB相編碼器主要構成為A相與B相，每一相每轉過單位的角度就發出一個脈沖信號(一圈可以發出N個脈沖信號)，A相、B相為相互延遲1/4周期的脈沖輸出，根據延遲關系可以區別正反轉，而且通過取A相、B相的上升和下降沿可以進行單頻或2倍頻或4倍頻測速。
3.2 測速舉例
假設編碼器旋轉1圈輸出11個脈沖，減速比為 90。偽代碼如下：
單頻計數:
//設置一個計數器
int count = 0;
//當A為上升沿時
if(B為高電平){count++;
}else {count--;
}
//....
//速度=單位時間內統計的脈沖的個數 / （11*90） / 單位時間
2倍頻計數:
//設置一個計數器
int count = 0;
//當A為上升沿時
if(B為高電平){count++;
}else {count--;
}
//當A為下降沿時
if(B為低電平){count++;
}else {count--;
}//....
//速度=單位時間內統計的脈沖的個數 / （11*2*90） / 單位時間
4倍頻計數:
//設置一個計數器
int count = 0;
//當A為上升沿時
if(B為高電平){count++;
}else {count--;
}
//當A為下降沿時
if(B為低電平){count++;
}else {count--;
}
//當B為上升沿時
if(A為低電平){count++;
} else {count--;
}
//當B為下降沿時
if(A為高電平){count++;
} else {count--;
}
//....
//速度=單位時間內統計的脈沖的個數 / （11*4*90） / 單位時間?3.3 案例實現
案例：統計并輸出電機轉速。
思路：先統計單位時間內以單頻或2倍頻或4倍頻的方式統計脈沖數，再除以一圈對應的脈沖數，最后再除以時間所得即為電機轉速。
核心：計數時，需要在A相或B相的上升沿或下降沿觸發時，實現計數，在此需要使用中斷引腳與中斷函數。
Arduino Mega 2560 的中斷引腳:2 (interrupt 0), 3 (interrupt 1),18 (interrupt 5), 19 (interrupt 4), 20 (interrupt 3), 21 (interrupt 2)
3.3.1?編碼實現脈沖統計
/** 測速實現:* 階段1:脈沖數統計* 階段2:速度計算* * 階段1:* 1.定義所使用的中斷引腳,以及計數器(使用 volatile 修飾)* 2.setup 中設置波特率，將引腳設置為輸入模式* 3.使用 attachInterupt() 函數為引腳添加中斷出發時機以及中斷函數* 4.中斷函數編寫計算算法，并打印* A.單頻統計只需要統計單相上升沿或下降沿* B.2倍頻統計需要統計單相的上升沿和下降沿* C.4倍頻統計需要統計兩相的上升沿和下降沿* 5.上傳并查看結果 */
int motor_A = 21;//中端口是2
int motor_B = 20;//中斷口是3
volatile int count = 0;//如果是正轉，那么每計數一次自增1，如果是反轉，那么每計數一次自減1 void count_A(){//單頻計數實現//手動旋轉電機一圈，輸出結果為 一圈脈沖數 * 減速比/*if(digitalRead(motor_A) == HIGH){if(digitalRead(motor_B) == LOW){//A 高 B 低count++; } else {//A 高 B 高count--; }}*///2倍頻計數實現//手動旋轉電機一圈，輸出結果為 一圈脈沖數 * 減速比 * 2if(digitalRead(motor_A) == HIGH){if(digitalRead(motor_B) == HIGH){//A 高 B 高count++; } else {//A 高 B 低count--; }} else {if(digitalRead(motor_B) == LOW){//A 低 B 低count++; } else {//A 低 B 高count--; } }
}//與A實現類似
//4倍頻計數實現
//手動旋轉電機一圈，輸出結果為 一圈脈沖數 * 減速比 * 4
void count_B(){if(digitalRead(motor_B) == HIGH){if(digitalRead(motor_A) == LOW){//B 高 A 低count++;} else {//B 高 A 高count--;}} else {if(digitalRead(motor_A) == HIGH){//B 低 A 高count++;} else {//B 低 A 低count--;}}
}void setup() {Serial.begin(57600);//設置波特率 pinMode(motor_A,INPUT);pinMode(motor_B,INPUT);attachInterrupt(2,count_A,CHANGE);//當電平發生改變時觸發中斷函數//四倍頻統計需要為B相也添加中斷attachInterrupt(3,count_B,CHANGE);
}void loop() {//測試計數器輸出delay(2000);Serial.println(count);
}3.3.2?轉速計算
????????需要定義一個開始時間(用于記錄每個測速周期的開始時刻)，還需要定義一個時間區間(比如50毫秒)，時時獲取當前時刻，當當前時刻 - 上傳結束時刻 >= 時間區間時，就獲取當前計數并根據測速公式計算時時速度，計算完畢，計數器歸零，重置開始時間。
????????核心知識點：當使用中斷函數中的變量時，需要先禁止中斷noInterrupts()，調用完畢，再重啟中斷interrupts()。
int reducation = 90;//減速比，根據電機參數設置，比如 15 | 30 | 60
int pulse = 11; //編碼器旋轉一圈產生的脈沖數該值需要參考商家電機參數
int per_round = pulse * reducation * 4;//車輪旋轉一圈產生的脈沖數
long start_time = millis();//一個計算周期的開始時刻，初始值為 millis();
long interval_time = 50;//一個計算周期 50ms
double current_vel;//獲取當前轉速的函數
void get_current_vel(){long right_now = millis(); long past_time = right_now - start_time;//計算逝去的時間if(past_time >= interval_time){//如果逝去時間大于等于一個計算周期//1.禁止中斷noInterrupts();//2.計算轉速 轉速單位可以是秒，也可以是分鐘... 自定義即可current_vel = (double)count / per_round / past_time * 1000 * 60;//3.重置計數器count = 0;//4.重置開始時間start_time = right_now;//5.重啟中斷interrupts();Serial.println(current_vel);}
}void loop() {delay(10);get_current_vel();}?四、電機調速（PID）
4.1?PID控制原理
PID算法是一種經典、簡單、高效的動態速度調節方式，P代表比例，I代表積分，D代表微分。
e(t) 作為 PID 控制的輸入；
u(t) 作為 PID 控制器的輸出和被控對象的輸入；
Kp 控制器的比例系數；
Ki 控制器的積分時間，也稱積分系數；
Kd 控制器的微分時間，也稱微分系數。
P
????????如果實現上述場景中的車速控制，一種簡單的實現方式是: 確定目標速度，獲取當前速度，使用
(目標速度-當前速度)*某一系數?計算結果為輸出的PWM，再獲取當前速度，使用(目標速度-當前速度)*某一系數?計算結果為輸出的PWM并輸出...如此循環在上述模型中，調速實現是一個閉環，每一次循環都會根據當前時速與目標時速的差值，再乘以以固定系數，計算出需要輸出的PWM值，這其中的系數，稱之為比例。
I
????????上述模型算法中，最終速度與預期速度存在穩態誤差，這意味著最終結果可能永遠無法達成預期，解決的方法就是使用積分I。每次調速時，輸出的PWM還要累加根據積分I計算的結果，以消除靜態誤差。
D
????????當I值設置的過大時，可能會出先"超速"的情況，超速之后可能需要多次調整，產生系統震蕩，解決這種情況可以使用D微分，當速度越是接近目標速度時，D就會越施加反方向力，減弱P的控制，起到類似”阻尼“的作用。通過D的使用可以減小系統震蕩。
綜上，PID閉環控制實現是結合了比例、積分和微分的一種控制機制，通過P可以以比例的方式計算輸出，通過I可以消除穩態誤差，通過D可以減小系統震蕩，三者相結合，最終是要快速、精準且穩定的達成預期結果，而要實現該結果，還需要對這三個數值反復測試、調整...
4.2?PID控制實現?
在Arduino中PID算法已經被封裝了，直接整合調用即可，從而提高程序的安全性與開發效率。該庫是：Arduino-PID-Library。
案例：通過PID控制電機轉速，預期轉速為 80r/m。
4.2.1?添加Arduino-PID-Library
首先在 GitHub 下載 PID 庫:
?git clone?GitHub - br3ttb/Arduino-PID-Library然后將該文件夾移動到 arduino 的 libraries下:
sudo cp -r Arduino-PID-Library /home/用戶xxx/Arduino/libraries還要重命名文件夾:
sudo mv Arduino-PID-Library ArduinoPIDLibrary最后重啟 ArduinoIDE，即可！
4.2.2 代碼
PID調速中，測速是實現閉環的關鍵實現，所以需要復制之前的電機控制代碼以及測速代碼。
/** PID 調速實現:* 1.代碼準備，復制并修改電機控制以及測速代碼* 2.包含PID頭文件* 3.創建PID對象* 4.在setup中啟用自動調試* 5.調試并更新PWM*/#include <PID_v1.h>int DIRA = 4;
int PWMA = 5;int motor_A = 21; // 中端口是2
int motor_B = 20; // 中斷口是3
volatile int count = 0; // 如果是正轉，那么每計數一次自增1，如果是反轉，那么每計數一次自減1void count_A()
{// 單頻計數實現// 手動旋轉電機一圈，輸出結果為 一圈脈沖數 * 減速比/*if(digitalRead(motor_A) == HIGH){if(digitalRead(motor_B) == LOW){//A 高 B 低count++;} else {//A 高 B 高count--;}}*/// 2倍頻計數實現// 手動旋轉電機一圈，輸出結果為 一圈脈沖數 * 減速比 * 2if (digitalRead(motor_A) == HIGH){if (digitalRead(motor_B) == HIGH){ // A 高 B 高count++;}else{ // A 高 B 低count--;}}else{if (digitalRead(motor_B) == LOW){ // A 低 B 低count++;}else{ // A 低 B 高count--;}}
}// 與A實現類似
// 4倍頻計數實現
// 手動旋轉電機一圈，輸出結果為 一圈脈沖數 * 減速比 * 4
void count_B()
{if (digitalRead(motor_B) == HIGH){if (digitalRead(motor_A) == LOW){ // B 高 A 低count++;}else{ // B 高 A 高count--;}}else{if (digitalRead(motor_A) == HIGH){ // B 低 A 高count++;}else{ // B 低 A 低count--;}}
}int reducation = 90; // 減速比，根據電機參數設置，比如 15 | 30 | 60
int pulse = 11; // 編碼器旋轉一圈產生的脈沖數該值需要參考商家電機參數
int per_round = pulse * reducation * 4; // 車輪旋轉一圈產生的脈沖數
long start_time = millis(); // 一個計算周期的開始時刻，初始值為 millis();
long interval_time = 50; // 一個計算周期 50ms
double current_vel;// 獲取當前轉速的函數
void get_current_vel()
{long right_now = millis();long past_time = right_now - start_time; // 計算逝去的時間if (past_time >= interval_time){ // 如果逝去時間大于等于一個計算周期// 1.禁止中斷noInterrupts();// 2.計算轉速 轉速單位可以是秒，也可以是分鐘... 自定義即可current_vel = (double)count / per_round / past_time * 1000 * 60;// 3.重置計數器count = 0;// 4.重置開始時間start_time = right_now;// 5.重啟中斷interrupts();Serial.println(current_vel);}
}//-------------------------------------PID-------------------------------------------
// 創建 PID 對象
// 1.當前轉速 2.計算輸出的pwm 3.目標轉速 4.kp 5.ki 6.kd 7.當輸入與目標值出現偏差時，向哪個方向控制
double pwm; // 電機驅動的PWM值
double target = 80;
double kp = 1.5, ki = 3.0, kd = 0.1;
PID pid(¤t_vel, &pwm, &target, kp, ki, kd, DIRECT);// 速度更新函數
void update_vel()
{// 獲取當前速度get_current_vel();pid.Compute(); // 計算需要輸出的PWMdigitalWrite(DIRA, HIGH);analogWrite(PWMA, pwm);
}void setup()
{Serial.begin(57600); // 設置波特率pinMode(18, INPUT);pinMode(19, INPUT);// 兩個電機驅動引腳都設置為 OUTPUTpinMode(DIRA, OUTPUT);pinMode(PWMA, OUTPUT);attachInterrupt(2, count_A, CHANGE); // 當電平發生改變時觸發中斷函數// 四倍頻統計需要為B相也添加中斷attachInterrupt(3, count_B, CHANGE);pid.SetMode(AUTOMATIC);
}void loop()
{delay(10);update_vel();
}4.2.3 核心代碼解釋
1.包含PID頭文件
#include <PID_v1.h>
2.創建PID對象
//創建 PID 對象
//1.當前轉速 2.計算輸出的pwm 3.目標轉速 4.kp 5.ki 6.kd 7.當輸入與目標值出現偏差時，向哪個方向控制
double pwm;//電機驅動的PWM值
double target = 120;
double kp=1.5, ki=3.0, kd=0.1;
PID pid(¤t_vel,&pwm,&target,kp,ki,kd,DIRECT);
3.setup中啟用PID自動控制
pid.SetMode(AUTOMATIC);
4.計算輸出值
pid.Compute();4.2.4 PID調試?
PID控制的最終預期結果，是要快速、精準、穩定的達成預期結果，P主要用于控制響應速度，I主要用于控制精度，D主要用于減小震蕩增強系統穩定性，三者的取值是需要反復調試的，調試過程中需要查看系統的響應曲線，根據響應曲線以確定合適的PID值。
在 Arduino 中響應曲線的查看可以借助于 Serial.println() 將結果輸出，然后再選擇菜單欄的工具下串口繪圖器以圖形化的方式顯示響應結果:
?☆PID調試技巧:
參數整定找最佳，從小到大順序查
先是比例后積分，最后再專把微分加
曲線振屬蕩很頻繁，比例度盤要放大
曲線漂浮繞大灣，比例度盤往小扳
曲線偏離回復慢，積分時間往下降
曲線波動周期長，積分時間再加長
曲線振蕩頻率快，先把微分降下來
動差大來波動慢。微分時間應加長
理想曲線兩個波，前高后低4比1
一看二調多分析，調節質量不會低
)
