Control de servomotores para brazo robótico

Hola, soy nuevo por aquí y en lo que se refiere a la programación con arduino. Estoy montando un brazo robótico de fabicación casera utilizando como base un HD y recortando las piezas de una lámina de plastico, para que se mueva quiero utilizar cuatro servo SG90s y un motor a pasos 28BYJ-48 que seran controlados con dos joystick. Devido a mi inexperiencia con la programación he recolentado diversos códigos de Google y visto numerosos tutoriales en Youtube para lograr que se muevan los motorcitos y mover se mueven, pero encuentro un conflicto debido a que resulta que al motor paso a paso le afectan los delay de los servo y el motor a pasos gira muy despacio. Agradecería algun consejo de como arreglarlo o si debería de utilizar otro código. Aclarar que en el código que adjunto de momento solo he añadido dos servo y el motor paso a paso que representarían la cintura, el hombro y el codo del brazo, gracias por la atención y un saludo.

#include <Stepper.h>   
#include <Servo.h>     // Incluir la librería Servo

 
Servo servo1;          // Crear un objeto tipo Servo llamado servo1(muñeca)
Servo servo2;          // Crear un objeto tipo Servo llamado servo2(codo)

int pasos=100;
int pasosPorLectura=5;
int leer;              //Definiciones para el stepper
int rotacion;
Stepper stepper(pasos, 8,10,9,11);
int velocidadMotor;

int anguloX = 90 ;     // Empezamos en el centro
int anguloY = 90 ;

int salto = 5 ;        // Controla el salto por movimiento

void setup() 
{
  servo1.attach(7) ;   // Conectar servo1 al pin 7(wrist)
  servo2.attach(6) ;   // Conectar servo1 al pin 8(elbow)

  pinMode (A0, INPUT);
  stepper.setSpeed(300);
  
}

void loop()
{                                 //EJE X (SERVO1, wrist)
  int p = analogRead(A2);
  if ( p < 400 )                  // Si la lectura es menor de 400
    anguloX = anguloX + salto ;   // disminuimos el angulo
  else if (p > 600)               // Si mayor de 600
    anguloX = anguloX - salto ;   // Aumentamos el angulo
  servo1.write(anguloX);          // Y este es el que mueve el servo 1
  delay (20);                      // Este delay regula la velocidad del movimiento

                                  //EJE Y (SERVO2, elbow)
  int q = analogRead(A3);
  if ( q < 400 )                  // Si la lectura es menor de 400
    anguloY = anguloY + salto ;   // disminuimos el angulo
  else if (q > 600)               // Si mayor de 600
    anguloY = anguloY - salto ;   // Aumentamos el angulo
  servo2.write(anguloY);          // Y este es el que mueve el servo 2
  delay (40);                      // Este delay regula la velocidad del movimiento

                                  //HERE START STEPER MOTOR'S CODE
  leer=analogRead(A0);
  velocidadMotor=abs(map(leer, 0, 1023, -30, 30))*10;
  if (velocidadMotor>10)
  {
    rotacion = leer>511?1:-1;
    stepper.setSpeed(velocidadMotor);
    stepper.step(rotacion*10);
  }
}

Perdon pero cual es la consulta o duda en particular?

Pues que el delay que hay en los dos servos me relentizan el motor paso a paso y si los elimino giran demasiado rápido al accionar el joystick, pero si los dejo, el motor paso a paso funciona muuuuuuy lennnnnnnnto. Porque ya he hecho pruebas de quitar los delay {delay(20); y delay(40);} y funciona bien el motor a pasos (a una velocidad aceptable) pero los servos en nada que acciono el joystick completan su recorrido bruscamente cuando para un brazo robótico lo que deseo son movimientos suaves pero no a paso tortuga.

Hola Melxo. Tienes el "mal del delay()" pero sin delay(). El problema con el motor paso a paso viene de que cuando le ordenas dar varios pasos él no lo hace instantáneamente, sino que da un pasito, espera, da otro pasito, vuelve a espera, da otro pasito, espera una vez más... así hasta que da todos los pasos que le has pedido. Después continúa el programa... Se "bloquea" la ejecución del programa mientras se está moviendo el motor, como con el delay(). ¿Solución? Los famosos millis(). Pero implementar una solución de millis() no es tan fácil como nos quieren hacer creer.

Así que el truco es ese, quitar delay() y otras cosas que bloqueen temporalmente la ejecución del programa. Te he rehecho el programa para que no use delay() y que mueva el motor paso a paso llamando a la función sólo cuando ha de dar el paso y llamar a la función step(). Pero ojo, que si la llamas demasiado seguido, el motor puede que no reaccione bien y se salte pasos.

Pero cuidado, un "truco" que hay que usar para "engañar" al que controla el motor paso a paso, es decirle que puede ir a una velocidad superior a la que realmente irá. Porque si no, si la velocidad que él cree que va a ir es más baja, cuando le mandes un paso y al poco tiempo le mandes el siguiente paso, él va a esperar internamente si lo cree necesario, pues él también tiene un control interno con millis().

El post anterior lo publiqué accidentalmente antes de adjuntar el programa con mi propuesta. Y al editarlo no encontré opciones para añadir el fichero, así que aquí adjunto el fichero.

Sé que el código es muy ofuscado, lo he hecho un poco a las prisas y a base de "prueba y error",ya que quería afinar las velocidades pero no me ha salido muy bien. Se supone que si le das muy poco a los joysticks va despacito y según le das más, más rápido va... no sé si a ti te irá bien, los servos con los que los probé son de los baratos baratos. Y el motor paso a paso lo estoy alimentando con el Arduino.

