EEPW论坛

Arduino 太极创客 机械臂 在拼多多上买到62元，

但背后蕴含的物联网应用知识确实无价的宝藏，

以下代码是自己修改的，可以直接烧录：

/*MeArm机械臂控制程序-6by 太极创客 (2017-06-02)www.taichi-maker.com 本程序用于太极创客《零基础入门学用Arduino教程 - MeArm篇》。1   使用串口获得电机数据,设置机械臂动作    将串口指令解读改写为函数,加入状态汇报函数reportStatus2   加入servoCmd函数控制电机速度3   加入判断人类指令是否超出舵机限值4   MeArm执行一系列动作(执行程序) armIniPos - 1 （一维数组）5   MeArm执行一系列动作(执行程序) armIniPos - 2 （二维数组）6   加入手柄控制模式    加入机器判断：人类输入信息错误检查 如需要获得具体电路连接细节，请查阅太极创客制作的《零基础入门学用Arduino教程 - MeArm篇》页面。 */#include <Servo.h>                //使用servo库
Servo myservo; // 创建一个Servo对象
Servo base, fArm, rArm, claw ;    //创建4个servo对象
/*Servo baseServo,rArmServo,fArmServo,clawServo; // 在全局作用域中声明变量*/
// 创建四个Servo对象Servo baseServo;Servo shoulderServo;Servo elbowServo;Servo wristServo;
  char command; // 声明变量command
