Arduino机械臂和麦克纳姆轮平台自动操作
在本教程中，我将向您展示我是如何根据我之前的视频制作 Mecanum Wheels 机器人平台的，以便与我的 3D 打印机械臂一起工作并自动操作，这也是我之前视频中的一个 Arduino 项目。
您可以观看以下视频或阅读下面的书面教程。
因此，我们可以使用自定义构建的 Android 应用程序控制 Mecanum 轮机器人，方法与上一个视频中解释的相同。除此之外，现在该应用程序还具有用于控制机械臂的按钮。
最初的机器人手臂控制应用程序实际上有用于控制机器人关节的滑块，但这会导致手臂稳定性出现一些问题。通过这种方式，手臂工作得更好，因此我将把这个更新版本的机器人手臂控制应用程序和 Arduino 代码也提供给原始机器人手臂项目。
不过，这个机器人最酷的特点是能够存储动作，然后自动重复它们。
使用保存按钮，我们可以简单地存储每一步的电机位置。然后我们只需要点击运行按钮，机器人就会自动重复存储的动作一遍又一遍。
好的，这里我已经组装好了 Mecanum 车轮平台，您可以在我之前的视频中找到有关它的所有详细信息。
另外，这里我有机器人手臂和伺服电机的 3D 打印部件，现在我将向您展示如何组装它们。这是这个项目的 3D 模型。
您可以找到并下载此 3D 模型，也可以在 Thangs 上的浏览器中进行探索。
在 Thangs 下载 3D 模型。
用于 3D 打印的 STL 文件：
机械臂的第一个舵机将直接安装在麦克纳姆轮平台的顶盖上。
我标记了位置，并使用 10 毫米钻头打了几个孔。
然后用锉刀切开孔，然后微调伺服器的开口。我用四个 M3 螺栓和螺母将伺服系统固定在顶板上。
然后在这个舵机的这个输出轴上，使用舵机附带的圆角，我们需要连接下一个部分或机械臂的腰部。但是，我们可以注意到，通过这种方式，零件保持在板上方约 8 毫米处。所以，我附上了两块8mm的MDF板，这样腰部可以在上面滑动，这样关节会更稳定。
使用随舵机配件提供的自攻螺钉将圆角固定到腰部，然后使用随舵机附带的相应螺栓将圆角固定到舵轴上。
接下来我们有肩部伺服。我们只需将其放置到位并使用自攻螺钉将其固定到 3D 打印部件上。
圆形喇叭接下一部分，然后用舵机输出轴上的螺栓将两个部分相互固定。
我们应该注意，在固定零件之前，我们需要确保零件具有完整的运动范围。在这里，我还在肩关节处添加了一条橡皮筋，以便它对伺服器提供一点帮助，因为这个伺服器承载了手臂其余部分的重量以及有效载荷。
以类似的方式，我组装了机器人手臂的其余部分。
接下来，我们需要组装到夹具机构。夹持器由 SG90 伺服电机控制，我们首先在其上安装定制设计的齿轮连杆。我们将此链接与另一侧的另一个齿轮链接配对，该链接使用 M4 螺栓和螺母固定。实际上，所有其他链接都是使用M4螺栓和螺母连接的。
夹具的 3D 模型原本有 3mm 的孔，但我没有足够的 M3 螺栓，因此我使用 4mm 钻头扩大了孔并改用了 M4 螺栓。
组装好抓手机构后，我将其固定在最后一个伺服系统上，这样机器人手臂就完成了。
接下来我做了一些电缆管理。我将伺服线穿过机器人手臂专门设计的孔。我用一个 10 毫米的钻头在顶板上打了一个孔，这样电线就可以穿过了。
我使用扎带将所有电线固定在一起，现在剩下的就是将它们连接到 Arduino 板。
这是这个项目的电路图以及所有需要如何连接的东西。
您可以通过以下链接获取本项目所需的组件：
在之前的教程中，我解释了 Mecanum 轮机器人部件的工作原理，还向您展示了我是如何为它制作定制 PCB 的。
我在这个 PCB 上包含了一个 5V 电压调节器，这样我们就可以制作这个项目，或者连接伺服电机，因为它们在 5V 下工作。稳压器是 LM350，它可以处理高达 3 安培的电流。机器人手臂的所有六个伺服器都可以吸收大约 2 安培到 3 安培的电流，这意味着它可以处理它们，但这会导致调节器变得非常热。
因此，我在其上安装了一个散热器，以及一个小型 12V DC 风扇来吹一些空气，因为散热器本身不足以冷却调节器。
我将伺服信号线连接到 5 号到 10 号的 Arduino 数字引脚，为了供电，我使用了 PCB 上的 5V 引脚接头。最后，我将所有电线推入平台内部，并使用两个螺母将顶板固定在其上。
就是这样，现在我们完成了这个项目。
剩下的就是看看 Arduino 代码和 Android 应用程序是如何工作的。由于代码有点长，为了更好的理解，我将把程序的源代码分节贴出来，每个节都有说明。并且在文末我会贴出完整的源码。
所以首先我们需要定义6个伺服、4个步进电机和蓝牙通信，以及定义下面程序需要的一些变量。在设置部分，我们设置步进器的最大速度，定义舵机连接的引脚，开始蓝牙通信并将机械臂设置到初始位置。
然后在循环部分我们首先检查是否有任何传入数据。
这些数据来自智能手机或 Android 应用程序，让我们来看看它实际发送的数据类型。 Android 应用程序是使用 MIT App Inventor 在线应用程序制作的。它由简单的按钮组成，具有适当的图像作为背景。
如果我们看一下应用程序的块，我们可以看到它所做的只是在单击按钮时发送一个字节的数字。
因此，根据点击的按钮，我们告诉 Arduino 该做什么。例如，如果我们收到数字“2”，麦克纳姆轮平台将使用 moveForward 自定义函数向前移动。
此自定义函数将所有四个步进电机设置为正向旋转。
对于任何其他方向的移动，我们只需要向适当的方向旋转轮子即可。
对于控制机械臂，我们使用相同的方法。同样，我们在应用程序中有按钮，当按住按钮时，机器人手臂关节会向特定方向移动。
正如我之前提到的，在最初的机器人手臂控制应用程序中，我们使用滑块来控制伺服器的位置，但这会导致一些问题，因为这样我们必须将文本发送到 Arduino，而不是 1 字节数字。问题是 Arduino 有时会错过来自 App 的文本并出错或机器人手臂晃动并表现异常。
通过这种方式，我们可以在按下特定按钮时简单地发送一个 1 字节的数字。
Arduino 代码进入该数字的 while 循环，并一直停留在那里，直到我们触摸按钮，因为此时我们发送数字 0，这意味着机器人应该什么都不做。
因此，根据触摸的按钮，舵机可以正向或负向移动。相同的工作原理适用于所有伺服电机。为了改变移动速度，我们使用来自滑块的值，范围从 100 到 250。
通过将它们除以 10，我们得到从 10 到 25 的值，这些值用作驱动伺服器的 while 循环中的微秒延迟。
为了存储机器人的动作，我们只需将舵机和步进器的当前位置保存到数组中，每次点击保存按钮。
然后当我们按下 Run 按钮时，我们调用 runSteps() 自定义函数。这个自定义函数使用一些 for 和 while 循环遍历所有存储的步骤。
我们应该注意到它从第一个位置开始到最后一个位置，并一遍又一遍地重复。因此，在保存步骤时，我们实际上需要将机器人定位为第一步与最后一步的位置相同。在运行步骤的同时，我们还可以改变平台和机械臂的速度，以及暂停和重置所有步骤。
在这里你可以下载这个应用程序以及可编辑的项目文件：
这是这个 Arduino 机器人项目的完整 Arduino 代码：
So that’s pretty much everything for this tutorial. The project works well, but please note that it’s far from perfect. The automatic movements might not be that precise because of the slipping of the mecanum wheels as well as the poor performance of the servo motors. These cheap servo motors can also shake or jitter even when not moving just because don’t have enough strength to hold the weight of the 3D printed parts.
I hope you enjoyed this tutorial and learned something new. Feel free to ask any question in the comments section below and check my Arduino Projects Collection.概览
构建 Arduino 机器人
Arduino 机器人电路图
Arduino 代码
#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);
}Code language: Arduino (arduino)// Check for incoming data
if (Bluetooth.available() > 0) {
dataIn = Bluetooth.read(); // Read the dataCode language: Arduino (arduino)
if (dataIn == 2) {
m = 2;
}
//
if (m == 2) {
moveForward();
}Code language: Arduino (arduino)void moveForward() {
LeftFrontWheel.setSpeed(wheelSpeed);
LeftBackWheel.setSpeed(wheelSpeed);
RightFrontWheel.setSpeed(wheelSpeed);
RightBackWheel.setSpeed(wheelSpeed);
}Code language: Arduino (arduino)
// 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);
}Code language: Arduino (arduino)// If arm speed slider is changed
if (dataIn > 101 & dataIn < 250) {
speedDelay = dataIn / 10; // Change servo speed (delay time)
}Code language: Arduino (arduino)// 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;
}Code language: Arduino (arduino)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
}
}Code language: 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);
}
}
}
}
}Code language: Arduino (arduino)
制造工艺