Control Unificado de Brazo Robótico Arduino con Plataforma Mecanum

En este tutorial te mostraré cómo hice que mi Plataforma Robot con Ruedas Mecanum de mi video anterior, funcione junto y opere automáticamente con mi Brazo Robótico Impreso en 3D, también un proyecto Arduino de uno de mis videos anteriores.

Puedes ver el siguiente video o leer el tutorial escrito a continuación.

Resumen

Así que podemos controlar el robot de ruedas Mecanum con la aplicación Android personalizada de la misma manera que se explicó en el video anterior. Además de eso, ahora la app también tiene botones para controlar el brazo robótico.

Control Unificado de Brazo Robótico Arduino con Plataforma Mecanum

La app original de control del brazo robótico en realidad tenía sliders para controlar las articulaciones del robot, pero eso causaba algunos problemas con la estabilidad del brazo. De esta forma el brazo funciona mucho mejor, por lo que proporcionaré esta versión actualizada de la app de control del brazo robótico y el código Arduino al proyecto original del brazo robótico también.

Sin embargo, la característica más genial de este robot es la capacidad de almacenar los movimientos y luego repetirlos automáticamente.

Usando el botón Guardar (Save), simplemente podemos almacenar las posiciones de los motores para cada paso. Luego solo necesitamos hacer clic en el botón Ejecutar (Run) y el robot repetirá automáticamente los movimientos almacenados una y otra vez.

Construcción del Robot Arduino

Ok, así que aquí tengo la plataforma de ruedas Mecanum ya ensamblada y puedes encontrar todos los detalles sobre ella en mi video anterior.

También, aquí tengo las piezas impresas en 3D del brazo robótico y los servomotores y ahora te mostraré cómo ensamblarlos. Aquí está el modelo 3D de este proyecto.

Puedes encontrar y descargar este modelo 3D, así como explorarlo en tu navegador en Thangs.

Descarga el modelo 3D en Thangs.

Archivos STL para Impresión 3D:

El primer servo del brazo robótico se montará directamente en la tapa superior de la plataforma de ruedas Mecanum.

Marqué la ubicación, y usando una broca de 10mm hice varios agujeros.

Luego usando una lima, corté a través de los agujeros y luego afiné la abertura para el servo. Aseguré el servo a la placa superior usando cuatro tornillos M3 y tuercas.

Luego en el eje de salida de este servo, usando el cuerno redondo que viene como accesorio con el servo, necesitamos sujetar la siguiente parte o la cintura del brazo robótico. Sin embargo, notamos que de esta forma la pieza se queda alrededor de 8mm por encima de la placa. Por lo tanto, pegué dos piezas de tablero MDF de 8mm, para que la parte de la cintura pueda deslizarse sobre ellas y así la articulación será más estable.

El cuerno redondo se asegura a la parte de la cintura usando los tornillos autorroscantes que vienen como accesorios con el servo, y luego el cuerno redondo se asegura al eje del servo usando los tornillos apropiados que también vienen con el servo.

Luego tenemos el servo del hombro. Simplemente lo ponemos en su lugar y lo aseguramos a la pieza impresa en 3D usando tornillos autorroscantes.

El cuerno redondo va en la siguiente parte, y luego las dos partes se aseguran entre sí usando un tornillo en el eje de salida del servo.

Debemos notar que antes de asegurar las piezas, necesitamos asegurar que la pieza tenga el rango completo de movimiento. Aquí también agregué una goma elástica a la articulación del hombro para que dé un poco de ayuda al servo, porque este servo carga el peso del resto del brazo así como la carga útil.

De manera similar, ensamblé el resto del brazo robótico.

Luego, necesitamos ensamblar el mecanismo de la pinza (gripper). La pinza se controla con un servomotor SG90, en el cual primero sujetamos un engranaje de enlace diseñado a medida. Emparejamos este enlace con otro enlace engranado en el otro lado, que se asegura usando tornillo M4 y tuercas. En realidad, todos los demás enlaces se conectan usando tornillos M4 y tuercas.

El modelo 3D de la pinza originalmente tiene agujeros de 3mm pero no tenía suficientes tornillos M3, por lo que amplié los agujeros usando broca de 4mm y usé tornillos M4 en su lugar.

Una vez ensamblado el mecanismo de la pinza, lo aseguré al último servo y así el brazo robótico quedó completo.

Luego hice un poco de gestión de cables. Pasé los cables de los servos a través de los agujeros específicamente diseñados en el brazo robótico. Usando una broca de 10mm hice un agujero en la placa superior para que los cables puedan pasar.

