Recently I built a 4-axis robot for palletization applications with its 4th axis being mechanically controlled, the rest of the 3 axis are controlled by nema17 motors using a drv8825 driver, a cnc shield v3 and a arduino mega, the problem I am encountering is that despite de correct calculations of the inverse kinematics, I am struggling with the precision of its movements, mainly translating on its y-axis and z-axis, I attached below some images, videos and the code
Forward Kinematics
Inverse Kinematics
#include <MobaTools.h>
#include <math.h>
#include <FastTrig.h>
#include <string.h>
#define pin 38
#define step1 2
#define step2 3
#define step3 4
#define dir1 5
#define dir2 6
#define dir3 7
#define l1 69.5
#define l2 100
#define l3 100
MoToStepper motor1(800, STEPDIR);
MoToStepper motor2(800, STEPDIR);
MoToStepper motor3(800, STEPDIR);
MoToServo servo1;
int step = 0, pos1 = 0, pos2 = 0, pos3 = 0, servo_pos = 0;
double xp, yp, zp, alpha1, beta1, gamma1;
bool Joint_Mode = true, First_Message = true, Repeat = false, Play = false;
double degreesToRadians(double degrees) {
return degrees * M_PI / 180.0;
}
double radiansToDegrees(double radians) {
return radians * 180.0 / M_PI;
}
void setup() {
Serial.begin(9600);
delay(1000);
servo1.attach(pin);
motor1.attach(step1, dir1);
motor2.attach(step2, dir2);
motor3.attach(step3, dir3);
motor1.setMaxSpeed(200);
motor2.setMaxSpeed(200);
motor3.setMaxSpeed(200);
motor1.setRampLen(10);
motor2.setRampLen(10);
motor3.setRampLen(10);
Serial.println("Set the Step size using - and + keys.");
Serial.println("Joint mode can be toggled on and off with the q ans w keys.");
Serial.println("Effector is controlled using i, o and p keys");
Serial.println("The robot is controlled with the numpad using all the numbers");
alpha1 = motor1.currentPosition() * 0.1125;
beta1 = motor2.currentPosition() * 0.1125;
gamma1 = motor3.currentPosition() * 0.1125;
xp = (l1 * sin(degreesToRadians(beta1)) + l3 * sin(degreesToRadians(90 - beta1 + gamma1))) * sin(degreesToRadians(alpha1));
yp = (l1 * sin(degreesToRadians(beta1)) + l3 * sin(degreesToRadians(90 - beta1 + gamma1))) * cos(degreesToRadians(alpha1));
zp = l1 + l2 * cos(degreesToRadians(beta1)) - l3 * cos(degreesToRadians(90 - beta1 + gamma1));
zp = zp - l1;
}
void loop() {
double alpha, beta, gamma;
if (Serial.available() > 0) {
char input = Serial.read();
switch (input) {
case '+':
if (step >= 100) {
Serial.println("Step can't be set higher than 100!");
break;
}
Serial.print("Step set to: ");
if (step < 5)
++step;
else if (step == 5)
step += 5;
else
step += 10;
Serial.print(step);
Serial.print('\n');
break;
case '-':
if (step <= 0) {
Serial.println("Step can't be set as a negative value!");
break;
}
Serial.print("Step set to: ");
if (step <= 5)
--step;
else if (step == 10)
step -= 5;
else
step -= 10;
Serial.print(step);
Serial.print('\n');
break;
case '7':
pos1 += step;
xp += step;
break;
case '8':
pos1 = 0;
xp = 0;
break;
case '9':
pos1 -= step;
xp -= step;
break;
case '4':
pos2 += step;
yp += step;
break;
case '5':
pos2 = 0;
yp = 100;
break;
case '6':
pos2 -= step;
yp -= step;
break;
case '1':
pos3 += step;
zp += step;
break;
case '2':
pos3 = 0;
zp = 100;
break;
case '3':
pos3 -= step;
zp -= step;
break;
case '0':
pos1 = 0;
xp = 0;
pos2 = 0;
yp = 100;
pos3 = 0;
zp = 100;
break;
case 'i':
if (servo_pos == 180) {
Serial.println("Servo cant go above 180");
break;
}
servo_pos += 10;
break;
case 'o':
servo_pos = 0;
break;
case 'p':
if (servo_pos == 0) {
Serial.println("Servo cant go below 0");
break;
}
servo_pos -= 10;
break;
case 'q':
Joint_Mode = true;
Serial.println("Joint mode on");
break;
case 'w':
Joint_Mode = false;
Serial.println("Joint mode off");
break;
}
if (Joint_Mode == false) {
alpha = xp;
double l = sqrt((xp * xp) + (yp * yp));
double a = sqrt((zp * zp) + (l * l));
beta = degreesToRadians(90) - acos(((l2 * l2) + (a * a) - (l3 * l3)) / (2.0 * l2 * a)) - atan(zp / l);
gamma = acos(((l2 * l2) + (l3 * l3) - (a * a)) / (2.0 * l2 * l3)) - degreesToRadians(90);
//alpha = radiansToDegrees(alpha);
gamma = radiansToDegrees(gamma);
beta = radiansToDegrees(beta);
motor1.moveTo(alpha / 0.1125);
motor2.moveTo(beta / 0.1125);
motor3.moveTo(gamma / 0.1125);
Serial.print("alpha = ");
Serial.println(alpha , 10);
Serial.print("beta = ");
Serial.println(beta , 10);
Serial.print("gamma = ");
Serial.println(gamma , 10);
Serial.println();
} else {
motor1.moveTo(pos1 * 16);
motor2.moveTo(pos2 * 16);
motor3.moveTo(pos3 * 16);
}
servo1.write(servo_pos);
}
}
I modified some bits of the code, taking into account the x and y axis not the z and y, but the problem still persists, the robot translates at a 45 degree angle on the z axis, I tried all the combinations with adding, substracting 45 and 90 degree angles, I dont know what to do, any suggestions?