task1: already assembled
task2: sample code provided
Link 1= 105mm
Link 2= 150mm
Link 3= 150mm
#include <Servo.h>
Servo motor1;
Servo motor2;
Servo motor3;
float l1 = 105.0;
float l2 = 150.0;
float l3 = 150.0;
//X, Y and Z values for the end effector target
int x = 100;
int y = 100;
int z = 100;
float pi = 3.1415926;
//add variables for lengths - l1, l2 and l3;
float radToDeg = 180/pi // calculate radians to degrees;
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
double d = sqrt(x * x + y * y);
// Add solution for Theta3 (arm)
theta3 = acos((d * d + z * z - l2 * l2 - l3 * l3) / (2 * l2 * l3));
// Calculate theta2 (shoulder)
theta2 = atan2(l3 * sin(theta3), l2 + l3 * cos(theta3));
theta2 = atan2(-z, d) - theta2;
// Add solution for Theta 2
theta1 = theta1 * radToDeg;
theta2 = theta2 * radToDeg;
theta3 = theta3 * radToDeg;
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 += 180;
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);
Serial.begin(9600);
}
void loop() {
ik(x, y, z);
}
76218767580__BEBF2E9C-4AEA-4855-BD25-47C3130C6E9A.MOV
task 3:
void loop() {
ik(0, 100, 100);
ik(0, 120, 70);
ik(0, 140, 100);
ik(0, 120, 120);
ik(0, 100, 100);
}