重要的內(nèi)容寫在前面：

1. 該系列是以up主太極創(chuàng)客的零基礎(chǔ)入門學(xué)用Arduino教程為基礎(chǔ)制作的學(xué)習(xí)筆記。
2. 個人把這個教程學(xué)完之后，整體感覺是很好的，如果有條件的可以先學(xué)習(xí)一些相關(guān)課程，學(xué)起來會更加輕松，相關(guān)課程有數(shù)字電路（強烈推薦先學(xué)數(shù)電，不然可能會有一些地方理解起來很困難）、模擬電路等，然后就是C++（注意C++是必學(xué)的）。
3. 文章中的代碼都是跟著老師邊學(xué)邊敲的，不過比起老師的版本我還把注釋寫得詳細了些，并且個人認為重要的地方都有詳細的分析。
4. 一些函數(shù)的介紹有參考太極創(chuàng)客官網(wǎng)給出的中文翻譯，為了便于現(xiàn)查現(xiàn)用，把個人認為重要的部分粘貼了過來并做了一些修改。
5. 如有錯漏歡迎指正。

視頻鏈接：2-1 MeArm項目概述_嗶哩嗶哩_bilibili

太極創(chuàng)客官網(wǎng)：太極創(chuàng)客 – Arduino, ESP8266物聯(lián)網(wǎng)的應(yīng)用、開發(fā)和學(xué)習(xí)資料

四、開發(fā)機械臂程序

1、準備工作

（1）電路部分按下圖所示連接即可。

（2）連接完成后將下面的初始化調(diào)整程序下載到開發(fā)板中，讓舵機轉(zhuǎn)軸轉(zhuǎn)到規(guī)定的初始位置。

#include <Servo.h> Servo base, rArm, fArm, claw ; ?//建立4個舵機對象void setup() 
{ base.attach(11); ? ? // base 伺服舵機連接引腳11 舵機代號'b'rArm.attach(10); ? ? // rArm 伺服舵機連接引腳10 舵機代號'r'fArm.attach(9); ? ? ?// fArm 伺服舵機連接引腳9 ?舵機代號'f'claw.attach(6); ? ? ?// claw 伺服舵機連接引腳6 ?舵機代號'c'Serial.begin(9600);
} 
void loop() 
{ base.write(90); // 將base（底盤）舵機設(shè)置為初始位置delay(100);rArm.write(90); // 將rArm（后臂）舵機設(shè)置為初始位置delay(100);fArm.write(90); // 將fArm（前臂）舵機設(shè)置為初始位置delay(100);claw.write(90); // 將claw（鉗子）舵機設(shè)置為初始位置delay(3000); 
} 

（3）將4個MeArm舵機搖臂按以下示意圖裝配到舵機上。在MeArm機械臂安裝過程中不要讓調(diào)整好的舵機搖臂轉(zhuǎn)動，如不小心轉(zhuǎn)動了已經(jīng)調(diào)整好的舵機搖臂，需要將搖臂恢復(fù)圖示狀態(tài)或使用MeArm舵機初始化調(diào)整程序再次對舵機進行初始化調(diào)整。

（4）根據(jù)圖紙或說明書或視頻教程安裝機械臂，安裝完畢后查看電路連接是否出現(xiàn)問題（比如正負極短接、舵機引線接錯、舵機未與Arduino共地等問題），然后再運行調(diào)試程序，看看機械臂會不會產(chǎn)生異動或者異響，及時調(diào)整舵機搖臂的位置或者更換有問題的舵機。

2、通過串口控制機械臂（一步到位）

（1）連接完成后將下面的程序下載到開發(fā)板中。

①全局變量及包含的頭文件：

#include <Servo.h> ? ? ? ? ? ? ? ?//使用servo庫
Servo base, fArm, rArm, claw ; ? ?//創(chuàng)建4個servo對象
//建立4個int型變量存儲當(dāng)前電機角度值，初始角度值為設(shè)備啟動后初始狀態(tài)所需要的電機角度數(shù)值
int basePos = 90;
int rArmPos = 90;
int fArmPos = 90;
int clawPos = 90;
//存儲電機極限值(const指定該數(shù)值為常量，常量數(shù)值在程序運行中不能改變)
const int baseMIN = 0;
const int baseMAX = 180;
const int rArmMIN = 45;
const int rArmMAX = 180;
const int fArmMIN = 35;
const int fArmMAX = 120;
const int clawMIN = 25;
const int clawMAX = 100;

