Task 1

#include "Servo.h"
Servo motor1, motor2;
float l1 = 100;
float l2 = 100;
float angle1;      
float angle2;
float rad_angle1;  
float rad_angle2; 
float x;
float y;   
float pos_x;
float pos_y;
float pi = 3.1415926;
void setup()
{
  motor1.attach(2);
  motor2.attach(3);
  Serial.begin(9600);
  pos_x = 0;
  pos_y = 200;
  InverseKinematics(0, 200);
  delay(1000);
}
void InverseKinematics(float x, float y){

  //Calculate IK and convert to radians -  rad_angle2 = beta, rad_angle1 = alpha. Num and denom refer to the positioins of the calculations relative to the fractions in the maths equations

  float betaNum = (x * x) + (y * y) - (l1 * l1) - (l2 * l2);
  float betaDenom = 2 * l1 * l2;
  rad_angle2 = acosf((betaNum/betaDenom));
 
  float alphaNum = (l2 * sinf(rad_angle2));
  float alphaDenom = l2 + l2 * cosf(rad_angle2);
  rad_angle1 = atan2(y,x) - atan2(alphaNum,alphaDenom);
  
  //Convert to degrees  
  angle1 = (rad_angle1*180)/pi;
  angle2 = (rad_angle2*180)/pi;
  //Apply degrees to servos 
  
  float d = sqrt(sq(x) + sq(y));

  float theta2 = acos((sq(x) + sq(y) - sq(l1) - sq(l2)) / (2.0 * l1 * l2));
  float theta1 = atan2(y, x) - atan2(l2 * sin(-theta2), l1 + l2 * cos(-theta2));

  //Convert to degrees
  angle1 = theta1 * (180 / pi);
  angle2 = theta2 * (180 / pi);

  motor1.write(angle1); 
  motor2.write(angle2);
  delay(500);
  pos_x = x;
  pos_y = y;
}

L1 100mm

L2 100mm

  while (Serial.available() == 0) {
  }
  Serial.println("enter the x value then y value desired");
  x = Serial.parseFloat();
  Serial.println(x);
  y = Serial.parseFloat();
  Serial.println(y);
  InverseKinematics();

task 2

void loop()
{
  if(pos_x == 0 && pos_y == 200)
  {
    InverseKinematics(100, 100);
    delay(1000);
  }
  else if(pos_x == 100 && pos_y == 100)
  {
    InverseKinematics(-100, 100);
  }

20250218_154757.mp4

task 3

triangle:

InverseKinematics(0, 200);
    delay(1000);
    InverseKinematics(-50, 150);
    delay(1000);
    InverseKinematics(50, 1500);
    delay(1000);
    InverseKinematics(0, 200);
    delay(1000);

76158791517__CB9302A7-9167-4B94-9433-526E8CF2E761.MOV

square:

  //square
  /*
  InverseKinematics(100,100);
  delay(1000);
  InverseKinematics(100,50);
  delay(1000);
  InverseKinematics(50,50);
  delay(1000);
  InverseKinematics(50,100);
  delay(1000);
  */

76158800480__BC56A8ED-2C4F-41DB-AD28-4A12FCEF2DB8.MOV