//Servo myservo; // 创建一个Servo对象const int minAngle = 0; // 舵机的最小角度限制const int maxAngle = 180; // 舵机的最大角度限制
// 创建两个Servo对象/*Servo myservo1;Servo myservo2;*/
// 定义连接到Arduino的LED引脚int ledPin = 3;
// 定义摇杆的模拟输入引脚/*const int potX = A0; // 左摇杆X轴连接到A0const int potY = A1; // 左摇杆Y轴连接到A1const int potRightX = A2; // 右摇杆X轴连接到A2const int potRightY = A3; // 右摇杆Y轴连接到A3*/
// 定义摇杆的模拟输入引脚const int potBase = A0;const int potShoulder = A1;const int potElbow = A3;const int potWrist = A2;
// 定义舵机连接的数字引脚const int basePin = 11;const int shoulderPin = 10;const int elbowPin = 9;const int wristPin = 8; //存储电机极限值(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 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;        fromPos = base.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: Base Servo Value Out Of Limit!");        return;      }     case 'c':      if(toPos >= clawMin && toPos <= clawMax){            servo2go = claw;        fromPos = claw.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: Claw Servo Value Out Of Limit!");        return;              }     case 'f':      if(toPos >= fArmMin && toPos <= fArmMax){        servo2go = fArm;        fromPos = fArm.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: fArm Servo Value Out Of Limit!");        return;      }             case 'r':      if(toPos >= rArmMin && toPos <= rArmMax){        servo2go = rArm;        fromPos = rArm.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: rArm Servo Value Out Of Limit!");        return;      }        }   //指挥电机运行  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] = {    {'b', 90, DSD},    {'r', 90, DSD},    {'f', 90, DSD},    {'c', 90, DSD}   };   for (int i = 0; i < 4; i++){    servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);  }}
void armDataCmd(char command);         void setup(){
  // 初始化串行通信  Serial.begin(9600);
  // 将舵机连接到数字引脚  baseServo.attach(basePin);  shoulderServo.attach(shoulderPin);  elbowServo.attach(elbowPin);  wristServo.attach(wristPin);
  /*myservo.attach(7); // 将舵机连接到数字引7    base.attach(11);     // base 伺服舵机连接引脚targetAngle11 舵机代号'b'  delay(20);          // 稳定性等待  rArm.attach(10);     // rArm 伺服舵机连接引脚10 舵机代号'r'  delay(20);          // 稳定性等待  fArm.attach(9);      // fArm 伺服舵机连接引脚9  舵机代号'f'  delay(20);          // 稳定性等待  claw.attach(8);      // claw 伺服舵机连接引脚6  舵机代号'c'  delay(20);          // 稳定性等待  *//*   base.write(90);   delay(10);  fArm.write(90);   delay(10);  rArm.write(90);   delay(10);  claw.write(90);    delay(10); */
  Serial.begin(9600);  Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial");  Serial.println("Input mode: 1 (armDataCmd) & 0 (armJoyCmd)");  delay(1000); // 等待3秒
   // 检查串行缓冲区是否有数据可读  if (Serial.available() > 0){   // 读取一个字符    char serialCmd = Serial.read();    // 打印接收到的命令到串行监视器，用于调试    Serial.println("Received command: " + String(serialCmd));    // 如果模式为1，调用armDataCmd函数处理命令    if( mode == 1 ){      armDataCmd(serialCmd); //指令模式    }         digitalWrite(LED_BUILTIN, HIGH); // 打开内置LED灯      delay(1000); // 等待1秒      digitalWrite(LED_BUILTIN, LOW); // 关闭内置LED灯      }  else {//      armJoyCmd(serialCmd); //手柄模式         }pinMode(ledPin, OUTPUT); // 设置LED引脚为输出模式 }
void armDataCmd(char serialCmd){ //Arduino根据串行指令执行相应操作                                 //指令示例：b45 底盘转到45度角度位置                                 //          o 输出机械臂舵机状态信息  //判断人类是否因搞错模式而输入错误的指令信息（指令模式下输入手柄按键信息）  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(10);    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){  
       // 使用switch语句根据接收到的命令执行不同的操作  //switch (command) {    case '1':    // 如果命令是'1'，打印信息并打开LED灯    Serial.println("Command 'A' received: LED ON");    digitalWrite(ledPin, HIGH); // 设置LED引脚为高电平，打开LED灯    break; // 退出switch语句    case 'B':      // 如果命令是'c'，打印信息并关闭LED灯      Serial.println("Command 'c' received: LED OFF");      digitalWrite(ledPin, LOW); // 设置LED引脚为低电平，关闭LED灯      break; // 退出switch语句      default:      // 如果命令不是'A'或'B'，打印未知命令信息      Serial.println("Unknown command received");      break; // 退出switch语句     case 'm' :   //切换至手柄模式         mode = 0;        Serial.println("Command: Switch to Joy-Stick Mode.");        break;               case 'o':  // 输出舵机状态信息        reportStatus();        break;       case 'i':        armIniPos();          break;
          }    }  }  void armJoyCmd(char serialCmd){ //Arduino根据手柄按键执行相应操作
     // 读取左摇杆的X轴和Y轴位置  int leftX = analogRead(A0); // X轴位置  int leftY = 1023 - analogRead(A1); // Y轴位置（取反）
  // 读取右摇杆的X轴和Y轴位置  int rightX = analogRead(A3); // X轴位置  int rightY = 1023 - analogRead(A2); // Y轴位置（取反）
  Serial.println("Command received: " + String(command));
  // 打印摇杆位置到串行监视器  Serial.print("Left X: ");  Serial.print(leftX);  Serial.print(", Left Y: ");  Serial.print(leftY);  Serial.print(", Right X: ");  Serial.print(rightX);  Serial.print(", Right Y: ");  Serial.println(rightY);
  delay(100); // 稍作延迟
  // 读取左摇杆的X轴和Y轴位置/*  leftX = analogRead(potX);  leftY = analogRead(potY);*/
  // 读取右摇杆的X轴和Y轴位置/*  rightX = analogRead(potRightX);  rightY = analogRead(potRightY);*/
  // 将模拟值映射到舵机角度（0-180度）/*  int angleLeftX = map(leftX, 0, 1023, 0, 180);  int angleLeftY = map(leftY, 0, 1023, 0, 180);  int angleRightX = map(rightX, 0, 1023, 0, 180);  int angleRightY = map(rightY, 0, 1023, 0, 180);*/
  // 打印摇杆位置到串行监视器 /* Serial.print("Left X: ");  Serial.print(leftX);  Serial.print(", Left Y: ");  Serial.print(leftY);  Serial.print(", Right X: ");  Serial.print(rightX);  Serial.print(", Right Y: ");  Serial.println(rightY);*/
  // 根据摇杆位置设置舵机角度/*myservo1.write(angleLeftX); // 控制左摇杆X轴对应的舵机  myservo2.write(angleLeftY); // 控制左摇杆Y轴对应的舵机*/  // 如果需要，也可以根据右摇杆的位置来控制舵机   //myservo1.write(angleRightX);   //myservo2.write(angleRightY);
  delay(15); // 等待舵机到达指定位置
   //判断人类是否因搞错模式而输入错误的指令信息（手柄模式下输入舵机指令）  if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){    Serial.println("+Warning: Robot in Joy-Stick Mode...");    delay(50);    while(Serial.available()>0) char wrongCommand = Serial.read();  //清除串口缓存的错误指令    return;  }        int baseJoyPos;  int rArmJoyPos;  int fArmJoyPos;  int clawJoyPos;  switch(serialCmd){
 case '1' :   //切换至命令模式         mode = 1;        Serial.println("Command: Switch to armDataCmd Mode.");        break;       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 loop(){  // 读取摇杆的位置  int basePos = analogRead(potBase);  int shoulderPos = analogRead(potShoulder);  int elbowPos = analogRead(potElbow);  int wristPos = analogRead(potWrist);  /*int targetAngle = analogRead(A2); // 从模拟脚A0读取值，范围是0-1023  // 将读取的模拟值映射到舵机的角度范围，并添加角度限制  targetAngle = map(targetAngle, 0, 1023, minAngle, maxAngle);  // 限制角度在0到180度之间  targetAngle = constrain(targetAngle, 0, 180);   myservo.write(targetAngle); // 设置舵机角度delay(3); // 等待15毫秒观察效果*/
// 将摇杆的位置映射到舵机的角度范围  basePos = map(basePos, 0, 1023, 0, 180);  shoulderPos = map(shoulderPos, 0, 1023, 0, 180);  elbowPos = map(elbowPos, 0, 1023, 0, 180);  wristPos = map(wristPos, 0, 1023, 0, 180);
/*  int basePosition = map(analogRead(potX), 0, 1023, baseMin, baseMax);  int rArmPosition = map(analogRead(potY), 0, 1023, rArmMin, rArmMax);  int fArmPosition = map(analogRead(potRightX), 0, 1023, fArmMin, fArmMax);  int clawPosition = map(analogRead(potRightY), 0, 1023, clawMin, clawMax);*/
  // 设置舵机的角度  baseServo.write(basePos);  shoulderServo.write(shoulderPos);  elbowServo.write(elbowPos);  wristServo.write(wristPos); /*  baseServo.write(basePosition);  rArmServo.write(rArmPosition);  fArmServo.write(fArmPosition);  clawServo.write(clawPosition);*/
  // 打印摇杆的位置到串行监视器，用于调试  Serial.print("Base: ");  Serial.print(basePos);  Serial.print(", Shoulder: ");  Serial.print(shoulderPos);  Serial.print(", Elbow: ");  Serial.print(elbowPos);  Serial.print(", Wrist: ");  Serial.println(wristPos);   if (Serial.available() > 0) {    command = Serial.read();    Serial.println("Received command: " + String(command));  }    // 呼吸灯效果：逐渐增加亮度，然后逐渐减少亮度  for (int brightness = 0; brightness <= 200; brightness++) {    // 设置LED的亮度    analogWrite(ledPin, brightness);    // 等待一段时间，以便人眼可以感知亮度变化    delay(10);  }  for (int brightness = 200; brightness >= 0; brightness--) {    // 设置LED的亮度    analogWrite(ledPin, brightness);    // 等待一段时间，以便人眼可以感知亮度变化    delay(10);  }}

里面有重复的部分，有些是不执行的，比如机械臂的这个部分：

//判断人类是否因搞错模式而输入错误的指令信息（手柄模式下输入舵机指令）  if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){    Serial.println("+Warning: Robot in Joy-Stick Mode...");    delay(50);    while(Serial.available()>0) char wrongCommand = Serial.read();  //清除串口缓存的错误指令    return;  }

     

  int baseJoyPos;  int rArmJoyPos;  int fArmJoyPos;  int clawJoyPos;  switch(serialCmd){

 case '1' :   //切换至命令模式         mode = 1;        Serial.println("Command: Switch to armDataCmd Mode.");        break;

   

    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;

     

  }  

这部分是串口命令控制舵机，未理解，所以没有改动，下面是原来的程序，希望大佬对应修改，发出来：

/*MeArm机械臂控制程序-6by 太极创客 (2017-06-02)www.taichi-maker.com 本程序用于太极创客《零基础入门学用Arduino教程 - MeArm篇》。1   使用串口获得电机数据,设置机械臂动作    将串口指令解读改写为函数,加入状态汇报函数reportStatus2   加入servoCmd函数控制电机速度3   加入判断人类指令是否超出舵机限值4   MeArm执行一系列动作(执行程序) armIniPos - 1 （一维数组）5   MeArm执行一系列动作(执行程序) armIniPos - 2 （二维数组）6   加入手柄控制模式    加入机器判断：人类输入信息错误检查 如需要获得具体电路连接细节，请查阅太极创客制作的《零基础入门学用Arduino教程 - MeArm篇》页面。 */#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);  fArm.write(90);  delay(10);  rArm.write(90);  delay(10);  claw.write(90);    delay(10);   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 ){      armDataCmd(serialCmd); //指令模式    } else {      armJoyCmd(serialCmd); //手柄模式    }  }} void armDataCmd(char serialCmd){ //Arduino根据串行指令执行相应操作                                 //指令示例：b45 底盘转到45度角度位置                                 //          o 输出机械臂舵机状态信息  //判断人类是否因搞错模式而输入错误的指令信息（指令模式下输入手柄按键信息）  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){ //Arduino根据手柄按键执行相应操作   //判断人类是否因搞错模式而输入错误的指令信息（手柄模式下输入舵机指令）  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;  int rArmJoyPos;  int fArmJoyPos;  int 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 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;        fromPos = base.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: Base Servo Value Out Of Limit!");        return;      }     case 'c':      if(toPos >= clawMin && toPos <= clawMax){            servo2go = claw;        fromPos = claw.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: Claw Servo Value Out Of Limit!");        return;              }     case 'f':      if(toPos >= fArmMin && toPos <= fArmMax){        servo2go = fArm;        fromPos = fArm.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: fArm Servo Value Out Of Limit!");        return;      }             case 'r':      if(toPos >= rArmMin && toPos <= rArmMax){        servo2go = rArm;        fromPos = rArm.read();  // 获取当前电机角度值用于“电机运动起始角度值”        break;      } else {        Serial.println("+Warning: rArm Servo Value Out Of Limit!");        return;      }        }   //指挥电机运行  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] = {    {'b', 90, DSD},    {'r', 90, DSD},    {'f', 90, DSD},    {'c', 90, DSD}  };   for (int i = 0; i < 4; i++){    servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]);  }}

原程序只有欢迎的串口打印，没有任何功能。