Al menos creo que con esto tendrás algo que te puede dar ideas.

foro_brazo.ino (6.38 KB)

Hola IgnoranteAbsoluto. Antes de nada agradecerte tu tiempo y atención.

Quiero especificar que he cambiado los servomotores por unos MG996R (torque 12Kg.) ya que los MG90S no aportaban la suficiente fuerza para mover el brazo, este cambio ha sido frustrante ya que pese a que los MG996R se supone que deberían de ir sobrados tampoco obtengo resultados óptimos a la hora de que muevan el brazo. Por ello estoy a la espera de recibir un par de PDI-6221MG (torque 20 Kg.). Además quiero aclarar que alimento los motores con una fuente de PC (5v, 20A) y la placa de arduino por USB.

He probado el código que has hecho y funciona muy bien, solo he modificado los máximos y mínimos grados de los servomotores ya que hacen tope y su recorrido es menor de 180º. Ahora el problema lo tengo en que el servo que representa al hombro no tiene suficiente fuerza para mover todo y hasta que no reciba los buenos no podre continuar.

Otra cosa que quiero modificar es que al presionar el joystick hacia delante el brazo sube y quiero que realice este movimiento alreves (llamame maniatico y acertaras, pero me parece que sería un movimiento más intuitivo) supongo que sera cambiar un par de parametros de signo pero no tengo claro cules son.

De nuevo agradecerte tu tiempo y atención , muchas gracias.

Edito casi todo el post ya que en un pricipio no me funcionó bien pero fue un error de conexión de cables.

Hola Melxo, me alegro de que te funcione. Lo del máximo y mínimo de los servos los puse configurable al definir el objeto porque supuse que tal vez necesitases cambiarlo. Lo del sentido del joystick, que funcione al revés de como funciona ahora, no lo hice configurable, se me olvidó hacerlo. Voy a ver si lo puedo implementar y si eso ya te digo qué hay que cambiar.

Al final, aunque ha sido rápido el hacer la modificación, como hay que cambiar en varios sitios un par de cosas, mejor te bajas esta nueva versión y vuelves a cambiar los parámetros en la definición según tus necesidades. Les he añadido un último parámetro a los constructores de ServoControlado y StepperControlado que controla si ha de invertir el "sentido del mando", por defecto es false. En tu caso mira a ver qué servo es el que quieres que "funcione al revés" y le cambias el false por true. Ya nos contarás qué tal te va con los PDI-6221MG, que me interesa saber qué servos valen la pena tener para cosas como estas.

foro_brazo.ino (6.73 KB)

Perfecto, de momento a la espera de recibir los PDI-6221MG lo dejo en pausa ya que los pedi en Aliexpress y de llegar tardaran un mes. Ahora ya puedo mover el stepper y los servomotores a la vez y a una velocidad adecuada y progresiva, cuando retome el proyecto con los nuevos servos daré noticias.

De nuevo agradecerte tu atención IgnoranteAbsoluto.

Ya he recibido los servomotores PDI-6221MG y ahora resulta que no me funcionan, no tengo ni idea de si sera por el código o tendra que ver la frecuencia de trabajo que tienen estos servos y no creo que de la casualidad de que me hayan envíado los dos defectuosos.

Espero a ver si alguien puede iluminarme un camino a seguir para hacerlos funcionar o de alguien que los haya utilizado previamente porque he buscado por internet en general y no hayo nada, ni en foros extranjeros. Si he visto que han sido utilizados en otros proyectos pero no especifican que haya que hacer nada diferente en lo que respecta al código.

Adjunto un video de el montaje de prueba donde se observa como funciona el MG996R perfectamente y sinembargo el PDI-6221MG ni se inmuta, si emite un ruidito como una pulsación pero nada más.

Añado que tal y como esta montado intercambio las conexiones de los sevos y sigue sin ir.

Y el código que utilizo es el que me proporciono IgnoranteAbsoluto.

Mira esto:

EDIT:Found a YouTube that measures the current under load

https://www.youtube.com/watch?v=hSgzR5Ga-lU

Under load 3.17A.

Revisa si la fuente entrega la corriente requerida.

Otra cosa, tu enlace no funciona y no tiene etiquetas. editalo!!

Enlace editado :stuck_out_tongue:

Este video ya lo vi y no me aporta información nueva, y esta comprobado que la salida que cojo de la fuente es la de 5V 20A, con el voltimetro me marca 5,8V.

Yo creo que tiene que ver algo con la "Dead band" que en el caso del MG996R es de 5 micro segundos y la de el PDI-6221MG es de 2 microsegundos.

Frustrado por no poder hacer que funcionen los servos PDI-6221MG, y ya que los compré para obtener más fuerza, he pensado en poner los dos MG996R en la base del brazo (lo que sería el hombro) y en lo que sería el codo pondría dos MG90S y asi obtener el doble de fuerza para mover todo el brazo.

Suponiendo que solucionaría un problema, he probado los servos desmontados pero a la hora de añadir más servos al código que me proporciono IgnoranteAbsoluto, no me responden los servos que añado, adjunto video del problema y el montaje donde al final intercambio la conexión de un MG996R por un PDI-6221MG para que se aprecie que no funciona.

Porque no intentas desarmarlo? Algo ocurre y esa vibración lo prueba

Mira

Deberías encontrar algo asi, si puedes desmontar los engranajes tal vez descubras algo que lo tenga bloqueado.