Usando una brida aseguré todos los cables juntos, y ahora lo que queda es conectarlos a la placa Arduino.

Diagrama de Circuito Robot Arduino

Aquí está el diagrama de circuito de este proyecto y cómo todo necesita conectarse.

Componentes Necesarios

Motor Paso a Paso – NEMA 17
Driver DRV8825
Servomotor MG996R
Servomotor SG90 Micro
Módulo Bluetooth HC-05
Batería Li-Po
Placa Arduino Mega

En el tutorial anterior expliqué cómo funciona la parte del robot de ruedas Mecanum y también te mostré cómo hice un PCB personalizado para ello.

Incluí un regulador de voltaje de 5V en este PCB para que podamos hacer este proyecto, o conectar los servomotores porque trabajan a 5V. El regulador de voltaje es el LM350, que puede manejar hasta 3 amperios de corriente. Los seis servos del brazo robótico pueden consumir de alrededor de 2 a 3 amperios de corriente, lo que significa que puede manejarlos pero eso hará que el regulador se caliente mucho.

Por lo tanto, le puse un disipador de calor, así como un pequeño ventilador DC de 12V para soplar aire, porque el disipador solo no era suficiente para enfriar el regulador.

Conecté los cables de señal de los servos a los pines digitales del Arduino del 5 al 10, y para la alimentación usé el header de 5V en el PCB. Finalmente, metí todos los cables dentro de la plataforma y aseguré la placa superior usando las dos tuercas.

¡Y eso es todo, ahora hemos terminado con este proyecto!

Código Arduino

Lo que queda es echar un vistazo a cómo funcionan el código Arduino y la aplicación Android. Como el código es un poco largo, para mejor comprensión, publicaré el código fuente del programa en secciones con descripción para cada sección. Y al final de este artículo publicaré el código fuente completo.

Así que primero necesitamos definir los 6 servos, los 4 motores paso a paso y la comunicación Bluetooth, así como definir algunas variables necesarias para el programa a continuación. En la sección setup establecemos la velocidad máxima de los steppers, definimos los pines a los que están conectados los servos, iniciamos la comunicación Bluetooth y ponemos el brazo robótico en posición inicial.

Sección de Definiciones y 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)
// Definir los motores paso a paso y los pines que usarán
AccelStepper LeftBackWheel(1, 42, 43); // (Tipo: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 para almacenar posiciones/pasos
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // posición actual
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // posición anterior
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // para almacenar posiciones/pasos
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;
void setup() {
  // Establecer valores iniciales de velocidad para los 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); // Baud rate por defecto del módulo Bluetooth
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);
  // Mover brazo robótico a posición inicial
  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)

Luego en la sección loop empezamos verificando si hay datos entrantes.

Lectura de Datos Bluetooth

// Verificar datos entrantes
if (Bluetooth.available() > 0) {
  dataIn = Bluetooth.read(); // Leer el dato
Code language: Arduino (arduino)

Este dato viene del smartphone o la app Android, así que veamos qué tipo de datos está enviando realmente. La app Android está hecha usando la aplicación en línea MIT App Inventor. Consiste en botones simples que tienen imágenes apropiadas como fondo.

Si echamos un vistazo a los bloques de la app, podemos ver que todo lo que hace es enviar números de un byte cuando se hacen clic en los botones.

Así que, dependiendo del botón presionado, le decimos al Arduino qué hacer. Por ejemplo, si recibimos el número '2', la plataforma de ruedas Mecanum se moverá hacia adelante, usando la función personalizada moveForward().

Control de Movimiento de la Plataforma

if (dataIn == 2) { m = 2; }
// if (m == 2) { moveForward(); }Code language: Arduino (arduino)

Esta función personalizada configura los cuatro motores paso a paso para rotar hacia adelante.

void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}Code language: Arduino (arduino)

Para moverse en cualquier otra dirección, solo necesitamos rotar las ruedas en las direcciones apropiadas.

Para controlar el brazo robótico, usamos el mismo método. De nuevo, tenemos botones en la app y al mantener presionados los botones, las articulaciones del brazo robótico se mueven en la dirección particular.

Como mencioné antes, en la app original de control del brazo robótico usábamos sliders para controlar las posiciones de los servos pero eso causaba algunos problemas porque de esa forma teníamos que enviar Texto al Arduino, en lugar de número de 1 byte. El problema es que el Arduino a veces pierde el Texto que viene de la App y comete error o el brazo robótico tiembla y se comporta de forma anormal.

