Hola a todos los internautas. Recién estoy por terminar la carrera, por lo que no tengo un conocimiento tan avanzado en programación de Arduino. Y más contra este reto que es utilizar motores paso a paso y drivers TB6600 que recién los veo. Utilizando varios videos de youtube y un poco de ayudita con el código he podido medio realizar que el brazo robot se mueva. El problema es el siguiente No puedo controlarlo ni va a la posición que quiero.
El brazo de 3GDL R-R-R cuenta con 3 motores paso a paso, sus respectivos drivers y 3 límites de carrera. Se desea que le puedas ingresar una secuencia de coordenadas x,y,z y el brazo pueda realizar la secuencia. El problema es que a veces pareciera que a los motores le cuesta mover el armazón, y en ese proceso pierde pasos. Lo que más me ha funcionado fue utilizar cinemática directa, ingresaba los ángulos y obtenía aproximadamente la posición que quería a puro prueba y error. Sin embargo, las coordenadas que me bota no tienen sentido alguno, pareciera que existiera en otro plano porque a veces mover 10mm a la izquierda o a la derecha viajaba entre los positivos y negativos como si nada, sin sentido. Además, descubrí que al ingresar los ángulos, el movimiento que realizará no es recta, sino tiene como una oscilación. Por lo que su función que es para un brazo tipo CNC no es posible porque debería realizar movimientos angulares y rectos. A continuación les dejo el codigo que estuve utilizando:
#include <AccelStepper.h>
#include <math.h>
const int dir1 = 7, step1 = 4;
const int dir2 = 2, step2 = 5;
const int dir3 = 3, step3 = 6;
const int sensor1Pin = A3;
const int sensor2Pin = A4;
const int sensor3Pin = A5;
const int pot1Pin = A0; // Motor1
const int pot2Pin = A1; // Motor2
const int pot3Pin = A2; // Motor3
AccelStepper motor1(AccelStepper::DRIVER, step1, dir1);
AccelStepper motor2(AccelStepper::DRIVER, step2, dir2);
AccelStepper motor3(AccelStepper::DRIVER, step3, dir3);
const float L1 = 100;
const float L2 = 130;
const float L3 = 170;
const float pi = PI;
const float pasos_por_grado = 1600/360;
float q1, q2, q3;
float theta1, theta2, theta3;
float x, y, z, r, D;
bool referenciado = false;
String inputString = "";
bool stringComplete = false;
unsigned long lastPrintTime = 0;
const unsigned long printInterval = 500; // Imprimir cada 500 ms
int lastPotAngle1 = 0;
int lastPotAngle2 = 0;
int lastPotAngle3 = 0;
const int deadband = 2; // zona muerta en grados
void hacerReferencia() {
motor2.setSpeed(-800);
while (digitalRead(sensor3Pin) != LOW) motor2.runSpeed();
motor2.stop(); motor2.setCurrentPosition(0);
motor3.setSpeed(-800);
while (digitalRead(sensor2Pin) != LOW) motor3.runSpeed();
motor3.stop(); motor3.setCurrentPosition(0);
motor1.setSpeed(-800);
while (digitalRead(sensor1Pin) != LOW) motor1.runSpeed();
motor1.stop(); motor1.setCurrentPosition(0);
}
void moverA_angulos(float q1_ref, float q2_ref, float q3_ref) {
motor1.moveTo(q1_ref * pasos_por_grado);
motor2.moveTo(q2_ref * pasos_por_grado);
motor3.moveTo(q3_ref * pasos_por_grado);
while (motor1.distanceToGo() != 0 || motor2.distanceToGo() != 0 || motor3.distanceToGo() != 0) {
motor1.run();
motor2.run();
motor3.run();
imprimirPosicionActual();
}
}
void imprimirPosicionActual() {
q1 = motor1.currentPosition() / pasos_por_grado * pi / 180.0;
q2 = motor2.currentPosition() / pasos_por_grado * pi / 180.0;
q3 = motor3.currentPosition() / pasos_por_grado * pi / 180.0;
r = L2 * cos(q2) + L3 * cos(q2 + q3);
x = r * cos(q1);
y = r * sin(q1);
z = L1 + L2 * sin(q2) + L3 * sin(q2 + q3);
Serial.print("X: "); Serial.print(x);
Serial.print(" Y: "); Serial.print(y);
Serial.print(" Z: "); Serial.print(z);
Serial.print(" θ1: "); Serial.print(q1 * 180 / pi);
Serial.print(" θ2: "); Serial.print(q2 * 180 / pi);
Serial.print(" θ3: "); Serial.println(q3 * 180 / pi);
}
int leerPotSuavizado(int pin) {
long suma = 0;
for (int i = 0; i < 5; i++) {
suma += analogRead(pin);
}
return suma / 5;
}
void setup() {
Serial.begin(9600);
inputString.reserve(200);
pinMode(sensor1Pin, INPUT_PULLUP);
pinMode(sensor2Pin, INPUT_PULLUP);
pinMode(sensor3Pin, INPUT_PULLUP);
motor1.setMaxSpeed(1600); motor1.setAcceleration(1000);
motor2.setMaxSpeed(1600); motor2.setAcceleration(1000);
motor3.setMaxSpeed(1600); motor3.setAcceleration(1000);
hacerReferencia();
referenciado = true;
moverA_angulos(0, 0, 0);
delay(1000);
Serial.println("Ingrese q1,q2,q3 separados por comas. Ejemplo: 45,30,60");
}
void loop() {
if (stringComplete) {
int q1_i = inputString.indexOf(',');
int q2_i = inputString.lastIndexOf(',');
if (q1_i > 0 && q2_i > q1_i) {
float q1_input = inputString.substring(0, q1_i).toFloat();
float q2_input = inputString.substring(q1_i + 1, q2_i).toFloat();
float q3_input = inputString.substring(q2_i + 1).toFloat();
moverA_angulos(q1_input, q2_input, q3_input);
} else {
Serial.println("Formato incorrecto. Use: q1,q2,q3");
}
inputString = "";
stringComplete = false;
Serial.println("Ingrese nuevos valores q1,q2,q3:");
} else {
// Leer y suavizar potenciómetros
int pot1Val = leerPotSuavizado(pot1Pin);
int pot2Val = leerPotSuavizado(pot2Pin);
int pot3Val = leerPotSuavizado(pot3Pin);
float angle1 = map(pot1Val, 0, 1023, 0, 900);
float angle2 = map(pot2Val, 0, 1023, 0, 400);
float angle3 = map(pot3Val, 0, 1023, 0, 230);
// Zona muerta para cada motor
if (abs(angle1 - lastPotAngle1) >= deadband) {
motor1.moveTo(angle1 * pasos_por_grado);
lastPotAngle1 = angle1;
}
if (abs(angle2 - lastPotAngle2) >= deadband) {
motor2.moveTo(angle2 * pasos_por_grado);
lastPotAngle2 = angle2;
}
if (abs(angle3 - lastPotAngle3) >= deadband) {
motor3.moveTo(angle3 * pasos_por_grado);
lastPotAngle3 = angle3;
}
motor1.run();
motor2.run();
motor3.run();
}
if (millis() - lastPrintTime >= printInterval) {
imprimirPosicionActual();
lastPrintTime = millis();
}
}
void serialEvent() {
while (Serial.available()) {
char inChar = (char)Serial.read();
inputString += inChar;
if (inChar == '\n') {
stringComplete = true;
}
}
}
Además, pensé que podía ser mi cableado y la cantidad de pasos o micropasos que este tiene. Pero considero que lo configuré correctamente todos, pero sigue sin funcionar. Así que les dejo el cableado:
Tambien les dejo mi configuración de los drivers, el robot y los modelos de los motores.
Nota: Sé que en el código aparecen potenciometros. Es que intentaba controlarlos manualmente, grabar la posición y hacer que replique la secuencia. Spoiler, tampoco lo logré. Ya que al volver al Home, perdía pasos y en el 4 intento era inestabilidad total. Además, intenté utilizar un CNC shield para controlar los motores, pero tenia el mismo problema, a un motor le costaba moverse, el cual es levantar el brazo. Tuve la suposición de subirle un poco la corriente y así fue probando, pero más era lo que se calentaba que movia.
. Estoy que me guio con la app RoboDK del Dobot Magician para ver las coordenadas, pero nada.