②初始化工作部分：

void setup()
{base.attach(11); ? ? // base 伺服舵機連接引腳11 舵機代號'b'delay(200); ? ? ? ? ?// 穩(wěn)定性等待rArm.attach(10); ? ? // rArm 伺服舵機連接引腳10 舵機代號'r'delay(200); ? ? ? ? ?// 穩(wěn)定性等待fArm.attach(9); ? ? ?// fArm 伺服舵機連接引腳9 ?舵機代號'f'delay(200); ? ? ? ? ?// 穩(wěn)定性等待claw.attach(6); ? ? ?// claw 伺服舵機連接引腳6 ?舵機代號'c'delay(200); ? ? ? ? ?// 穩(wěn)定性等待Serial.begin(9600); Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial."); ? 
}

③循環(huán)工作部分：

void loop()
{//使用串口監(jiān)視器輸入電機指令控制機械臂電機if (Serial.available() > 0) { //指令舉例: b45，將底盤舵機調(diào)整到45度位置char serialCmd = Serial.read(); ?//獲取串口接收緩存中的一個字符armDataCmd(serialCmd); ? ? ? ? ? //更改所記錄的“當(dāng)前舵機角度”}//根據(jù)記錄的當(dāng)前舵機角度進行設(shè)置base.write(basePos);delay(10);fArm.write(fArmPos); delay(10);rArm.write(rArmPos); delay(10);claw.write(clawPos); ?delay(10); ? 
}

④更改所記錄的“當(dāng)前舵機角度”：

void armDataCmd(char serialCmd)
{Serial.print("serialCmd = ");Serial.print(serialCmd); ?int servoData = Serial.parseInt(); ? //獲取串口接收緩存中的整數(shù)數(shù)據(jù)作為角度值switch(serialCmd) ? ?//根據(jù)命令的第一個字符判斷需要控制哪個舵機{case 'b': ?if(servoData > baseMAX) servoData = baseMAX; ?//判斷是否越上界if(servoData < baseMIN) servoData = baseMIN; ?//判斷是否越下界basePos = servoData; ?//更改當(dāng)前舵機角度Serial.print(" ?Set base servo value: ");Serial.println(servoData);break;case 'c': ?if(servoData > clawMAX) servoData = clawMAX; ?//判斷是否越上界if(servoData < clawMIN) servoData = clawMIN; ?//判斷是否越下界clawPos = servoData; ?//更改當(dāng)前舵機角度Serial.print(" ?Set claw servo value: ");Serial.println(servoData);break;case 'f': ?if(servoData > fArmMAX) servoData = fArmMAX; ?//判斷是否越上界if(servoData < fArmMIN) servoData = fArmMIN; ?//判斷是否越下界fArmPos = servoData; ?//更改當(dāng)前舵機角度Serial.print(" ?Set fArm servo value: ");Serial.println(servoData);break;case 'r': ?if(servoData > rArmMAX) servoData = rArmMAX; ?//判斷是否越上界if(servoData < rArmMIN) servoData = rArmMIN; ?//判斷是否越下界rArmPos = servoData; ?//更改當(dāng)前舵機角度Serial.print(" ?Set rArm servo value: ");Serial.println(servoData);break;case 'o': ?reportStatus();break;default:Serial.println(" Unknown Command.");} ?
}
void reportStatus()
{Serial.println("");Serial.println("");Serial.println("++++++ Robot-Arm Status Report +++++");Serial.print("Claw Position: clawPos = "); Serial.println(claw.read());Serial.print("Base Position: basePos = "); Serial.println(base.read());Serial.print("Rear ?Arm Position: rArmPos = "); Serial.println(rArm.read());Serial.print("Front Arm Position: fArmPos = "); Serial.println(fArm.read());Serial.println("++++++++++++++++++++++++++++++++++++");Serial.println("");
}

（2）然后進行人工調(diào)試。

①通過串口助手向Arduino發(fā)送內(nèi)容“b45”，機械臂的base舵機搖臂將立刻旋轉(zhuǎn)至45°的位置，同理可調(diào)試其它3個舵機。

