重要的内容写在前面：
- 该系列是以up主太极创客的零基础入门学用Arduino教程为基础制作的学习笔记。
- 个人把这个教程学完之后，整体感觉是很好的，如果有条件的可以先学习一些相关课程，学起来会更加轻松，相关课程有数字电路（强烈推荐先学数电，不然可能会有一些地方理解起来很困难）、模拟电路等，然后就是C++（注意C++是必学的）。
- 文章中的代码都是跟着老师边学边敲的，不过比起老师的版本我还把注释写得详细了些，并且个人认为重要的地方都有详细的分析。
- 一些函数的介绍有参考太极创客官网给出的中文翻译，为了便于现查现用，把个人认为重要的部分粘贴了过来并做了一些修改。
- 如有错漏欢迎指正。
视频链接：2-1 MeArm项目概述_哔哩哔哩_bilibili
太极创客官网：太极创客 – Arduino, ESP8266物联网的应用、开发和学习资料
四、开发机械臂程序
1、准备工作
（1）电路部分按下图所示连接即可。
（2）连接完成后将下面的初始化调整程序下载到开发板中，让舵机转轴转到规定的初始位置。
#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（底盘）舵机设置为初始位置
delay(100);
rArm.write(90); // 将rArm（后臂）舵机设置为初始位置
delay(100);
fArm.write(90); // 将fArm（前臂）舵机设置为初始位置
delay(100);
claw.write(90); // 将claw（钳子）舵机设置为初始位置
delay(3000);
} （3）将4个MeArm舵机摇臂按以下示意图装配到舵机上。在MeArm机械臂安装过程中不要让调整好的舵机摇臂转动，如不小心转动了已经调整好的舵机摇臂，需要将摇臂恢复图示状态或使用MeArm舵机初始化调整程序再次对舵机进行初始化调整。
（4）根据图纸或说明书或视频教程安装机械臂，安装完毕后查看电路连接是否出现问题（比如正负极短接、舵机引线接错、舵机未与Arduino共地等问题），然后再运行调试程序，看看机械臂会不会产生异动或者异响，及时调整舵机摇臂的位置或者更换有问题的舵机。
2、通过串口控制机械臂（一步到位）
（1）连接完成后将下面的程序下载到开发板中。
①全局变量及包含的头文件：
#include <Servo.h> //使用servo库
Servo base, fArm, rArm, claw ; //创建4个servo对象
//建立4个int型变量存储当前电机角度值，初始角度值为设备启动后初始状态所需要的电机角度数值
int basePos = 90;
int rArmPos = 90;
int fArmPos = 90;
int clawPos = 90;
//存储电机极限值(const指定该数值为常量，常量数值在程序运行中不能改变)
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); // 稳定性等待
rArm.attach(10); // rArm 伺服舵机连接引脚10 舵机代号'r'
delay(200); // 稳定性等待
fArm.attach(9); // fArm 伺服舵机连接引脚9 舵机代号'f'
delay(200); // 稳定性等待
claw.attach(6); // claw 伺服舵机连接引脚6 舵机代号'c'
delay(200); // 稳定性等待
Serial.begin(9600);
Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial.");
}③循环工作部分：
void loop()
{
//使用串口监视器输入电机指令控制机械臂电机
if (Serial.available() > 0)
{ //指令举例: b45，将底盘舵机调整到45度位置
char serialCmd = Serial.read(); //获取串口接收缓存中的一个字符
armDataCmd(serialCmd); //更改所记录的“当前舵机角度”
}
//根据记录的当前舵机角度进行设置
base.write(basePos);
delay(10);
fArm.write(fArmPos);
delay(10);
rArm.write(rArmPos);
delay(10);
claw.write(clawPos);
delay(10);
}④更改所记录的“当前舵机角度”：
void armDataCmd(char serialCmd)
{
Serial.print("serialCmd = ");
Serial.print(serialCmd);
int servoData = Serial.parseInt(); //获取串口接收缓存中的整数数据作为角度值
switch(serialCmd) //根据命令的第一个字符判断需要控制哪个舵机
{
case 'b':
if(servoData > baseMAX) servoData = baseMAX; //判断是否越上界
if(servoData < baseMIN) servoData = baseMIN; //判断是否越下界
basePos = servoData; //更改当前舵机角度
Serial.print(" Set base servo value: ");Serial.println(servoData);break;
case 'c':
if(servoData > clawMAX) servoData = clawMAX; //判断是否越上界
if(servoData < clawMIN) servoData = clawMIN; //判断是否越下界
clawPos = servoData; //更改当前舵机角度
Serial.print(" Set claw servo value: ");Serial.println(servoData);break;
case 'f':
if(servoData > fArmMAX) servoData = fArmMAX; //判断是否越上界
if(servoData < fArmMIN) servoData = fArmMIN; //判断是否越下界
fArmPos = servoData; //更改当前舵机角度
Serial.print(" Set fArm servo value: ");Serial.println(servoData);break;
case 'r':
if(servoData > rArmMAX) servoData = rArmMAX; //判断是否越上界
if(servoData < rArmMIN) servoData = rArmMIN; //判断是否越下界
rArmPos = servoData; //更改当前舵机角度
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）然后进行人工调试。
①通过串口助手向Arduino发送内容“b45”，机械臂的base舵机摇臂将立刻旋转至45°的位置，同理可调试其它3个舵机。
②通过串口助手向Arduino发送内容“b200”，由于200°超出base舵机的上界180°，机械臂的base舵机摇臂将立刻旋转至上界180°的位置，同理可调试其它3个舵机的上下界。（需要注意的是，上下界指的是机械臂舵机能达到的不损坏机械臂时的最大/小角度，这个角度可以对四个舵机分别进行调试而得出，每个机械臂的舵机旋转上下界可能略有差异，但只要每个舵机都经过正确的初始化调整，差异应该是很小的）
3、通过串口控制机械臂（有缓慢转动的过程）
（1）在上例中，通过Arduino直接控制舵机旋转，会发现舵机摇臂旋转的速度非常快，然而现实中大多自动工作的机械臂都是缓慢转动的，如果每一个动作都是“一气呵成”，这将增加非常多不必要的麻烦与危险，为了让机械臂缓慢转动，可以将一次大幅度的转动分成若干次小幅度的转动完成，每次小幅度转动间隔一定的时间，这样即可实现机械臂的缓慢转动。
（2）连接完成后将下面的初始化调整程序下载到开发板中，然后进行人工调试。
①全局变量及包含的头文件：
#include <Servo.h> //使用servo库
Servo base, fArm, rArm, claw ; //创建4个servo对象
//建立4个int型变量存储当前电机角度值，初始角度值为设备启动后初始状态所需要的电机角度数值
int basePos = 90;
int rArmPos = 90;
int fArmPos = 90;
int clawPos = 90;
//存储电机极限值(const指定该数值为常量，常量数值在程序运行中不能改变)
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); //稳定性等待
rArm.attach(10); //rArm 伺服舵机连接引脚10 舵机代号'r'
delay(200); //稳定性等待
fArm.attach(9); //fArm 伺服舵机连接引脚9 舵机代号'f'
delay(200); //稳定性等待
claw.attach(6); //claw 伺服舵机连接引脚6 舵机代号'c'
delay(200); //稳定性等待
Serial.begin(9600);
Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial.");
}
③循环工作部分：
void loop()
{
//使用串口监视器输入电机指令控制机械臂电机
if (Serial.available() > 0)
{ //指令举例: b45，将底盘舵机调整到45度位置
char serialCmd = Serial.read(); //获取串口接收缓存中的一个字符
armDataCmd(serialCmd); //更改所记录的“当前舵机角度”
}
//根据记录的当前舵机角度进行设置
base.write(basePos);
delay(10);
fArm.write(fArmPos);
delay(10);
rArm.write(rArmPos);
delay(10);
claw.write(clawPos);
delay(10);
}④更改所记录的“当前舵机角度”：（reportStatus函数的实现沿用上例即可）
void armDataCmd(char serialCmd)
{
Serial.print("serialCmd = ");Serial.print(serialCmd);
int servoData = Serial.parseInt(); //获取串口接收缓存中的整数数据作为角度值
int fromPos, toPos;
switch(serialCmd) //根据命令的第一个字符判断需要控制哪个舵机
{
case 'b':
fromPos = base.read(); //读取base舵机的当前角度值
toPos = servoData; //命令中的角度值作为调整后角度值
if(servoData > baseMAX) servoData = baseMAX; //判断是否越上界
if(servoData < baseMIN) servoData = baseMIN; //判断是否越下界
if (fromPos <= toPos) //如果“起始角度值”小于“目标角度值” ，每15ms向目标转动1°
for (int i=fromPos; i<=toPos; i++){base.write(i);delay(15);}
else //否则“起始角度值”大于“目标角度值”，每15ms向目标转动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舵机的当前角度值
toPos = servoData; //命令中的角度值作为调整后角度值
if(servoData > clawMAX) servoData = clawMAX; //判断是否越上界
if(servoData < clawMIN) servoData = clawMIN; //判断是否越下界
if (fromPos <= toPos) //如果“起始角度值”小于“目标角度值” ，每15ms向目标转动1°
for (int i=fromPos; i<=toPos; i++){claw.write(i);delay(15);}
else //否则“起始角度值”大于“目标角度值”，每15ms向目标转动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舵机的当前角度值
toPos = servoData; //命令中的角度值作为调整后角度值
if(servoData > fArmMAX) servoData = fArmMAX; //判断是否越上界
if(servoData < fArmMIN) servoData = fArmMIN; //判断是否越下界
if (fromPos <= toPos) //如果“起始角度值”小于“目标角度值” ，每15ms向目标转动1°
for (int i=fromPos; i<=toPos; i++){fArm.write(i);delay(15);}
else //否则“起始角度值”大于“目标角度值”，每15ms向目标转动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舵机的当前角度值
toPos = servoData; //命令中的角度值作为调整后角度值
if(servoData > rArmMAX) servoData = rArmMAX; //判断是否越上界
if(servoData < rArmMIN) servoData = rArmMIN; //判断是否越下界
if (fromPos <= toPos) //如果“起始角度值”小于“目标角度值” ，每15ms向目标转动1°
for (int i=fromPos; i<=toPos; i++){rArm.write(i);delay(15);}
else //否则“起始角度值”大于“目标角度值”，每15ms向目标转动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）然后进行人工调试。
①通过串口助手向Arduino发送内容“b45”，机械臂的base舵机摇臂将缓慢地旋转至45°的位置，同理可调试其它3个舵机。
②通过串口助手向Arduino发送内容“b200”，由于200°超出base舵机的上界180°，机械臂的base舵机摇臂将缓慢地旋转至180°的位置，同理可调试其它3个舵机的上下界。（需要注意的是，每个机械臂的舵机旋转上下界可能略有差异，但只要每个舵机都经过正确的初始化调整，差异应该是很小的）
4、通过串口控制机械臂（有设置快捷指令）
（1）电路连接完成后将下面的程序下载到开发板中。
①全局变量及包含的头文件：
#include <Servo.h> //使用servo库
Servo base, fArm, rArm, claw ; //创建4个servo对象
//存储电机极限值(const指定该数值为常量，常量数值在程序运行中不能改变)
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 (默认电机运动延迟时间)
//此变量用于控制电机运行速度，增大此变量数值将降低电机运行速度，从而控制机械臂动作速度②初始化工作部分：
void setup()
{
base.attach(11); //base 伺服舵机连接引脚11 舵机代号'b'
delay(200); //稳定性等待
rArm.attach(10); //rArm 伺服舵机连接引脚10 舵机代号'r'
delay(200); //稳定性等待
fArm.attach(9); //fArm 伺服舵机连接引脚9 舵机代号'f'
delay(200); //稳定性等待
claw.attach(6); //claw 伺服舵机连接引脚6 舵机代号'c'
delay(200); //稳定性等待
base.write(90); delay(10); //base 伺服舵机旋转角度初始化+稳定性等待
fArm.write(90); delay(10); //fArm 伺服舵机旋转角度初始化+稳定性等待
rArm.write(90); delay(10); //rArm 伺服舵机旋转角度初始化+稳定性等待
claw.write(90); delay(10); //claw 伺服舵机旋转角度初始化+稳定性等待
Serial.begin(9600);
Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial");
}
③循环工作部分：
void loop()
{
if (Serial.available() > 0)
{
char serialCmd = Serial.read(); //获取指令中的第一个字符
armDataCmd(serialCmd); //根据串行指令执行相应操作
}
}
void armDataCmd(char serialCmd)
{
if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r') //如果第一个字符是舵机代号
{
int servoData = Serial.parseInt(); //获取指令中的整数数据
servoCmd(serialCmd, servoData, DSD); //调用机械臂舵机运行函数（参数：舵机名，目标角度，单次延迟时间）
}
else
{
switch(serialCmd)
{
case 'o': //输出舵机状态信息
reportStatus();break;
case 'i': //机械臂初始化
armIniPos();break;
default: //未知指令反馈
Serial.println("Unknown Command.");
}
}
}④机械臂舵机运行函数：
void servoCmd(char servoName, int toPos, int servoDelay)
{
Servo servo2go; //创建servo对象
//串口监视器输出接收指令信息
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拷贝到servo2go
fromPos = base.read(); //获取当前base电机角度值用于“电机运动起始角度值”
break;
}
else {Serial.println("+Warning: Base Servo Value Out Of Limit!");return;}
case 'c':
if(toPos >= clawMin && toPos <= clawMax){ //判断是否越界，越界就报错
servo2go = claw; //把对象claw拷贝到servo2go
fromPos = claw.read(); //获取当前claw电机角度值用于“电机运动起始角度值”
break;
}
else {Serial.println("+Warning: Claw Servo Value Out Of Limit!");return;}
case 'f':
if(toPos >= fArmMin && toPos <= fArmMax){ //判断是否越界，越界就报错
servo2go = fArm; //把对象fArm拷贝到servo2go
fromPos = fArm.read(); //获取当前fArm电机角度值用于“电机运动起始角度值”
break;
}
else {Serial.println("+Warning: fArm Servo Value Out Of Limit!");return;}
case 'r':
if(toPos >= rArmMin && toPos <= rArmMax){ //判断是否越界，越界就报错
servo2go = rArm; //把对象rArm拷贝到servo2go
fromPos = rArm.read(); //获取当前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);}
}⑤报告舵机当前角度函数：
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("");
}⑥机械臂重新初始化函数：
void armIniPos()
{
Serial.println("+Command: Restore Initial Position.");
int robotIniPosArray[4][3] = //使用二维数组存储4个舵机的初始化信息
{/* 舵机代号 目标角度 单次延迟 */
{ 'b', 90, DSD},
{ 'r', 90, DSD},
{ 'f', 90, DSD},
{ 'c', 90, DSD}
};
for (int i = 0; i < 4; i++) //调用4次机械臂舵机运行函数，分别初始化4个舵机
{
servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);
}
}（2）然后进行人工调试。
①通过串口助手向Arduino发送内容“b45”，机械臂的base舵机摇臂将缓慢地旋转至45°的位置，同理可调试其它3个舵机。
②通过串口助手向Arduino发送内容“b200”，由于200°超出base舵机的上界180°，机械臂的base舵机不会有任何动作，同时Arduino通过串口报指令有误，同理可调试其它3个舵机。
③通过串口助手向Arduino发送内容“o”，Arduino将通过串口发送四个舵机当前的状态。
④通过串口助手向Arduino发送内容“i”，Arduino将控制四个舵机恢复初始状态。
5、通过串口控制机械臂（设有手柄控制方式）
（1）电路连接完成后将下面的程序下载到开发板中。
①全局变量及包含的头文件：
#include <Servo.h> //使用servo库
Servo base, fArm, rArm, claw ; //创建4个servo对象
//存储电机极限值(const指定该数值为常量，常量数值在程序运行中不能改变)
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 (默认电机运动延迟时间)
//此变量用于控制电机运行速度，增大此变量数值将降低电机运行速度，从而控制机械臂动作速度
bool mode; //记录当前的模式：mode = 1 —— 指令模式，mode = 0 —— 手柄模式
int moveStep = 3; //每一次按下手柄按键的舵机移动量（仅适用于手柄模式）②初始化工作部分：
void setup()
{
base.attach(11); //base 伺服舵机连接引脚11 舵机代号'b'
delay(200); //稳定性等待
rArm.attach(10); //rArm 伺服舵机连接引脚10 舵机代号'r'
delay(200); //稳定性等待
fArm.attach(9); //fArm 伺服舵机连接引脚9 舵机代号'f'
delay(200); //稳定性等待
claw.attach(6); //claw 伺服舵机连接引脚6 舵机代号'c'
delay(200); //稳定性等待
base.write(90); delay(10); //base 伺服舵机旋转角度初始化+稳定性等待
fArm.write(90); delay(10); //fArm 伺服舵机旋转角度初始化+稳定性等待
rArm.write(90); delay(10); //rArm 伺服舵机旋转角度初始化+稳定性等待
claw.write(90); delay(10); //claw 伺服舵机旋转角度初始化+稳定性等待
Serial.begin(9600);
Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial");
}③循环工作部分：
void loop()
{
if (Serial.available()>0)
{
char serialCmd = Serial.read(); //获取指令中的第一个字符
if(mode == 1) //根据mode判断现在处于什么模式
{
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); //调用机械臂舵机运行函数（参数：舵机名，目标角度，单次延迟）
}
else
switch(serialCmd)
{
case 'm': //切换至手柄模式
mode = 0; Serial.println("Command: Switch to Joy-Stick Mode.");break;
case 'o': //输出舵机状态信息
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; //目标角度=当前角度-单次操作移动角度
servoCmd('b', baseJoyPos, DSD);break; //调用机械臂舵机运行函数
case 'd': //Base向右
Serial.println("Received Command: Base Turn Right");
baseJoyPos = base.read() + moveStep; //目标角度=当前角度+单次操作移动角度
servoCmd('b', baseJoyPos, DSD);break; //调用机械臂舵机运行函数
case 's': //rArm向下
Serial.println("Received Command: Rear Arm Down");
rArmJoyPos = rArm.read() + moveStep; //目标角度=当前角度+单次操作移动角度
servoCmd('r', rArmJoyPos, DSD);break; //调用机械臂舵机运行函数
case 'w': //rArm向上
Serial.println("Received Command: Rear Arm Up");
rArmJoyPos = rArm.read() - moveStep; //目标角度=当前角度-单次操作移动角度
servoCmd('r', rArmJoyPos, DSD);break; //调用机械臂舵机运行函数
case '8': //fArm向上
Serial.println("Received Command: Front Arm Up");
fArmJoyPos = fArm.read() + moveStep; //目标角度=当前角度+单次操作移动角度
servoCmd('f', fArmJoyPos, DSD);break; //调用机械臂舵机运行函数
case '5': //fArm向下
Serial.println("Received Command: Front Arm Down");
fArmJoyPos = fArm.read() - moveStep; //目标角度=当前角度-单次操作移动角度
servoCmd('f', fArmJoyPos, DSD);break; //调用机械臂舵机运行函数
case '4': //Claw关闭
Serial.println("Received Command: Claw Close Down");
clawJoyPos = claw.read() + moveStep; //目标角度=当前角度+单次操作移动角度
servoCmd('c', clawJoyPos, DSD);break; //调用机械臂舵机运行函数
case '6': //Claw打开
Serial.println("Received Command: Claw Open Up");
clawJoyPos = claw.read() - moveStep; //目标角度=当前角度-单次操作移动角度
servoCmd('c', clawJoyPos, DSD);break; //调用机械臂舵机运行函数
case 'm': //切换至指令模式
mode = 1;
Serial.println("Command: Switch to Instruction Mode.");
break;
case 'o': //输出舵机状态信息
reportStatus();break;
case 'i': //机械臂初始化
armIniPos();break;
default: //未知指令反馈
Serial.println("Unknown Command.");return;
}
}
⑥报告舵机当前角度函数：
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("");
}⑦机械臂重新初始化函数：
void armIniPos()
{
Serial.println("+Command: Restore Initial Position.");
int robotIniPosArray[4][3] = //使用二维数组存储4个舵机的初始化信息
{/* 舵机代号 目标角度 单次延迟 */
{ 'b', 90, DSD},
{ 'r', 90, DSD},
{ 'f', 90, DSD},
{ 'c', 90, DSD}
};
for (int i = 0; i < 4; i++) //调用4次机械臂舵机运行函数，分别初始化4个舵机
{
servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);
}
}⑧机械臂舵机运行函数：
void servoCmd(char servoName, int toPos, int servoDelay)
{
Servo servo2go; //创建servo对象
//串口监视器输出接收指令信息
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拷贝到servo2go
fromPos = base.read(); //获取当前base电机角度值用于“电机运动起始角度值”
break;
}
else {Serial.println("+Warning: Base Servo Value Out Of Limit!");return;}
case 'c':
if(toPos >= clawMin && toPos <= clawMax){ //判断是否越界，越界就报错
servo2go = claw; //把对象claw拷贝到servo2go
fromPos = claw.read(); //获取当前claw电机角度值用于“电机运动起始角度值”
break;
}
else {Serial.println("+Warning: Claw Servo Value Out Of Limit!");return;}
case 'f':
if(toPos >= fArmMin && toPos <= fArmMax){ //判断是否越界，越界就报错
servo2go = fArm; //把对象fArm拷贝到servo2go
fromPos = fArm.read(); //获取当前fArm电机角度值用于“电机运动起始角度值”
break;
}
else {Serial.println("+Warning: fArm Servo Value Out Of Limit!");return;}
case 'r':
if(toPos >= rArmMin && toPos <= rArmMax){ //判断是否越界，越界就报错
servo2go = rArm; //把对象rArm拷贝到servo2go
fromPos = rArm.read(); //获取当前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）根据程序注释进行人工调试。
6、配合HC-06蓝牙模块控制机械臂
（1）按照下图所示将电路连接好。
（2）沿用上例的程序即可，手机连接上HC-06蓝牙模块，接着打开配套软件的手柄操作界面，设置好每个键所对应的指令信息，在机械臂处于手柄操作模式的前提下对其进行调试。
