Arduino 机械臂与麦克纳姆轮平台的无缝协同控制

概述

本教程将展示如何让我之前视频中的麦克纳姆轮机器人平台与 3D 打印机械臂（也是之前视频中的 Arduino 项目）协同工作并自动运行。

您可以观看以下视频或阅读下面的文字教程。

我们可以像上个视频解释的那样，用自制 Android 应用控制麦克纳姆轮机器人。此外，现在应用还增加了控制机械臂的按钮。

原来的机械臂控制应用使用滑块控制关节，但导致机械臂稳定性问题。现在这种方式工作好多了，因此我将提供更新版的机械臂控制应用和 Arduino 代码给原机械臂项目。

不过，这台机器人最酷的功能是能存储动作并自动重复执行。

使用“保存”按钮，可简单存储每一步电机的位置。然后点击“运行”按钮，机器人将自动重复存储的动作循环执行。

构建 Arduino 机器人

好的，这里已有组装好的麦克纳姆轮平台，详情请见上个视频。

这里还有机械臂的 3D 打印零件和舵机，现在展示如何组装它们。这是项目的 3D 模型。

您可以在 Thangs 上找到并下载此 3D 模型，也可在浏览器中探索。

在 Thangs 下载 3D 模型。

STL 文件用于 3D 打印：

机械臂的第一个舵机将直接安装在麦克纳姆轮平台的顶盖上。

我标记了位置，用 10mm 钻头钻了几个孔。

然后用锉刀切通孔并微调开口以适配舵机。用四个 M3 螺栓螺母将舵机固定到顶板。

在这个舵机的输出轴上，使用舵机附带的圆形舵盘，连接机械臂的下一部分即腰部。但我们注意到这样零件会悬在板面上方约 8mm。因此我粘了两块 8mm MDF 板，使腰部零件能滑动在其上，让关节更稳定。

圆形舵盘用舵机附带的自攻螺丝固定到腰部零件，然后用舵机附带的螺栓固定到舵机输出轴。

接下来是肩部舵机。直接放入位置并用自攻螺丝固定到 3D 打印零件。

圆形舵盘装到下一零件，两零件用舵机输出轴上的螺栓相互固定。

注意：固定零件前需确保零件有完整运动范围。这里我还在肩关节加了橡皮筋辅助舵机，因为该舵机需承担剩余手臂重量及负载。

以类似方式组装其余机械臂。

接下来组装夹爪机构。夹爪由 SG90 微型舵机控制，先在其上安装自定义设计的齿形连杆。另一侧配对另一个齿形连杆，用 M4 螺栓螺母固定。实际上所有其他连杆都用 M4 螺栓螺母连接。

夹爪 3D 模型原为 3mm 孔，但我没有足够 M3 螺栓，所以用 4mm 钻头扩孔并用 M4 螺栓代替。

组装完夹爪机构后，将其固定到最后一个舵机，机械臂组装完成。

接着做线缆管理。将舵机线穿过机械臂专门设计的走线孔。在顶板上用 10mm 钻头钻孔让线穿过。

用扎带将所有线束固定在一起，剩下的就是连接到 Arduino 板。

Arduino 机器人电路图

这是项目的电路图及连接方式。

所需元件

步进电机 – NEMA 17
DRV8825 步进驱动器
MG996R 舵机
SG90 微型舵机
HC-05 蓝牙模块
锂聚合物电池
Arduino Mega 开发板

上个教程解释了麦克纳姆轮机器人部分的工作原理，并展示了如何为其制作定制 PCB。

我在 PCB 上集成了 5V 稳压器（LM350），可提供高达 3A 电流，以便连接舵机（工作电压 5V）。机械臂六个舵机可消耗 2-3A 电流，虽在范围内但会导致稳压器极热。

因此我给它加了散热片和一个小型 12V 直流风扇吹风散热，因为仅靠散热片不够。

我将舵机信号线连接到 Arduino 数字引脚 5-10，供电用 PCB 上的 5V 排针。最后将所有线塞入平台内部，用两个螺母固定顶板。

至此项目硬件部分完成。

Arduino 代码

剩下的就是看 Arduino 代码和 Android 应用如何工作。代码较长，为便于理解，我将分段发布并附说明。文章末尾提供完整源代码。

首先定义 6 个舵机、4 个步进电机和蓝牙通信，以及程序所需变量。在 setup 部分设置步进电机最大速度、定义舵机连接引脚、启动蓝牙通信并将机械臂移至初始位置。