②通過串口助手向Arduino發(fā)送內(nèi)容“b200”，由于200°超出base舵機的上界180°，機械臂的base舵機搖臂將立刻旋轉(zhuǎn)至上界180°的位置，同理可調(diào)試其它3個舵機的上下界。（需要注意的是，上下界指的是機械臂舵機能達到的不損壞機械臂時的最大/小角度，這個角度可以對四個舵機分別進行調(diào)試而得出，每個機械臂的舵機旋轉(zhuǎn)上下界可能略有差異，但只要每個舵機都經(jīng)過正確的初始化調(diào)整，差異應(yīng)該是很小的）

3、通過串口控制機械臂（有緩慢轉(zhuǎn)動的過程）

（1）在上例中，通過Arduino直接控制舵機旋轉(zhuǎn)，會發(fā)現(xiàn)舵機搖臂旋轉(zhuǎn)的速度非?？?#xff0c;然而現(xiàn)實中大多自動工作的機械臂都是緩慢轉(zhuǎn)動的，如果每一個動作都是“一氣呵成”，這將增加非常多不必要的麻煩與危險，為了讓機械臂緩慢轉(zhuǎn)動，可以將一次大幅度的轉(zhuǎn)動分成若干次小幅度的轉(zhuǎn)動完成，每次小幅度轉(zhuǎn)動間隔一定的時間，這樣即可實現(xiàn)機械臂的緩慢轉(zhuǎn)動。

（2）連接完成后將下面的初始化調(diào)整程序下載到開發(fā)板中，然后進行人工調(diào)試。

①全局變量及包含的頭文件：

#include <Servo.h> ? ? ? ? ? ? ? ?//使用servo庫
Servo base, fArm, rArm, claw ; ? ?//創(chuàng)建4個servo對象
//建立4個int型變量存儲當(dāng)前電機角度值，初始角度值為設(shè)備啟動后初始狀態(tài)所需要的電機角度數(shù)值
int basePos = 90;
int rArmPos = 90;
int fArmPos = 90;
int clawPos = 90;
//存儲電機極限值(const指定該數(shù)值為常量，常量數(shù)值在程序運行中不能改變)
const int baseMIN = 0;
const int baseMAX = 180;
const int rArmMIN = 45;
const int rArmMAX = 180;
const int fArmMIN = 35;
const int fArmMAX = 120;
const int clawMIN = 25;
const int clawMAX = 100;

②初始化工作部分：

void setup()
{base.attach(11); ? ? //base 伺服舵機連接引腳11 舵機代號'b'delay(200); ? ? ? ? ?//穩(wěn)定性等待rArm.attach(10); ? ? //rArm 伺服舵機連接引腳10 舵機代號'r'delay(200); ? ? ? ? ?//穩(wěn)定性等待fArm.attach(9); ? ? ?//fArm 伺服舵機連接引腳9 ?舵機代號'f'delay(200); ? ? ? ? ?//穩(wěn)定性等待claw.attach(6); ? ? ?//claw 伺服舵機連接引腳6 ?舵機代號'c'delay(200); ? ? ? ? ?//穩(wěn)定性等待Serial.begin(9600); Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial."); ? 
}

③循環(huán)工作部分：

void loop()
{//使用串口監(jiān)視器輸入電機指令控制機械臂電機if (Serial.available() > 0) { //指令舉例: b45，將底盤舵機調(diào)整到45度位置char serialCmd = Serial.read(); ?//獲取串口接收緩存中的一個字符armDataCmd(serialCmd); ? ? ? ? ? //更改所記錄的“當(dāng)前舵機角度”}//根據(jù)記錄的當(dāng)前舵機角度進行設(shè)置base.write(basePos);delay(10);fArm.write(fArmPos); delay(10);rArm.write(rArmPos); delay(10);claw.write(clawPos); ?delay(10); ? 
}

④更改所記錄的“當(dāng)前舵機角度”：（reportStatus函數(shù)的實現(xiàn)沿用上例即可）