De esta forma simplemente enviamos un solo número de 1 byte cuando un botón particular se toca (touch down).

El código Arduino entra en el bucle while de ese número, y se queda allí hasta que soltamos el botón (touch up), porque en ese momento enviamos el número 0 que significa que el robot no debe hacer nada.

Control de Servos mediante Botones

// Mover servo 1 en dirección positiva
while (m == 16) {
  if (Bluetooth.available() > 0) {
    m = Bluetooth.read();
  }
  servo01.write(servo1PPos);
  servo1PPos++;
  delay(speedDelay);
}
// Mover servo 1 en dirección negativa
while (m == 17) {
  if (Bluetooth.available() > 0) {
    m = Bluetooth.read();
  }
  servo01.write(servo1PPos);
  servo1PPos--;
  delay(speedDelay);
}Code language: Arduino (arduino)

Así que, dependiendo de los botones tocados, los servos se mueven en dirección positiva o negativa. El mismo principio de funcionamiento se aplica para todos los servomotores. Para cambiar la velocidad de movimiento, usamos los valores que vienen del slider que van de 100 a 250.

Control de Velocidad del Brazo

// Si el slider de velocidad del brazo cambia
if (dataIn > 101 & dataIn < 250) {
  speedDelay = dataIn / 10; // Cambiar velocidad del servo (tiempo de delay)
}Code language: Arduino (arduino)

Al dividirlos por 10 obtenemos valores de 10 a 25, que se usan como delay en microsegundos en los bucles while para impulsar los servos.

Para almacenar los movimientos del robot, simplemente guardamos las posiciones actuales de los servos y los steppers en arrays, cada vez que se hace clic en el botón Guardar.

Guardado de Posiciones (Botón SAVE)

// Si se presiona el botón "SAVE"
if (m == 12) {
  // si es guardado inicial, establecer posición de steppers a 0
  if (index == 0) {
    LeftBackWheel.setCurrentPosition(0);
    LeftFrontWheel.setCurrentPosition(0);
    RightBackWheel.setCurrentPosition(0);
    RightFrontWheel.setCurrentPosition(0);
  }
  lbw[index] = LeftBackWheel.currentPosition(); // guardar posición en array
  lfw[index] = LeftFrontWheel.currentPosition();
  rbw[index] = RightBackWheel.currentPosition();
  rfw[index] = RightFrontWheel.currentPosition();
  servo01SP[index] = servo1PPos; // guardar posición en array
  servo02SP[index] = servo2PPos;
  servo03SP[index] = servo3PPos;
  servo04SP[index] = servo4PPos;
  servo05SP[index] = servo5PPos;
  servo06SP[index] = servo6PPos;
  index++; // Incrementar índice del array
  m = 0;
}Code language: Arduino (arduino)

Luego, cuando presionamos el botón Ejecutar (Run), llamamos a la función personalizada runSteps(). Esta función personalizada recorre todos los pasos almacenados usando algunos bucles for y while.

Ejecución Automática (Botón RUN)

if (m == 14) {
  runSteps(); 
  // Si se presiona el botón "RESET"
  if (dataIn != 14) {
    stopMoving();
    memset(lbw, 0, sizeof(lbw)); // Limpiar datos del array a 0
    memset(lfw, 0, sizeof(lfw));
    memset(rbw, 0, sizeof(rbw));
    memset(rfw, 0, sizeof(rfw));
    memset(servo01SP, 0, sizeof(servo01SP)); // Limpiar datos del array a 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; // Índice a 0
  }
}Code language: Arduino (arduino)

Debemos notar que empieza desde la primera posición y va a la última posición, y repite eso una y otra vez. Por lo tanto, al guardar los pasos, en realidad necesitamos posicionar el robot de manera que el primer paso tenga la misma posición que el último paso. Mientras recorre los pasos también podemos cambiar la velocidad tanto de la plataforma como del brazo robótico, así como pausar y reiniciar todos los pasos.

Aquí puedes descargar esta app así como el archivo de proyecto editable:

Aquí está el código Arduino completo para este proyecto de robot Arduino:

Código Arduino Completo

