Base simple de conocimiento para la cinematica directa e inversa de Scara.

Recientemente me decidí montar este prototipo de robot Scara, con la idea de aprender el funcionamiento de la cinematica directa e inversa ,llevando a la práctica para su posterior documentación todo lo que pueda proporcionar de forma sencilla un conocimiento adecuado a nuestros proyectos.

He utilizado el estupendo trabajo realizado por “Dejan” en “howtomechatronics.com”, para adaptar el software a la plataforma Blynk IOT en lo que se refiere a su control remoto (RC). En principio utilizo el Ide arduíno con la libreria AccelStepper en una placa Mega2560 y una CNC shield V3

OBJETIVO: Establecer un procedimiento de configuración basado en la librería AccelStepper.h para la comprobación de la cinematica directa DK e inversa IK con aplicación a un brazo robotico tipo “scara”.

Prototipo: Arduino Scara Robot,by Dejan

(https://howtomechatronics.com/projects/scara-robot-how-to-build-your-own-arduino-based-robot/)

Para mejorar la estabilidad del conjunto, he realizado algunas modificaciones:

GENERALIDADES:

El robot modificado queda con tres grados de libertad y es impulsado por tres motores paso a paso NEMA 17. ….Pensado para su posterior uso con el firmware de GRBL .y Marlin.

El control GUI es realizado a traves de la plataforma Blynk IOT conectado con un ESP8266

La comunicacion serial se estable entre Tx y gnd desde el ESP8266 hasta la Rx- Gnd del Serial1 del Mega 2560 sin que haya invcoveniencia a la hora de cargar el programa, puesto que solo utiliza TX

PLANOS DE TRABAJO

Primero tenemos que ver que existen tres planos de observación:

-El primero es el XY o plano de trabajo del efector del brazo L2., es el espacio que vamos a dedicar para hallar las coordenadas cartesianas Xp,Yp en la cinematica directa que como sabemos es aquella que conociendo los angulos de giro de los brazos L1 y L2 nos va a hallar las citadas coordenadas.

-El segundo plano sería el X0Y0 ,plano donde se mueve el brazo L1,cuyo eje de giro Z es el eje de coordenadas X0 Y0.

-El tercer plano que corresponde al brazo L2 y que nomino X1Y1, tiene su eje Z en el extremo de L1, y a diferencia del plano X0Y0 que permace estático y paralelo al plano XY, este se mueve a la vez que lo hace el brazo L1 actuando su eje X1 como una prolongación de L1 y su eje Y1 perpendicular.

Llamamos Q1 (theta1) y Q2(theta2) los angulos de giro de los brazos L1 y L2, y xP,yP las coordenadas cartesianas correspondientes y tambien inputx,inputy como las entradas de dichas coordenadas en la cinematica inversa.

La medición de L1 se realiza desde su eje de giro hasta el eje de giro de L2 y desde aqui hasta el extremo del efector para L2.

Tenemos que tener siempre en cuenta que el calculo de correcciones se hace con referencia a la primera figura donde q1=q2=0 grados. En cualquier posición de los brazos L1 y L2 se moveran segun sus planos antes mencionados,…

De forma generalizada usaremos la solución geométrica conocida, a la que hemos añadido las correcciones y desvios con referencia a la posición de reposo ideal.

Cy corresponde al offset del plano XY o altura con respecto al plano X0Y0, Cx sería la corrección posterior de Xp de modo que cuando Theta1 y Theta2 =0, Xp=0

void forwardKinematics() :
xP = ((L1 * cos(theta1F)) + (L2 * cos(theta1F + theta2F)))-+Cx
yP = ((L1 * sin(theta1F)) + (L2 * sin(theta1F + theta2F)))-Cy

Theta1F=Theta1*PI/180 +-TurnX*PI/180—–radianes

Theta2F=Theta2*PI/180 +-TurnY*PI/180—–radianes

void inverseKinematics():

Inputx=inputx +-Cx

Inputy=inputy +-Cy

theta2F = acos((sq(Inputx) + sq(Inputy) – sq(L1) – sq(L2)) / (2 * L1 * L2));

theta1F =atan(Inputx / Inputy) – atan((L2 * sin(theta2F)) / (L1 + L2 * cos(theta2F)));

theta2 = (-1)* ((theta2F * 180 / PI)+-TurnY);
theta1 = (theta1F * 180 / PI)+-TurnX;

if (Inputx >= 0 && Inputy >= 0) { // 1st quadrant
theta1 = 90- theta1;
}
if (Inputx < 0 && Inputy > 0) { // 2nd quadrant
theta1 = 90- theta1;
}

if (Inputx < 0 && Inputy< 0) { // 3nd quadrant

theta1 = 270- theta1; theta2 = (-1) * theta2; }

if (Inputx > 0 &&Inputy < 0) { //4nd quadrant

theta1 = -90- theta1; theta2 = (-1) *theta2; }

if (Inputx > 0 &&Inputy == 0) {
theta1 = 270+ theta1;

Datos generales

stepper	*reductor	*microsteps	*steps/grades(200/360)=	*steps/radian(200/2*PI)=	steps/mm
X(L1)	20	8	88,8888	5095.5
Y(L2)	16	8	71,11111	4076.4
Z(T8)	200*1/8 (rev/mm)	8			200

,Datos particulares:

	longitudes(mm)	angulo desde su limite hasta el 0x ideal	angulo desde su limite hasta el -0y ideal(extension opuesta de L1)
L1	228
L2	134
wx		15
wy			25

A partir de aqui voy a detallar un procedimiento para introducir las correcciones oportunas de forma que la DK e IK trabajen correctamente

Previamente veamos la situación de LimitX ,LimitY. Los angulos wx y wy nos van a dar el punto de inicio previo hasta alcanzar el reposo o (0,0) que elijamos.

• Elección del plano/superficie de trabajo o de impresión extendiendo manualmente L1 y L2 y comprobando las extensiones máximas y minimas así como las restricciones propias de las zonas que no puede alcanzar estableciendo la distancia Cy apropiada.
• Establecer nuestro (0,0) particular que deseemos o que sea mas propicio a nuestras necesidades. En mi caso he elegido una posición centralizada en el plano XY. Esto se hara manualmente y anotando aproximadamente los angulos formados.

• Mis mediciones aproximadas han sido de un Cy=110 mmm,,L1=135º,,L2=40º. Recordemos que estos angulos se toman con respecto al eje X0, pero ahora hay que tener en cuenta los valores wx y wy angulos de tope de sus limites. Nos van a servir para calcular el homing X,Y que posteriormente corregiremos.
• HomingX=wx+135=150º,, HomingY=42 ( en Y no se tiene ahora en cuenta wy porque esta por debajo de su limite el angulo formado, a diferencia de wx que esta por encima)
• stepperX.setCurrentPosition=-150*88.88888=-3333,, stepperY.setCurrentPosition=40*71,1111=-2844

 while (digitalRead(limitX) != 1) {
    stepperX.setSpeed(-1500);
    stepperX.runSpeed();
    stepperX.setCurrentPosition(-13333);
  } 

  while (digitalRead(limitY) != 1) {
    stepperY.setSpeed(-1500);
    stepperY.runSpeed();
    stepperY.setCurrentPosition(-3200);
  } /
  delay(20);
  stepperX.moveTo(0);
  while (stepperX.currentPosition() != 0) {
    stepperX.run();
  }
  stepperY.moveTo(0);
  while (stepperY.currentPosition() != 0) {
    stepperY.run();
  }

• Una vez hecho esto comprobamos la situación del efector con respecto al (0,0) que nos habiamos fijado teniendo que hacer las siguientes correcciones:: restar 9º a X y sumar 5º a Y, quedando un HomingX=141º y HomingY=45º y finalmente

 while (digitalRead(limitX) != 1) {
    stepperX.setSpeed(-1500);
    stepperX.runSpeed();
    stepperX.setCurrentPosition(-12533);
  } 

  while (digitalRead(limitY) != 1) {
    stepperY.setSpeed(-1500);
    stepperY.runSpeed();
    stepperY.setCurrentPosition(-3200);
  } 

• Ahora ya estamos en condiciones de hallar los valores de giro TurnX y TurnY que introduciremos en las formulas geometricas de la cinematica DK/IK:
• TurnX=141-wx=126º (este es el giro dado del brazo L1 desde el eje X0 hasta la posicion de reposo)
• TurnY=-(180-(45+wy))=105º(giro en negativo desde el eje Y1 hasta su posicion de reposo , vemos que ahora si tenemos que utilizar el valor de wy)
• Aqui podemos ver como queda la DK una vez hecha las correcciones. Theta1F se le han adicionado 126º que se multiplican *PI/180 para su expresion en radianes necesaria y Theta2F se le han restado 105ª*PI/180, Recordemos que los giros CCW se toma + y CW (-). Al mismo tiempo a yP se le resta el valor Cy=110 mm y a xP=1,4 mm, el valor Cx que se obtiene observando que se tiene que cumplir que Xp sea 0, En resumen, nuestro valor Theta (0,0) nos va a dar en la DK la posicion central cartesiana (0,126)

void forwardKinematics() {
 
  theta1=theta1f;
theta2=theta2f;
float theta1F=(theta1)*PI/180+126*PI/180;//offset +126 GRADOS
float   theta2F=(theta2)*PI/180-105*PI/180; //OFSSET DE -105 GRADOS
  xP = ((L1 * cos(theta1F)) + (L2 * cos(theta1F + theta2F)))-1.4;
  yP = ((L1 * sin(theta1F)) + (L2 * sin(theta1F + theta2F)))-110;
 
  


  
}

• Ahora vemos como nos queda la IK con las correcciones hechas y observemos donde se introducen;
• Si introducimos los valores inputx=0 ,,inputy=126, los valores Theta seran=0

void inverseKinematics(){

Inputy= inputy+110;
  Inputx=inputx+1.4;
 
 
  theta2F = acos((sq(Inputx) + sq(Inputy) - sq(L1) - sq(L2)) / (2 * L1 * L2));// en radianes,,si inputx,y es 1
   
  theta1F =atan(Inputx / Inputy) - atan((L2 * sin(theta2F)) / (L1 + L2 * cos(theta2F)));

 

 theta2 =   (-1)* ((theta2F * 180 / PI)-105); //paso de radianes a grados y hace que el sentido eje x sea correcto
 theta1 =  (theta1F * 180 / PI)+126;
 
if (Inputx >= 0 && Inputy >= 0) {       // 1st quadrant
    theta1 = 90- theta1;
  }
  if (Inputx < 0 && Inputy > 0) {       // 2nd quadrant
    theta1 = 90- theta1;
  }
if (Inputx < 0 && Inputy< 0) {       // 3nd quadrant
    theta1 = 270- theta1;
    theta2 = (-1) * theta2;
  }
 if (Inputx > 0 &&Inputy < 0) {       //4nd quadrant
    theta1 = -90- theta1;
    theta2 = (-1) *theta2;
  }
 if (Inputx > 0 &&Inputy == 0) {       
    theta1 = 270+ theta1;
  }

Conclusión:

Este estudio solo sirve como introducción y conocimiento del fucionamiento de la DK/IK utilizando la libreria AccelStepper.h, ayudado de una GUI con Blynk.IOT, y sus resultados prácticos no son muy buenos si utilizamos solo la cinematica inversa (IK), ya que los desplazamientos no son rectilineos y no nos serviran si queremos hacer dibujos,por ejemplo. Un poco mejor con la DK y podremos pasar un buen rato diseñando la GUI con Blynk Asi que no nos queda mas remedio que irnos directamente desde aqui a Marlin, que seguro nos darámas alegrias a nuestras aplicaciones.

Codigo completo:

ESP8266

#define BLYNK_TEMPLATE_ID "TMPLApOpo0Uj"
#define BLYNK_TEMPLATE_NAME "My SCARA"
#include <Arduino.h>
#include <Wire.h>
#include <WiFiClient.h>
#define BLYNK_TEMPLATE_ID "TMPLApOpo0Uj"
#define BLYNK_TEMPLATE_NAME "My SCARA Jean"
#define BLYNK_AUTH_TOKEN "9N2nLeP-3WNBr0xq70EG7X-5prt-ZJYD"
#include <BlynkSimpleEsp8266.h>
//char ssid[] = "blynkiot";
char ssid[] = "atom_2.4g";//NOMBRE DE NUESTRA RED WIFI
char pass[] = "crixalcfilos";//Contraseña
char auth[] = "9N2nLeP-3WNBr0xq70EG7X-5prt-ZJYD";


/*
 v0-homing
 v1-grip
 v2-run

 v7-z
 v8-phi
 theta1-theta1
 theta2-theta2
 v11-input xp
 v12-input yp
 v13-controlIK
 */
 
boolean controlIK = false;
int run,grip,homing;
float L1 = 228; // L1 = 228mm
float L2 = 145; // L2 = 145mm
float xP,yP,theta1f,theta2f,phi,z,x,y,theta1,theta2,theta1F, theta2F,inputx,inputy,Inputx,Inputy;
String data;
String w;

 
BLYNK_WRITE(V0){homing=param.asInt();} 
BLYNK_WRITE(V1){grip=param.asInt();}
BLYNK_WRITE(V2){run=param.asInt();}
BLYNK_WRITE(V7){z=param.asDouble();}
BLYNK_WRITE(V9){theta1f=param.asDouble();}
BLYNK_WRITE(V10){theta2f=param.asDouble();}
BLYNK_WRITE(V11){inputx=param.asDouble();}
BLYNK_WRITE(V12){inputy=param.asDouble();}
BLYNK_WRITE(V13){controlIK=param.asInt();}
BLYNK_WRITE(V14){w=param.asString();}
BlynkTimer timer1,timer2;



void SV3(){
  
  Blynk.virtualWrite(V3,xP);  // posision xp procedente de DK
}

void SV4(){

   
  Blynk.virtualWrite(V4,yP);  // posision yp procedente de DK
}

void SV5(){

 
  Blynk.virtualWrite(V5,theta1);  // valor de theta1 
  }

void SV6(){
  
  Blynk.virtualWrite(V6,theta2);  // valor de theta2 
  }


void SV15(){
  
  Blynk.virtualWrite(V15,w); 
  }


void setup(){
   
  Blynk.begin(auth, ssid, pass);
  Serial.begin(115200);
  timer1.setInterval(1000L, SV3);
   timer1.setInterval(1000L, SV4); 
   timer2.setInterval(1500L, SV5);
   timer2.setInterval(1500L, SV6);
   timer2.setInterval(1500L, SV15);
}
void loop() {
   Blynk.run();
   timer1.run();
   timer2.run();
 
  data=(String(theta1)+","+String(theta2)+","+String(z)+","+String(grip)+","+String(homing)); 
  
if (w=="run") {Serial.println(data);delay(1000);}
if (homing) {Serial.println(data);delay(100);}//orden homing
if (run){Serial.println(data);delay(100);}//RUN

if (controlIK==false){forwardKinematics();}
 else if (controlIK==true){inverseKinematics();}

}

void forwardKinematics() {
 /*
  si queremos que visualmente el angulo coincida
  theta1=theta1f;
theta2=theta2f-90;//
*/
  theta1=theta1f;
theta2=theta2f;
float theta1F=(theta1)*PI/180+126*PI/180;//offset +127GRADOS
float   theta2F=(theta2)*PI/180-105*PI/180; //OFSSET DE -111 GRADOS
  xP = ((L1 * cos(theta1F)) + (L2 * cos(theta1F + theta2F)))-1.4;
  yP = ((L1 * sin(theta1F)) + (L2 * sin(theta1F + theta2F)))-110;
 
  


  
}

void inverseKinematics(){

Inputy= inputy+110;//if (Inputy>L1+L2){Inputy=L1+L2;}
  Inputx=inputx+1.4;//este offset es para ajustar el centro x=0
 
 
  theta2F = acos((sq(Inputx) + sq(Inputy) - sq(L1) - sq(L2)) / (2 * L1 * L2));// en radianes,,si inputx,y es 1
   
  theta1F =atan(Inputx / Inputy) - atan((L2 * sin(theta2F)) / (L1 + L2 * cos(theta2F)));

 

 theta2 =   (-1)* ((theta2F * 180 / PI)-105); //paso de radianes a grados y hace que el sentido eje x sea correcto
 theta1 =  (theta1F * 180 / PI)+126;
 
if (Inputx >= 0 && Inputy >= 0) {       // 1st quadrant
    theta1 = 90- theta1;
  }
  if (Inputx < 0 && Inputy > 0) {       // 2nd quadrant
    theta1 = 90- theta1;
  }

 if (Inputx < 0 && Inputy< 0) {       // 3nd quadrant
    theta1 = 270- theta1;
    theta2 = (-1) * theta2;
  }
 if (Inputx > 0 &&Inputy < 0) {       //4nd quadrant
    theta1 = -90- theta1;
    theta2 = (-1) *theta2;
  }
 if (Inputx > 0 &&Inputy == 0) {       
    theta1 = 270+ theta1;
  }

 


 

}

Mega 2560

#include <Arduino.h>

#include <AccelStepper.h>
#include <Servo.h>
#include <math.h>

#define limitX 11
#define limitY 10
#define limitZ 9


// Define the stepper motors and the pins the will use
AccelStepper stepperX(1, 2, 5); // (Type:driver, STEP, DIR)
AccelStepper stepperY(1, 3, 6);
AccelStepper stepperZ(1, 4, 7);

Servo grip;  // create servo object to control a servo


double L1 = 228; // L1 = 228mm
double L2 = 145; // L2 = 145mm
double theta1, theta2;
int g, x, z;
int data[6];
const float theta1StepbyDegrees =  88.88888888;
const float theta2StepbyDegrees =  71.11111111;
const float zStepbymm = 200;
int xmove, ymove, zmove;
String content = "";



void setup() {
  Serial1.begin(115200);
  Serial.begin(9600);

  pinMode(limitX, INPUT_PULLUP);
  pinMode(limitY, INPUT_PULLUP);
  pinMode(limitZ, INPUT_PULLUP);

  // stepper motors max speed
  stepperX.setMaxSpeed(2000);
  stepperX.setAcceleration(500);
  stepperY.setMaxSpeed(2000);
  stepperY.setAcceleration(500);
  stepperZ.setMaxSpeed(1000);
  stepperZ.setAcceleration(500);

  grip.attach(A0);
  grip.write(30);
  homing();
}

void loop() {

  if (Serial1.available()) {
    content = Serial1.readString(); // Read the incomding data from Processing
    // Extract the data from the string and put into separate integer variables (data[] array)
    for (int i = 0; i < 7; i++) {
      int index = content.indexOf(","); // locate the first ","
      data[i] = atol(content.substring(0, index).c_str()); //Extract the number from start to the ","
      content = content.substring(index + 1);
    }//Remove the number from the string

    if (data[4] == 1) {
      homing();
      delay(500);
    }
    xmove = data[0] * theta1StepbyDegrees;
    ymove = data[1] * theta2StepbyDegrees;
    zmove = data[2] * zStepbymm;

    grip.write(data[3]);
  }
  //aqui se termina available




  /*
    if (data[0]>=184){data[0]=184;}
     if (data[1]>=229){data[1]=229;}
      if (data[3]>=80){data[3]=80;}
        data[0] - x
       data[1] - y
       data[2] - z
       data[3] - g
       data[4] - Homing
  */


  stepperX.moveTo(xmove);
  stepperY.moveTo(ymove);
  stepperZ.moveTo(zmove);
  while (stepperX.currentPosition() != xmove || stepperY.currentPosition() != ymove || stepperZ.currentPosition() != zmove) {
    stepperX.run();
    stepperY.run();
    stepperZ.run();


  }




}

void homing() {
  Serial.println(stepperZ.currentPosition());
  // Homing stepperZXY

  while (digitalRead(limitZ) != 1) {
    stepperZ.setSpeed(-1000);
    stepperZ.runSpeed();
    stepperZ.setCurrentPosition(-20000);
  } //200x127.5 (127.5 mm) When limit switch pressed set position to 0 steps

  while (digitalRead(limitX) != 1) {
    stepperX.setSpeed(-1500);
    stepperX.runSpeed();
    stepperX.setCurrentPosition(-12533);
  } //5 grados -5 grados desvio When limit switch pressed set position to 0 steps

  while (digitalRead(limitY) != 1) {
    stepperY.setSpeed(-1500);
    stepperY.runSpeed();
    stepperY.setCurrentPosition(-3200);
  } //5 grados When limit switch pressed set position to -5440 steps
  delay(20);
  stepperX.moveTo(0);
  while (stepperX.currentPosition() != 0) {
    stepperX.run();
  }
  stepperY.moveTo(0);
  while (stepperY.currentPosition() != 0) {
    stepperY.run();
  }
  delay(20);
  stepperZ.moveTo(0);
  while (stepperZ.currentPosition() != 0) {
    stepperZ.run();
  }



}