void armDataCmd(char serialCmd)
{Serial.print("serialCmd = ");Serial.print(serialCmd); ?int servoData = Serial.parseInt(); ?//獲取串口接收緩存中的整數(shù)數(shù)據(jù)作為角度值int fromPos, toPos;switch(serialCmd) ? //根據(jù)命令的第一個字符判斷需要控制哪個舵機{case 'b': ?fromPos = base.read(); ? ?//讀取base舵機的當(dāng)前角度值toPos = servoData; ? ? ? ?//命令中的角度值作為調(diào)整后角度值if(servoData > baseMAX) servoData = baseMAX; ?//判斷是否越上界if(servoData < baseMIN) servoData = baseMIN; ?//判斷是否越下界if (fromPos <= toPos) //如果“起始角度值”小于“目標角度值” ，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i<=toPos; i++){base.write(i);delay(15);}else ? ? ? ? ? ? ? ?//否則“起始角度值”大于“目標角度值”，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i>=toPos; i--){base.write(i);delay(15);}basePos = servoData;Serial.print(" ?Set base servo value: ");Serial.println(servoData);break;case 'c':fromPos = claw.read(); ? ?//讀取claw舵機的當(dāng)前角度值toPos = servoData; ? ? ? ?//命令中的角度值作為調(diào)整后角度值if(servoData > clawMAX) servoData = clawMAX; ?//判斷是否越上界if(servoData < clawMIN) servoData = clawMIN; ?//判斷是否越下界if (fromPos <= toPos) //如果“起始角度值”小于“目標角度值” ，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i<=toPos; i++){claw.write(i);delay(15);}else ? ? ? ? ? ? ? ?//否則“起始角度值”大于“目標角度值”，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i>=toPos; i--){claw.write(i);delay(15);}clawPos = servoData;Serial.print(" ?Set claw servo value: ");Serial.println(servoData);break; ?case 'f': ?fromPos = fArm.read(); ? ?//讀取fArm舵機的當(dāng)前角度值toPos = servoData; ? ? ? ?//命令中的角度值作為調(diào)整后角度值if(servoData > fArmMAX) servoData = fArmMAX; ?//判斷是否越上界if(servoData < fArmMIN) servoData = fArmMIN; ?//判斷是否越下界if (fromPos <= toPos) //如果“起始角度值”小于“目標角度值” ，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i<=toPos; i++){fArm.write(i);delay(15);}else ? ? ? ? ? ? ? ?//否則“起始角度值”大于“目標角度值”，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i>=toPos; i--){fArm.write(i);delay(15);}fArmPos = servoData;Serial.print(" ?Set fArm servo value: ");Serial.println(servoData);break;case 'r': ?fromPos = rArm.read(); ? ?//讀取rArm舵機的當(dāng)前角度值toPos = servoData; ? ? ? ?//命令中的角度值作為調(diào)整后角度值if(servoData > rArmMAX) servoData = rArmMAX; ?//判斷是否越上界if(servoData < rArmMIN) servoData = rArmMIN; ?//判斷是否越下界if (fromPos <= toPos) //如果“起始角度值”小于“目標角度值” ，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i<=toPos; i++){rArm.write(i);delay(15);}else ? ? ? ? ? ? ? ?//否則“起始角度值”大于“目標角度值”，每15ms向目標轉(zhuǎn)動1°for (int i=fromPos; i>=toPos; i--){rArm.write(i);delay(15);}rArmPos = servoData;Serial.print(" ?Set rArm servo value: ");Serial.println(servoData);break;case 'o': reportStatus();break;default: Serial.println(" Unknown Command.");} ?
}

（3）然后進行人工調(diào)試。

①通過串口助手向Arduino發(fā)送內(nèi)容“b45”，機械臂的base舵機搖臂將緩慢地旋轉(zhuǎn)至45°的位置，同理可調(diào)試其它3個舵機。

②通過串口助手向Arduino發(fā)送內(nèi)容“b200”，由于200°超出base舵機的上界180°，機械臂的base舵機搖臂將緩慢地旋轉(zhuǎn)至180°的位置，同理可調(diào)試其它3個舵機的上下界。（需要注意的是，每個機械臂的舵機旋轉(zhuǎn)上下界可能略有差異，但只要每個舵機都經(jīng)過正確的初始化調(diào)整，差異應(yīng)該是很小的）

4、通過串口控制機械臂（有設(shè)置快捷指令）

（1）電路連接完成后將下面的程序下載到開發(fā)板中。

①全局變量及包含的頭文件：

#include <Servo.h> ? ? ? ? ? ? ? ?//使用servo庫
Servo base, fArm, rArm, claw ; ? ?//創(chuàng)建4個servo對象//存儲電機極限值(const指定該數(shù)值為常量，常量數(shù)值在程序運行中不能改變)
const int baseMin = 0;
const int baseMax = 180;
const int rArmMin = 45;
const int rArmMax = 180;
const int fArmMin = 35;
const int fArmMax = 120;
const int clawMin = 25;
const int clawMax = 100;int DSD = 15; //Default Servo Delay (默認電機運動延遲時間)
//此變量用于控制電機運行速度，增大此變量數(shù)值將降低電機運行速度，從而控制機械臂動作速度