/* 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)
// Definir los motores paso a paso y los pines que usarán
AccelStepper LeftBackWheel(1, 42, 43); // (Tipo: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 para almacenar posiciones/pasos
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // posición actual
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // posición anterior
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // para almacenar posiciones/pasos
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;
void setup() {
  // Establecer valores iniciales de velocidad para los 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); // Baud rate por defecto del módulo Bluetooth
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);
  // Mover brazo robótico a posición inicial
  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() {
  // Verificar datos entrantes
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.read(); // Leer el dato
    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; }
    // Mover la plataforma de ruedas Mecanum
    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(); }
    // Velocidad ruedas Mecanum
    if (dataIn > 30 & dataIn < 100) {
      wheelSpeed = dataIn * 20;
    }
    // Mover brazo robótico
    // Mover servo 1 en dirección positiva
    while (m == 16) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo01.write(servo1PPos);
      servo1PPos++;
      delay(speedDelay);
    }
    // Mover servo 1 en dirección negativa
    while (m == 17) {
      if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
      servo01.write(servo1PPos);
      servo1PPos--;
      delay(speedDelay);
    }
    // Mover 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);
    }
    // Mover 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);
    }
    // Mover 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);
    }
    // Mover 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);
    }
    // Mover 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);
    }
    // Si el slider de velocidad del brazo cambia
    if (dataIn > 101 & dataIn < 250) {
      speedDelay = dataIn / 10; // Cambiar velocidad del servo (delay time)
    }
    // Si se presiona botón "SAVE"
    if (m == 12) {
      // si es guardado inicial, establecer posición de steppers a 0
      if (index == 0) {
        LeftBackWheel.setCurrentPosition(0);
        LeftFrontWheel.setCurrentPosition(0);
        RightBackWheel.setCurrentPosition(0);
        RightFrontWheel.setCurrentPosition(0);
      }
      lbw[index] = LeftBackWheel.currentPosition();
      lfw[index] = LeftFrontWheel.currentPosition();
      rbw[index] = RightBackWheel.currentPosition();
      rfw[index] = RightFrontWheel.currentPosition();
      servo01SP[index] = servo1PPos;
      servo02SP[index] = servo2PPos;
      servo03SP[index] = servo3PPos;
      servo04SP[index] = servo4PPos;
      servo05SP[index] = servo5PPos;
      servo06SP[index] = servo6PPos;
      index++;
      m = 0;
    }
    // Si se presiona botón "RUN"
    if (m == 14) {
      runSteps();
      // Si se presiona botón "RESET"
      if (dataIn != 14) {
        stopMoving();
        memset(lbw, 0, sizeof(lbw));
        memset(lfw, 0, sizeof(lfw));
        memset(rbw, 0, sizeof(rbw));
        memset(rfw, 0, sizeof(rfw));
        memset(servo01SP, 0, sizeof(servo01SP));
        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;
      }
    }
  }
  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();
  // Monitorizar voltaje de batería
  int sensorValue = analogRead(A0);
  float voltage = sensorValue * (5.0 / 1023.00) * 3; // Convertir lectura de 5V a 12V aprox
  //Serial.println(voltage);
  // Si voltaje es inferior a 11V encender 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);
}
// Función modo automático - ejecutar pasos guardados
void runSteps() {
  while (dataIn != 13) {
    // Ejecutar pasos una y otra vez hasta que se presione botón "RESET"
    for (int i = 0; i <= index - 2; i++) {
      // Recorrer todos los pasos(index)
      if (Bluetooth.available() > 0) {
        // Verificar datos entrantes
        dataIn = Bluetooth.read();
        if ( dataIn == 15) {
          // Si se presiona botón "PAUSE"
          while (dataIn != 14) {
            // Esperar hasta que se presione "RUN" de nuevo
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.read();
              if ( dataIn == 13) { break; }
            }
          }
        }
        // Si slider de velocidad cambia
        if (dataIn > 100 & dataIn < 150) {
          speedDelay = dataIn / 10; // Cambiar velocidad servo (delay time)
        }
        // Velocidad ruedas Mecanum
        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)

Eso es prácticamente todo para este tutorial. El proyecto funciona bien, pero ten en cuenta que está lejos de ser perfecto. Los movimientos automáticos pueden no ser tan precisos debido al deslizamiento de las ruedas Mecanum, así como al pobre rendimiento de los servomotores. Estos servomotores baratos también pueden temblar o vibrar incluso cuando no se mueven solo porque no tienen suficiente fuerza para sostener el peso de las piezas impresas en 3D.

Espero que hayas disfrutado este tutorial y hayas aprendido algo nuevo. Siéntete libre de hacer cualquier pregunta en la sección de comentarios a continuación y revisa mi Colección de Proyectos Arduino.