#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <Servo.h>
Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;
SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
// Define the stepper motors and the pins the will use
AccelStepper LeftBackWheel(1, 42, 43); // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 40, 41); // Stepper2
AccelStepper RightBackWheel(1, 44, 45); // Stepper3
AccelStepper RightFrontWheel(1, 46, 47); // Stepper4
#define led 14
int wheelSpeed = 1500;
int lbw[50], lfw[50], rbw[50], rfw[50]; // arrays for storing positions/steps
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;
void setup() {
  // Set initial seed values for the steppers
  LeftFrontWheel.setMaxSpeed(3000);
  LeftBackWheel.setMaxSpeed(3000);
  RightFrontWheel.setMaxSpeed(3000);
  RightBackWheel.setMaxSpeed(3000);
  pinMode(led, OUTPUT);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);
  // Move robot arm to initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 100;
  servo02.write(servo2PPos);
  servo3PPos = 120;
  servo03.write(servo3PPos);
  servo4PPos = 95;
  servo04.write(servo4PPos);
  servo5PPos = 60;
  servo05.write(servo5PPos);
  servo6PPos = 110;
  servo06.write(servo6PPos);
}

然后在 loop 部分开始检查是否有传入数据。

// Check for incoming data
if (Bluetooth.available() > 0) {
  dataIn = Bluetooth.read(); // Read the data

这些数据来自智能手机或 Android 应用，让我们看看它实际发送什么数据。Android 应用使用 MIT App Inventor 在线制作。由带有适当背景图片的简单按钮组成。

查看应用的积木块，可见它只是在按钮点击时发送单字节数字。

因此，根据点击的按钮，我们告诉 Arduino 做什么。例如，收到数字 '2' 时，麦克纳姆轮平台将通过 moveForward 自定义函数前进。

if (dataIn == 2) { m = 2; }
// if (m == 2) { moveForward(); }

该自定义函数设置所有四个步进电机正向旋转。

void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}

向其他方向移动，只需按适当方向旋转车轮即可。

控制机械臂使用相同方法。应用中也有按钮，按住按钮时机械臂关节向特定方向移动。

如前所述，原来的机械臂控制应用使用滑块控制舵机位置，但这导致一些问题，因为那样必须发送文本到 Arduino 而非单字节数字。问题在于 Arduino 有时会漏收来自 App 的文本，导致错误或机械臂抖动异常。

现在我们在特定按钮按下时简单发送单个单字节数字。

Arduino 代码进入该数字的 while 循环，并停留直到我们松开按钮，因为此时发送数字 0 表示机器人应什么都不做。

// Move servo 1 in positive direction
while (m == 16) {
  if (Bluetooth.available() > 0) {
    m = Bluetooth.read();
  }
  servo01.write(servo1PPos);
  servo1PPos++;
  delay(speedDelay);
}
// Move servo 1 in negative direction
while (m == 17) {
  if (Bluetooth.available() > 0) {
    m = Bluetooth.read();
  }
  servo01.write(servo1PPos);
  servo1PPos--;
  delay(speedDelay);
}

根据触摸的按钮，舵机向正或负方向移动。相同工作原理适用于所有舵机。改变移动速度使用来自滑块的值（100-250）。

// If arm speed slider is changed
if (dataIn > 101 & dataIn < 250) {
  speedDelay = dataIn / 10; // Change servo speed (delay time)
}

除以 10 得到 10-25 的值，用作 while 循环中驱动舵机的微秒延迟。

存储机器人动作，只需每次点击“保存”按钮时将舵机和步进电机当前位置保存到数组。

// If button "SAVE" is pressed
if (m == 12) {
  //if it's initial save, set the steppers position to 0
  if (index == 0) {
    LeftBackWheel.setCurrentPosition(0);
    LeftFrontWheel.setCurrentPosition(0);
    RightBackWheel.setCurrentPosition(0);
    RightFrontWheel.setCurrentPosition(0);
  }
  lbw[index] = LeftBackWheel.currentPosition(); // save position into the array
  lfw[index] = LeftFrontWheel.currentPosition();
  rbw[index] = RightBackWheel.currentPosition();
  rfw[index] = RightFrontWheel.currentPosition();
  servo01SP[index] = servo1PPos; // save position into the array
  servo02SP[index] = servo2PPos;
  servo03SP[index] = servo3PPos;
  servo04SP[index] = servo4PPos;
  servo05SP[index] = servo5PPos;
  servo06SP[index] = servo6PPos;
  index++; // Increase the array index
  m = 0;
}

