I am a second year robotics student and have been giving a task to draw a circle in a 3D enviroment so far I haven't had the best results I used the 2D equation for a circle in the X and Z axis and at the moment I have set the Y axis to 0 and somehow the Y axis is moving.
#include <Servo.h>
Servo motor1;
Servo motor2;
Servo motor3;
//add variables for lengths - l1, l2 and l3;
float l1 = 105;
float l2 = 150;
float l3 = 150;
float radToDeg = (180 / 3.14);
void ik(float x, float y, float z) {
// accounts for the vertical displacement along Z
z = z - l1;
float theta1, theta2, theta3;
//calculate the angle of the base joint using just the x and y value
// Add solution for Theta 1 (base)
theta1 = atan2(y, x);
// Calculate distance from the base to the end effector projection in the xy-plane
// Add solution for the d variable
float d = sqrt(sq(x) + sq(y));
// float a = d / 2;
//
// // Calculate theta2 (shoulder)
// // Add solution for Theta 2
// float angle1 = acos(a/ l2);
// float angle2 = acos(x/ d);
// theta2 = angle1 + angle2;
//
// // Add solution for Theta3 (arm)
// theta3 = 2*((3.14*1.5)-angle1);
// Calculate theta3
theta3 = acos((sq(d) + sq(z) - sq(l2) - sq(l3)) / (2 * l2 * l3));
// Calculate theta2
float b = atan2(l3 * sin(theta3), l2 + l3 * cos(theta3));
theta2 = atan2(-z, d) - b;
theta1 = theta1 * radToDeg;
theta2 = theta2 * radToDeg;
theta3 = theta3 * radToDeg;
theta2 = theta2 + 90;
theta3 = 180 - theta3;
Serial.print(theta1); Serial.print(" ");
Serial.print(theta2); Serial.print(" ");
Serial.print(theta3); Serial.print(" ");
Serial.println();
//plug theta values into the motor write function
motorWrite(theta1, theta2, theta3);
}
void motorWrite(float t1, float t2, float t3) {
//offset the angles to go from -90 and 90 to be between 0 and 180 instead
t1 += 90;
t2 += 90;
motor1.write(t1);
motor2.write(t2);
motor3.write((90 + t3 - t2)); // make link 3 dependent on theta 2
}
void setup() {
motor1.attach(2);
motor2.attach(3);
motor3.attach(4);
}
void loop() {
ik(sin(millis()/100) * 200, 0, cos(millis()/100) * 200);
}
Here is the code feel free to change it or do anything to it.
PSA I have asked my lectures and they are okay with me uploading this and asking questions as it is a good learning experience. YOU ARE NOT DOING MY HOMEWORK.
I tried the code above and this is a video of what has happend. Here is a youtube link to what is happening at the moment: https://youtube.com/shorts/uRWFePCdgaA?feature=share