Drawing SCARA arm. Inverse Kinematics

Projects General Guidance
1

Hi! I'm new here so sorry if my formatting is weird.

I am working on a 2 axis robotic arm. 2 servos for the motors. The motor at the shoulder is facing up but the motor at the elbow is upside down, so its degrees are reversed. Here is an image of the setup:

Right now i am trying to set it up so it will move the pen to follow some coordinates. the coordinates are saved in the code in a 3D array. The main loop reads each coordinate from the array, Sends those through some functions which do the kinematics and calculate the desired angles, and then writes those angles to the servos.

My code has comments and function names in dutch so instead here are some translated code snippets.

The main loop etc.:

const float coordinaat[][2] = //3d Array with the coordinates
{  
  {13    , 8    },
  {20.5  , 8    },
  {20.5  , 14    },
  {17.5  , 16.5  },
  {13    , 14    } 
};


void loop() 
{
  int coordinateAmount  = ((sizeof(coordinaat)) / (sizeof(coordinaat[0])));

  int coordinaatNr = 0;
  for(coordinaatNr = 0;  coordinaatNr < coordinateAmount;  coordinaatNr += 1)
  {
    
    float angle1 = calcAngle1(coordinaat[coordinaatNr][0], coordinaat[coordinaatNr][1]);
    float angle2 = calcAngle2(coordinaat[coordinaatNr][0], coordinaat[coordinaatNr][1]);

    servo1.write(angle1);
    servo2.write(angle2);
    delay(700);
  }
}

And for the inverse kinematics i have these functions:

float RadToDeg(double rad) {
  return float (rad * 180 / 3.1416);
}

//Function takes 3 lengths of the edges and returns the angle opposite to edge a
float LawOfCosines(float a, float b, float c){ 
  return acos((-(a*a) + b*b + c*c) / (2 * a * b));
}

float calcAngle1(float x,float y){
  float distance = sqrt(x*x + y*y);
  float D1 = RadToDeg(atan2(y, x));
  float D2 = RadToDeg(LawOfCosines(lengte2, lengte1, distance));

  return (D1 + D2);
}

float calcAngle2(float x,float y) {
  float distance = sqrt(x*x + y*y);
  return (180-RadToDeg(LawOfCosines(distance, lengte2, lengte1))); //Subtract from 180 because this servo is upside down and so the angles have to be reversed
}

I got a lot of info form this page. Also the names for the angles. See this image:

Problem is that is not working correctly but I have no idea what i am doing wrong. When it should draw a vertical line it just draws a tilted line. And the current coordinates should form a house but it doesn't look anywhere near that.

If i missed some details or if i should upload the full code please say so.

2

Your function to implement the law of cosines is wrong. You should be negating c^2 not a^2.

You can find some C++ code that controls the motion of a Scara 3D printer at RepRapFirmware/src/Movement/Kinematics at dev · Duet3D/RepRapFirmware · GitHub.