零基础入门学用Arduino 第二部分（二）

重要的内容写在前面：

该系列是以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拷贝到servo2gofromPos = 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拷贝到servo2gofromPos = 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拷贝到servo2gofromPos = 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拷贝到servo2gofromPos = 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);  //调用机械臂舵机运行函数（参数：舵机名，目标角度，单次延迟）} elseswitch(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拷贝到servo2gofromPos = 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拷贝到servo2gofromPos = 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拷贝到servo2gofromPos = 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拷贝到servo2gofromPos = 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）根据程序注释进行人工调试。