按“运行”按钮时调用 runSteps() 自定义函数。该函数通过 for 和 while 循环遍历所有存储的步骤。

if (m == 14) {
  runSteps();
  // If button "RESET" is pressed
  if (dataIn != 14) {
    stopMoving();
    memset(lbw, 0, sizeof(lbw)); // Clear the array data to 0
    memset(lfw, 0, sizeof(lfw));
    memset(rbw, 0, sizeof(rbw));
    memset(rfw, 0, sizeof(rfw));
    memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
    memset(servo02SP, 0, sizeof(servo02SP));
    memset(servo03SP, 0, sizeof(servo03SP));
    memset(servo04SP, 0, sizeof(servo04SP));
    memset(servo05SP, 0, sizeof(servo05SP));
    memset(servo06SP, 0, sizeof(servo06SP));
    index = 0; // Index to 0
  }
}

注意：它从第一个位置开始到最后一个位置，并反复重复。因此保存步骤时，实际上需要将机器人定位使第一步位置与最后一步相同。运行步骤时也可改变平台和机械臂速度，以及暂停和重置所有步骤。

您可以在此下载此应用及可编辑项目文件：

这是该 Arduino 机器人项目的完整 Arduino 代码：

/* Arduino Robot Arm and Mecanum Wheels Robot Smartphone Control via Bluetooth by Dejan, www.HowToMechatronics.com */
#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <Servo.h>
Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;
SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)
// Define the stepper motors and the pins the will use
AccelStepper LeftBackWheel(1, 42, 43); // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 40, 41); // Stepper2
AccelStepper RightBackWheel(1, 44, 45); // Stepper3
AccelStepper RightFrontWheel(1, 46, 47); // Stepper4
#define led 14
int wheelSpeed = 1500;
int lbw[50], lfw[50], rbw[50], rfw[50]; // arrays for storing positions/steps
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // current position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // for storing positions/steps
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;
void setup() {
  // Set initial seed values for the steppers
  LeftFrontWheel.setMaxSpeed(3000);
  LeftBackWheel.setMaxSpeed(3000);
  RightFrontWheel.setMaxSpeed(3000);
  RightBackWheel.setMaxSpeed(3000);
  pinMode(led, OUTPUT);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400); // Default baud rate of the Bluetooth module
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);
  // Move robot arm to initial position
  servo1PPos = 90;
  servo01.write(servo1PPos);
  servo2PPos = 100;
  servo02.write(servo2PPos);
  servo3PPos = 120;
  servo03.write(servo3PPos);
  servo4PPos = 95;
  servo04.write(servo4PPos);
  servo5PPos = 60;
  servo05.write(servo5PPos);
  servo6PPos = 110;
  servo06.write(servo6PPos);
}
void loop() {
  // Check for incoming data
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.read(); // Read the data
    if (dataIn == 0) { m = 0; }
    if (dataIn == 1) { m = 1; }
    if (dataIn == 2) { m = 2; }
    if (dataIn == 3) { m = 3; }
    if (dataIn == 4) { m = 4; }
    if (dataIn == 5) { m = 5; }
    if (dataIn == 6) { m = 6; }
    if (dataIn == 7) { m = 7; }
    if (dataIn == 8) { m = 8; }
    if (dataIn == 9) { m = 9; }
    if (dataIn == 10) { m = 10; }
    if (dataIn == 11) { m = 11; }
    if (dataIn == 12) { m = 12; }
    if (dataIn == 14) { m = 14; }
    if (dataIn == 16) { m = 16; }
    if (dataIn == 17) { m = 17; }
    if (dataIn == 18) { m = 18; }
    if (dataIn == 19) { m = 19; }
    if (dataIn == 20) { m = 20; }
    if (dataIn == 21) { m = 21; }
    if (dataIn == 22) { m = 22; }
    if (dataIn == 23) { m = 23; }
    if (dataIn == 24) { m = 24; }
    if (dataIn == 25) { m = 25; }
    if (dataIn == 26) { m = 26; }
    if (dataIn == 27) { m = 27; }
    // Move the Mecanum wheels platform
    if (m == 4) { moveSidewaysLeft(); }
    if (m == 5) { moveSidewaysRight(); }
    if (m == 2) { moveForward(); }
    if (m == 7) { moveBackward(); }
    if (m == 3) { moveRightForward(); }
    if (m == 1) { moveLeftForward(); }
    if (m == 8) { moveRightBackward(); }
    if (m == 6) { moveLeftBackward(); }
    if (m == 9) { rotateLeft(); }
    if (m == 10) { rotateRight(); }
    if (m == 0) { stopMoving(); }
    // Mecanum wheels speed
    if (dataIn > 30 & dataIn < 100) { wheelSpeed = dataIn * 20; }
    // Move robot arm
    // Move servo 1 in positive direction
    while (m == 16) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo01.write(servo1PPos);
      servo1PPos++;
      delay(speedDelay);
    }
    // Move servo 1 in negative direction
    while (m == 17) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo01.write(servo1PPos);
      servo1PPos--;
      delay(speedDelay);
    }
    // Move servo 2
    while (m == 19) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo02.write(servo2PPos);
      servo2PPos++;
      delay(speedDelay);
    }
    while (m == 18) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo02.write(servo2PPos);
      servo2PPos--;
      delay(speedDelay);
    }
    // Move servo 3
    while (m == 20) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo03.write(servo3PPos);
      servo3PPos++;
      delay(speedDelay);
    }
    while (m == 21) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo03.write(servo3PPos);
      servo3PPos--;
      delay(speedDelay);
    }
    // Move servo 4
    while (m == 23) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo04.write(servo4PPos);
      servo4PPos++;
      delay(speedDelay);
    }
    while (m == 22) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo04.write(servo4PPos);
      servo4PPos--;
      delay(speedDelay);
    }
    // Move servo 5
    while (m == 25) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo05.write(servo5PPos);
      servo5PPos++;
      delay(speedDelay);
    }
    while (m == 24) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo05.write(servo5PPos);
      servo5PPos--;
      delay(speedDelay);
    }
    // Move servo 6
    while (m == 26) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo06.write(servo6PPos);
      servo6PPos++;
      delay(speedDelay);
    }
    while (m == 27) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo06.write(servo6PPos);
      servo6PPos--;
      delay(speedDelay);
    }
    // If arm speed slider is changed
    if (dataIn > 101 & dataIn < 250) {
      speedDelay = dataIn / 10; // Change servo speed (delay time)
    }
    // If button "SAVE" is pressed
    if (m == 12) {
      //if it's initial save, set the steppers position to 0
      if (index == 0) {
        LeftBackWheel.setCurrentPosition(0);
        LeftFrontWheel.setCurrentPosition(0);
        RightBackWheel.setCurrentPosition(0);
        RightFrontWheel.setCurrentPosition(0);
      }
      lbw[index] = LeftBackWheel.currentPosition(); // save position into the array
      lfw[index] = LeftFrontWheel.currentPosition();
      rbw[index] = RightBackWheel.currentPosition();
      rfw[index] = RightFrontWheel.currentPosition();
      servo01SP[index] = servo1PPos; // save position into the array
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      index++; // Increase the array index
      m = 0;
    }
    // If button "RUN" is pressed
    if (m == 14) {
      runSteps();
      // If button "RESET" is pressed
      if (dataIn != 14) {
        stopMoving();
        memset(lbw, 0, sizeof(lbw)); // Clear the array data to 0
        memset(lfw, 0, sizeof(lfw));
        memset(rbw, 0, sizeof(rbw));
        memset(rfw, 0, sizeof(rfw));
        memset(servo01SP, 0, sizeof(servo01SP)); // Clear the array data to 0
        memset(servo02SP, 0, sizeof(servo02SP));
        memset(servo03SP, 0, sizeof(servo03SP));
        memset(servo04SP, 0, sizeof(servo04SP));
        memset(servo05SP, 0, sizeof(servo05SP));
        memset(servo06SP, 0, sizeof(servo06SP));
        index = 0; // Index to 0
      }
    }
  }
  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();
  // Monitor the battery voltage
  int sensorValue = analogRead(A0);
  float voltage = sensorValue * (5.0 / 1023.00) * 3; // Convert the reading values from 5v to suitable 12V i
  //Serial.println(voltage);
  // If voltage is below 11V turn on the LED
  if (voltage < 11) {
    digitalWrite(led, HIGH);
  } else {
    digitalWrite(led, LOW);
  }
}
void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveSidewaysRight() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveSidewaysLeft() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void rotateLeft() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void rotateRight() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveRightForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveRightBackward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftForward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void stopMoving() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(0);
}
// Automatic mode custom function - run the saved steps
void runSteps() {
  while (dataIn != 13) {
    // Run the steps over and over again until "RESET" button is pressed
    for (int i = 0; i <= index - 2; i++) {
      // Run through all steps(index)
      if (Bluetooth.available() > 0) {
        // Check for incomding data
        dataIn = Bluetooth.read();
        if ( dataIn == 15) {
          // If button "PAUSE" is pressed
          while (dataIn != 14) {
            // Wait until "RUN" is pressed again
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.read();
              if ( dataIn == 13) { break; }
            }
          }
        }
        // If speed slider is changed
        if (dataIn > 100 & dataIn < 150) {
          speedDelay = dataIn / 10; // Change servo speed (delay time)
        }
        // Mecanum wheels speed
        if (dataIn > 30 & dataIn < 100) {
          wheelSpeed = dataIn * 10;
          dataIn = 14;
        }
      }
      LeftFrontWheel.moveTo(lfw[i]);
      LeftFrontWheel.setSpeed(wheelSpeed);
      LeftBackWheel.moveTo(lbw[i]);
      LeftBackWheel.setSpeed(wheelSpeed);
      RightFrontWheel.moveTo(rfw[i]);
      RightFrontWheel.setSpeed(wheelSpeed);
      RightBackWheel.moveTo(rbw[i]);
      RightBackWheel.setSpeed(wheelSpeed);
      while (LeftBackWheel.currentPosition() != lbw[i] & LeftFrontWheel.currentPosition() != lfw[i] & RightFrontWheel.currentPosition() != rfw[i] & RightBackWheel.currentPosition() != rbw[i]) {
        LeftFrontWheel.runSpeedToPosition();
        LeftBackWheel.runSpeedToPosition();
        RightFrontWheel.runSpeedToPosition();
        RightBackWheel.runSpeedToPosition();
      }
      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) { }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j);
          delay(speedDelay);
        }
      }
      // Servo 2
      if (servo02SP[i] == servo02SP[i + 1]) { }
      if (servo02SP[i] > servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
          servo02.write(j);
          delay(speedDelay);
        }
      }
      if (servo02SP[i] < servo02SP[i + 1]) {
        for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
          servo02.write(j);
          delay(speedDelay);
        }
      }
      // Servo 3
      if (servo03SP[i] == servo03SP[i + 1]) { }
      if (servo03SP[i] > servo03SP[i + 1]) {
        for ( int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {
          servo03.write(j);
          delay(speedDelay);
        }
      }
      if (servo03SP[i] < servo03SP[i + 1]) {
        for ( int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {
          servo03.write(j);
          delay(speedDelay);
        }
      }
      // Servo 4
      if (servo04SP[i] == servo04SP[i + 1]) { }
      if (servo04SP[i] > servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {
          servo04.write(j);
          delay(speedDelay);
        }
      }
      if (servo04SP[i] < servo04SP[i + 1]) {
        for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {
          servo04.write(j);
          delay(speedDelay);
        }
      }
      // Servo 5
      if (servo05SP[i] == servo05SP[i + 1]) { }
      if (servo05SP[i] > servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
          servo05.write(j);
          delay(speedDelay);
        }
      }
      if (servo05SP[i] < servo05SP[i + 1]) {
        for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
          servo05.write(j);
          delay(speedDelay);
        }
      }
      // Servo 6
      if (servo06SP[i] == servo06SP[i + 1]) { }
      if (servo06SP[i] > servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
      if (servo06SP[i] < servo06SP[i + 1]) {
        for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
          servo06.write(j);
          delay(speedDelay);
        }
      }
    }
  }
}

这就是本教程的主要内容。项目运行良好，但请注意它远非完美。自动动作可能不够精确，因为麦克纳姆轮打滑以及舵机性能较差。这些廉价舵机甚至在不移动时也会抖动，因为没有足够力量支撑 3D 打印零件的重量。

希望您喜欢这个教程并学到新知识。欢迎在下方评论区提问，查看我的 Arduino 项目合集。

制作可靠的 Arduino CNC 泡沫切割机 – DIY 教程制作 Arduino 麦克纳姆轮机器人：360° 全向移动 DIY 指南

制造工艺

3D打印

自动化控制系统

工业技术