②初始化工作部分：

void setup()
{base.attach(11); ? ? //base 伺服舵機連接引腳11 舵機代號'b'delay(200); ? ? ? ? ?//穩(wěn)定性等待rArm.attach(10); ? ? //rArm 伺服舵機連接引腳10 舵機代號'r'delay(200); ? ? ? ? ?//穩(wěn)定性等待fArm.attach(9); ? ? ?//fArm 伺服舵機連接引腳9 ?舵機代號'f'delay(200); ? ? ? ? ?//穩(wěn)定性等待claw.attach(6); ? ? ?//claw 伺服舵機連接引腳6 ?舵機代號'c'delay(200); ? ? ? ? ?//穩(wěn)定性等待base.write(90); delay(10); ? ? //base 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待fArm.write(90); delay(10); ? ? //fArm 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待rArm.write(90); delay(10); ? ? //rArm 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待claw.write(90); delay(10);  ? ?//claw 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待Serial.begin(9600); Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial"); ? 
}

③循環(huán)工作部分：

void loop()
{if (Serial.available() > 0) { ?char serialCmd = Serial.read(); ?//獲取指令中的第一個字符armDataCmd(serialCmd); ? ? ? ? ? //根據(jù)串行指令執(zhí)行相應(yīng)操作}
}void armDataCmd(char serialCmd)
{if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r') ?//如果第一個字符是舵機代號{int servoData = Serial.parseInt(); ? ?//獲取指令中的整數(shù)數(shù)據(jù)servoCmd(serialCmd, servoData, DSD); ?//調(diào)用機械臂舵機運行函數(shù)（參數(shù)：舵機名，目標角度，單次延遲時間）} else {switch(serialCmd){ ? ?case 'o': ?//輸出舵機狀態(tài)信息reportStatus();break;case 'i': ?//機械臂初始化armIniPos();break; ? ? default: ? //未知指令反饋Serial.println("Unknown Command.");}} ?
}

④機械臂舵機運行函數(shù)：

void servoCmd(char servoName, int toPos, int servoDelay)
{ ?Servo servo2go; ?//創(chuàng)建servo對象//串口監(jiān)視器輸出接收指令信息Serial.println("");Serial.print("+Command: Servo ");Serial.print(servoName);Serial.print(" to ");Serial.print(toPos);Serial.print(" at servoDelay value ");Serial.print(servoDelay);Serial.println(".");Serial.println(""); ?int fromPos; //建立變量，存儲電機起始運動角度值switch(servoName) ? //根據(jù)命令的第一個字符判斷需要控制哪個舵機{case 'b':if(toPos >= baseMin && toPos <= baseMax){ ?//判斷是否越界，越界就報錯servo2go = base; ? ? ? ?//把對象base拷貝到servo2gofromPos = base.read(); ?//獲取當(dāng)前base電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: Base Servo Value Out Of Limit!");return;}case 'c':if(toPos >= clawMin && toPos <= clawMax){ ?//判斷是否越界，越界就報錯servo2go = claw; ? ? ? ?//把對象claw拷貝到servo2gofromPos = claw.read(); ?//獲取當(dāng)前claw電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: Claw Servo Value Out Of Limit!");return;}case 'f':if(toPos >= fArmMin && toPos <= fArmMax){ ?//判斷是否越界，越界就報錯servo2go = fArm; ? ? ? ?//把對象fArm拷貝到servo2gofromPos = fArm.read(); ?//獲取當(dāng)前fArm電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: fArm Servo Value Out Of Limit!");return;}case 'r':if(toPos >= rArmMin && toPos <= rArmMax){ ?//判斷是否越界，越界就報錯servo2go = rArm; ? ? ? ?//把對象rArm拷貝到servo2gofromPos = rArm.read(); ?//獲取當(dāng)前rArm電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: rArm Servo Value Out Of Limit!");return;} ?}//通過對象servo2go指揮電機運行if (fromPos <= toPos) //如果“起始角度值”小于“目標角度值”for (int i=fromPos; i<=toPos; i++){servo2go.write(i);delay(servoDelay);}else ? ? ? ? ? ? ? ? ?//否則“起始角度值”大于“目標角度值”for (int i=fromPos; i>=toPos; i--){servo2go.write(i);delay(servoDelay);}
}

⑤報告舵機當(dāng)前角度函數(shù)：

void reportStatus()
{Serial.println("");Serial.println("");Serial.println("+ Robot-Arm Status Report +");Serial.print("Claw Position: "); Serial.println(claw.read());Serial.print("Base Position: "); Serial.println(base.read());Serial.print("Rear ?Arm Position:"); Serial.println(rArm.read());Serial.print("Front Arm Position:"); Serial.println(fArm.read());Serial.println("++++++++++++++++++++++++++");Serial.println("");
}

⑥機械臂重新初始化函數(shù)：

void armIniPos()
{Serial.println("+Command: Restore Initial Position.");int robotIniPosArray[4][3] = ? //使用二維數(shù)組存儲4個舵機的初始化信息{/* ?舵機代號 目標角度 單次延遲  */{ ? ?'b', ? ? 90, ? ?DSD},{ ? ?'r', ? ? 90, ? ?DSD},{ ? ?'f', ? ? 90, ? ?DSD},{ ? ?'c', ? ? 90, ? ?DSD} }; ? for (int i = 0; i < 4; i++) ?//調(diào)用4次機械臂舵機運行函數(shù)，分別初始化4個舵機{servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);}
}

（2）然后進行人工調(diào)試。

①通過串口助手向Arduino發(fā)送內(nèi)容“b45”，機械臂的base舵機搖臂將緩慢地旋轉(zhuǎn)至45°的位置，同理可調(diào)試其它3個舵機。

②通過串口助手向Arduino發(fā)送內(nèi)容“b200”，由于200°超出base舵機的上界180°，機械臂的base舵機不會有任何動作，同時Arduino通過串口報指令有誤，同理可調(diào)試其它3個舵機。

③通過串口助手向Arduino發(fā)送內(nèi)容“o”，Arduino將通過串口發(fā)送四個舵機當(dāng)前的狀態(tài)。

④通過串口助手向Arduino發(fā)送內(nèi)容“i”，Arduino將控制四個舵機恢復(fù)初始狀態(tài)。

5、通過串口控制機械臂（設(shè)有手柄控制方式）

（1）電路連接完成后將下面的程序下載到開發(fā)板中。

①全局變量及包含的頭文件：

#include <Servo.h> ? ? ? ? ? ? ? ?//使用servo庫
Servo base, fArm, rArm, claw ; ? ?//創(chuàng)建4個servo對象//存儲電機極限值(const指定該數(shù)值為常量，常量數(shù)值在程序運行中不能改變)
const int baseMin = 0;
const int baseMax = 180;
const int rArmMin = 45;
const int rArmMax = 180;
const int fArmMin = 35;
const int fArmMax = 120;
const int clawMin = 25;
const int clawMax = 100;int DSD = 15; //Default Servo Delay (默認電機運動延遲時間)
//此變量用于控制電機運行速度，增大此變量數(shù)值將降低電機運行速度，從而控制機械臂動作速度bool mode;        ? //記錄當(dāng)前的模式：mode = 1 —— 指令模式，mode = 0 —— 手柄模式
int moveStep = 3; ?//每一次按下手柄按鍵的舵機移動量（僅適用于手柄模式）

②初始化工作部分：

void setup()
{base.attach(11); ? ? //base 伺服舵機連接引腳11 舵機代號'b'delay(200); ? ? ? ? ?//穩(wěn)定性等待rArm.attach(10); ? ? //rArm 伺服舵機連接引腳10 舵機代號'r'delay(200); ? ? ? ? ?//穩(wěn)定性等待fArm.attach(9); ? ? ?//fArm 伺服舵機連接引腳9 ?舵機代號'f'delay(200); ? ? ? ? ?//穩(wěn)定性等待claw.attach(6); ? ? ?//claw 伺服舵機連接引腳6 ?舵機代號'c'delay(200); ? ? ? ? ?//穩(wěn)定性等待base.write(90); delay(10); ? ? //base 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待fArm.write(90); delay(10); ? ? //fArm 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待rArm.write(90); delay(10); ? ? //rArm 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待claw.write(90); delay(10);  ? ?//claw 伺服舵機旋轉(zhuǎn)角度初始化+穩(wěn)定性等待Serial.begin(9600); Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial"); ? 
}

③循環(huán)工作部分：

void loop()
{if (Serial.available()>0) { ?char serialCmd = Serial.read(); ?//獲取指令中的第一個字符if(mode == 1) ?//根據(jù)mode判斷現(xiàn)在處于什么模式{armDataCmd(serialCmd); ?//指令模式} else {armJoyCmd(serialCmd); ? //手柄模式}}
}

④指令模式下的處理邏輯：

void armDataCmd(char serialCmd)
{//判斷用戶是否因搞錯模式而輸入錯誤的指令信息（即指令模式下輸入手柄按鍵信息）if ( ? serialCmd == 'w' || serialCmd == 's' || serialCmd == 'a' || serialCmd == 'd'|| serialCmd == '5' || serialCmd == '4' || serialCmd == '6' || serialCmd == '8' ){Serial.println("+Warning: Robot in Instruction Mode..."); delay(100);while(Serial.available() > 0) char wrongCommand = Serial.read(); ?//清除串口緩存中的錯誤指令return;}  ? ? ? ? ? ? ? if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){int servoData = Serial.parseInt();servoCmd(serialCmd, servoData, DSD); ?//調(diào)用機械臂舵機運行函數(shù)（參數(shù)：舵機名，目標角度，單次延遲）} elseswitch(serialCmd){ ? case 'm': ?  //切換至手柄模式 mode = 0; Serial.println("Command: Switch to Joy-Stick Mode.");break;case 'o': ? ?//輸出舵機狀態(tài)信息reportStatus();break;case 'i': ? ?//機械臂初始化armIniPos();break; ?default: ? ? //未知指令反饋Serial.println("Unknown Command.");} ?
}

⑤手柄模式下的處理邏輯：

void armJoyCmd(char serialCmd)
{//判斷用戶是否因搞錯模式而輸入錯誤的指令信息（即手柄模式下輸入舵機指令）if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){Serial.println("+Warning: Robot in Joy-Stick Mode...");delay(100);while(Serial.available()>0) char wrongCommand = Serial.read(); ?//清除串口緩存中的錯誤指令return;} int baseJoyPos, rArmJoyPos, fArmJoyPos, clawJoyPos;switch(serialCmd){case 'a': ?//Base向左Serial.println("Received Command: Base Turn Left"); ? ? ? ? ? ? ? ?baseJoyPos = base.read() - moveStep;? //目標角度=當(dāng)前角度-單次操作移動角度servoCmd('b', baseJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù)?case 'd': ?//Base向右Serial.println("Received Command: Base Turn Right"); ? ? ? ? ? ? ? ?baseJoyPos = base.read() + moveStep;? //目標角度=當(dāng)前角度+單次操作移動角度servoCmd('b', baseJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ? ? ? ?case 's': ?//rArm向下Serial.println("Received Command: Rear Arm Down"); ? ? ? ? ? ? ? ?rArmJoyPos = rArm.read() + moveStep;? //目標角度=當(dāng)前角度+單次操作移動角度servoCmd('r', rArmJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ? ? ? ? ? ? ?case 'w': ?//rArm向上Serial.println("Received Command: Rear Arm Up"); ? ? rArmJoyPos = rArm.read() - moveStep;? //目標角度=當(dāng)前角度-單次操作移動角度servoCmd('r', rArmJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ?case '8': ?//fArm向上Serial.println("Received Command: Front Arm Up"); ? ? ? ?fArmJoyPos = fArm.read() + moveStep;? //目標角度=當(dāng)前角度+單次操作移動角度servoCmd('f', fArmJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ?case '5': ?//fArm向下Serial.println("Received Command: Front Arm Down"); ? ? ? ?fArmJoyPos = fArm.read() - moveStep;? //目標角度=當(dāng)前角度-單次操作移動角度servoCmd('f', fArmJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ?case '4': ?//Claw關(guān)閉Serial.println("Received Command: Claw Close Down"); ? ? ? ?clawJoyPos = claw.read() + moveStep;? //目標角度=當(dāng)前角度+單次操作移動角度servoCmd('c', clawJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ?case '6': ?//Claw打開Serial.println("Received Command: Claw Open Up"); ? ? clawJoyPos = claw.read() - moveStep;? //目標角度=當(dāng)前角度-單次操作移動角度servoCmd('c', clawJoyPos, DSD);break; //調(diào)用機械臂舵機運行函數(shù) ?case 'm': ? //切換至指令模式 mode = 1; Serial.println("Command: Switch to Instruction Mode.");break;case 'o': ? //輸出舵機狀態(tài)信息reportStatus();break;case 'i': ? //機械臂初始化armIniPos();break;default: ? ?//未知指令反饋Serial.println("Unknown Command.");return;} ?
}

⑥報告舵機當(dāng)前角度函數(shù)：

void reportStatus()
{Serial.println("");Serial.println("");Serial.println("+ Robot-Arm Status Report +");Serial.print("Claw Position: "); Serial.println(claw.read());Serial.print("Base Position: "); Serial.println(base.read());Serial.print("Rear ?Arm Position:"); Serial.println(rArm.read());Serial.print("Front Arm Position:"); Serial.println(fArm.read());Serial.println("++++++++++++++++++++++++++");Serial.println("");
}

⑦機械臂重新初始化函數(shù)：

void armIniPos()
{Serial.println("+Command: Restore Initial Position.");int robotIniPosArray[4][3] = ? //使用二維數(shù)組存儲4個舵機的初始化信息{/* ?舵機代號 目標角度 單次延遲  */{ ? ?'b', ? ? 90, ? ?DSD},{ ? ?'r', ? ? 90, ? ?DSD},{ ? ?'f', ? ? 90, ? ?DSD},{ ? ?'c', ? ? 90, ? ?DSD} }; ? for (int i = 0; i < 4; i++) ?//調(diào)用4次機械臂舵機運行函數(shù)，分別初始化4個舵機{servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);}
}

⑧機械臂舵機運行函數(shù)：

void servoCmd(char servoName, int toPos, int servoDelay)
{ ?Servo servo2go; ?//創(chuàng)建servo對象//串口監(jiān)視器輸出接收指令信息Serial.println("");Serial.print("+Command: Servo ");Serial.print(servoName);Serial.print(" to ");Serial.print(toPos);Serial.print(" at servoDelay value ");Serial.print(servoDelay);Serial.println(".");Serial.println(""); ?int fromPos; //建立變量，存儲電機起始運動角度值switch(servoName){case 'b':if(toPos >= baseMin && toPos <= baseMax){ ?//判斷是否越界，越界就報錯servo2go = base; ? ? ? ?//把對象base拷貝到servo2gofromPos = base.read(); ?//獲取當(dāng)前base電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: Base Servo Value Out Of Limit!");return;}case 'c':if(toPos >= clawMin && toPos <= clawMax){ ?//判斷是否越界，越界就報錯servo2go = claw; ? ? ? ?//把對象claw拷貝到servo2gofromPos = claw.read(); ?//獲取當(dāng)前claw電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: Claw Servo Value Out Of Limit!");return;}case 'f':if(toPos >= fArmMin && toPos <= fArmMax){ ?//判斷是否越界，越界就報錯servo2go = fArm; ? ? ? ?//把對象fArm拷貝到servo2gofromPos = fArm.read(); ?//獲取當(dāng)前fArm電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: fArm Servo Value Out Of Limit!");return;}case 'r':if(toPos >= rArmMin && toPos <= rArmMax){ ?//判斷是否越界，越界就報錯servo2go = rArm; ? ? ? ?//把對象rArm拷貝到servo2gofromPos = rArm.read(); ?//獲取當(dāng)前rArm電機角度值用于“電機運動起始角度值”break;} else {Serial.println("+Warning: rArm Servo Value Out Of Limit!");return;} ?}//通過對象servo2go指揮電機運行if (fromPos <= toPos) //如果“起始角度值”小于“目標角度值”for (int i=fromPos; i<=toPos; i++){servo2go.write(i);delay(servoDelay);}else ? ? ? ? ? ? ? ? ?//否則“起始角度值”大于“目標角度值”for (int i=fromPos; i>=toPos; i--){servo2go.write(i);delay(servoDelay);}
}

（2）根據(jù)程序注釋進行人工調(diào)試。

6、配合HC-06藍牙模塊控制機械臂

（1）按照下圖所示將電路連接好。

（2）沿用上例的程序即可，手機連接上HC-06藍牙模塊，接著打開配套軟件的手柄操作界面，設(shè)置好每個鍵所對應(yīng)的指令信息，在機械臂處于手柄操作模式的前提下對其進行調(diào)試。